orb: read & apply publisher rules from file (currently disabled via #ifdef)

If enabled, orb reads a rules file (./rootfs/orb_publisher.rules) on
startup. This can contain rules about which module is allowed to publish
which topic. It is completely transparent, so a publisher does not know
if he's not allowed to publish, and publications will look as if they
succeeded.

To test, add
#define ORB_USE_PUBLISHER_RULES
to uORBManager.hpp

src/modules/uORB/uORBManager.cpp

@@ -38,6 +38,7 @@
 #include <errno.h>
 #include <px4_config.h>
 #include <px4_posix.h>
+#include <px4_tasks.h>
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
 #include "px4_config.h"
@@ -63,6 +64,21 @@ bool uORB::Manager::initialize()
 uORB::Manager::Manager()
 	: _comm_channel(nullptr)
 {
+
+#ifdef ORB_USE_PUBLISHER_RULES
+	const char *file_name = "./rootfs/orb_publisher.rules";
+	int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
+
+	if (ret == PX4_OK) {
+		_has_publisher_rules = true;
+		PX4_INFO("Using orb rules from %s", file_name);
+
+	} else {
+		PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 }
 
 int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
@@ -96,6 +112,30 @@ orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
 		int priority, unsigned int queue_size)
 {
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	// check publisher rule
+	if (_has_publisher_rules) {
+		const char *prog_name = px4_get_taskname();
+
+		if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
+			if (_publisher_rule.ignore_other_topics) {
+				if (!findTopic(_publisher_rule, meta->o_name)) {
+					PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
+					return (orb_advert_t)_Instance;
+				}
+			}
+
+		} else {
+			if (findTopic(_publisher_rule, meta->o_name)) {
+				PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
+				return (orb_advert_t)_Instance;
+			}
+		}
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 	int result, fd;
 	orb_advert_t advertiser;
 
@@ -140,6 +180,14 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 
 int uORB::Manager::orb_unadvertise(orb_advert_t handle)
 {
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	if (handle == _Instance) {
+		return PX4_OK; //pretend success
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 	return uORB::DeviceNode::unadvertise(handle);
 }
 
@@ -161,6 +209,14 @@ int uORB::Manager::orb_unsubscribe(int fd)
 
 int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	if (handle == _Instance) {
+		return PX4_OK; //pretend success
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 	return uORB::DeviceNode::publish(meta, handle, data);
 }
 
@@ -446,3 +502,143 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 	return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end());
 #endif
 }
+
+
+#ifdef ORB_USE_PUBLISHER_RULES
+
+bool uORB::Manager::startsWith(const char *pre, const char *str)
+{
+	size_t lenpre = strlen(pre),
+	       lenstr = strlen(str);
+	return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
+}
+
+bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name)
+{
+	const char **topics_ptr = rule.topics;
+
+	while (*topics_ptr) {
+		if (strcmp(*topics_ptr, topic_name) == 0) {
+			return true;
+		}
+
+		++topics_ptr;
+	}
+
+	return false;
+}
+
+void uORB::Manager::strTrim(const char **str)
+{
+	while (**str == ' ' || **str == '\t') { ++(*str); }
+}
+
+int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
+{
+	FILE *fp;
+	static const int line_len = 1024;
+	int ret = PX4_OK;
+	char *line = new char[line_len];
+
+	if (!line) {
+		return -ENOMEM;
+	}
+
+	fp = fopen(file_name, "r");
+
+	if (fp == NULL) {
+		delete[](line);
+		return -errno;
+	}
+
+	const char *restrict_topics_str = "restrict_topics:";
+	const char *module_str = "module:";
+	const char *ignore_others = "ignore_others:";
+
+	rule.ignore_other_topics = false;
+	rule.module_name = nullptr;
+	rule.topics = nullptr;
+
+	while (fgets(line, line_len, fp) && ret == PX4_OK) {
+
+		if (strlen(line) < 2 || line[0] == '#') {
+			continue;
+		}
+
+		if (startsWith(restrict_topics_str, line)) {
+			//read topics list
+			char *start = line + strlen(restrict_topics_str);
+			strTrim((const char **)&start);
+			char *topics = strdup(start);
+			int topic_len = 0, num_topics = 0;
+
+			for (int i = 0; topics[i]; ++i) {
+				if (topics[i] == ',' || topics[i] == '\n') {
+					if (topic_len > 0) {
+						topics[i] = 0;
+						++num_topics;
+					}
+
+					topic_len = 0;
+
+				} else {
+					++topic_len;
+				}
+			}
+
+			if (num_topics > 0) {
+				rule.topics = new const char *[num_topics + 1];
+				int topic = 0;
+				strTrim((const char **)&topics);
+				rule.topics[topic++] = topics;
+
+				while (topic < num_topics) {
+					if (*topics == 0) {
+						++topics;
+						strTrim((const char **)&topics);
+						rule.topics[topic++] = topics;
+
+					} else {
+						++topics;
+					}
+				}
+
+				rule.topics[num_topics] = nullptr;
+			}
+
+		} else if (startsWith(module_str, line)) {
+			//read module name
+			char *start = line + strlen(module_str);
+			strTrim((const char **)&start);
+			int len = strlen(start);
+
+			if (len > 0 && start[len - 1] == '\n') {
+				start[len - 1] = 0;
+			}
+
+			rule.module_name = strdup(start);
+
+		} else if (startsWith(ignore_others, line)) {
+			const char *start = line + strlen(ignore_others);
+			strTrim(&start);
+
+			if (startsWith("true", start)) {
+				rule.ignore_other_topics = true;
+			}
+
+		} else {
+			PX4_ERR("orb rules file: wrong format: %s", line);
+			ret = -EINVAL;
+		}
+	}
+
+	if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
+		PX4_ERR("Wrong format in orb publisher rules file");
+		ret = -EINVAL;
+	}
+
+	delete[](line);
+	fclose(fp);
+	return ret;
+}
+#endif /* ORB_USE_PUBLISHER_RULES */

src/modules/uORB/uORBManager.hpp

@@ -446,6 +446,46 @@ private: //class methods
 	virtual int16_t process_received_message(const char *messageName,
 			int32_t length, uint8_t *data);
 
+
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	struct PublisherRule {
+		const char **topics; //null-terminated list of topic names
+		const char *module_name; //only this module is allowed to publish one of the topics
+		bool ignore_other_topics;
+	};
+
+	/**
+	 * test if str starts with pre
+	 */
+	bool startsWith(const char *pre, const char *str);
+
+	/**
+	 * find a topic in a rule
+	 */
+	bool findTopic(const PublisherRule &rule, const char *topic_name);
+
+	/**
+	 * trim whitespace from the beginning of a string
+	 */
+	void strTrim(const char **str);
+
+	/**
+	 * Read publisher rules from a file. It has the format:
+	 *
+	 * restrict_topics: <topic1>, <topic2>, <topic3>
+	 * module: <module_name>
+	 * [ignore_others:true]
+	 *
+	 * @return 0 on success, <0 otherwise
+	 */
+	int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
+
+	PublisherRule _publisher_rule;
+	bool _has_publisher_rules = false;
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 };
 
 #endif /* _uORBManager_hpp_ */