From 92877e1f350088aaa90bfd3f9d27c0db6939f4c3 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Wed, 9 Dec 2020 10:59:42 +0200 Subject: [PATCH] Add IOCTL interface to uORBManager for nuttx protected/kernel build split When building uORB for NuttX flat build, or for some other target, everything works as before. When building uORB for NuttX protected or kernel build, this does the following: - The kernel side uORB library reigsters a boardctl handler for calls from userspace and services the boardctl_ioctls by calling the actual uORB functions - For user mode binaries, the uORBManager acts as a proxy, making boardctl_ioctl calls to the kernel side - The uORBDeviceNodes are always allocated in user heap for protected build, so that they are accessible from both kernel and user side. This is only for optimization, a few functions such as "is_advertised" get really heavy when called frequently via ioctl Signed-off-by: Jukka Laitinen --- platforms/common/uORB/uORBDeviceMaster.cpp | 8 + platforms/common/uORB/uORBDeviceNode.hpp | 13 + platforms/common/uORB/uORBManager.cpp | 131 +++++++- platforms/common/uORB/uORBManager.hpp | 101 +++++- platforms/common/uORB/uORBManagerUsr.cpp | 340 +++++++++++++++++++++ 5 files changed, 590 insertions(+), 3 deletions(-) create mode 100644 platforms/common/uORB/uORBManagerUsr.cpp diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/platforms/common/uORB/uORBDeviceMaster.cpp index 3998db275e..79d7aba6de 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/platforms/common/uORB/uORBDeviceMaster.cpp @@ -49,6 +49,10 @@ #include #endif // PX4_QURT +#if defined(__PX4_NUTTX) +#include +#endif + uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); @@ -111,7 +115,11 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver /* if we didn't get a device, that's bad, free the path too */ if (node == nullptr) { +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) + kmm_free((void *)devpath); +#else free((void *)devpath); +#endif return -ENOMEM; } diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index e6bab77564..f30d2f3495 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -233,6 +233,19 @@ public: // remove item from list of work items void unregister_callback(SubscriptionCallback *callback_sub); + // Allocate this always from user heap, also in NuttX + // protected build. This allows direct access to some member + // variables from the whole system; sort of a shared memmory + // TODO: For nuttx kernel build, in shared memory + void *operator new (size_t nbytes) + { + return malloc(nbytes); + } + void operator delete (void *p) + { + free(p); + } + protected: px4_pollevent_t poll_state(cdev::file_t *filp) override; diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index e3cc3bd31c..6a0b17ffb7 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -40,6 +40,10 @@ #include #include +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) +#include +#endif + #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -52,6 +56,9 @@ bool uORB::Manager::initialize() _Instance = new uORB::Manager(); } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) + px4_register_boardct_ioctl(_ORBIOCBASE, orb_ioctl); +#endif return _Instance != nullptr; } @@ -103,6 +110,112 @@ uORB::DeviceMaster *uORB::Manager::get_device_master() return _device_master; } +#if defined (__PX4_NUTTX) && !defined (CONFIG_BUILD_FLAT) && defined(__KERNEL__) +int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = PX4_OK; + + switch (cmd) { + case ORBIOCDEVEXISTS: { + orbiocdevexists_t *data = (orbiocdevexists_t *)arg; + + if (data->check_advertised) { + data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance); + + } else { + data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR; + } + } + break; + + case ORBIOCDEVADVERTISE: { + orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg; + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance); + + } else { + data->ret = PX4_ERROR; + } + } + break; + + case ORBIOCDEVUNADVERTISE: { + orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg; + data->ret = uORB::Manager::orb_unadvertise(data->handle); + } + break; + + case ORBIOCDEVPUBLISH: { + orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg; + data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data); + } + break; + + case ORBIOCDEVADDSUBSCRIBER: { + orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg; + data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation); + } + break; + + case ORBIOCDEVREMSUBSCRIBER: { + uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast(arg)); + } + break; + + case ORBIOCDEVQUEUESIZE: { + orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg; + data->size = uORB::Manager::orb_get_queue_size(data->handle); + } + break; + + case ORBIOCDEVDATACOPY: { + orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg; + data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation); + } + break; + + case ORBIOCDEVREGCALLBACK: { + orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg; + data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub); + } + break; + + case ORBIOCDEVUNREGCALLBACK: { + orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg; + uORB::Manager::unregister_callback(data->handle, data->callback_sub); + } + break; + + case ORBIOCDEVGETINSTANCE: { + orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg; + data->instance = uORB::Manager::orb_get_instance(data->handle); + } + break; + + case ORBIOCDEVMASTERCMD: { + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + if (arg == ORB_DEVMASTER_TOP) { + dev->showTop(nullptr, 0); + + } else { + dev->printStatistics(); + } + } + } + break; + + default: + ret = -ENOTTY; + } + + return ret; +} +#endif + int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { int ret = PX4_ERROR; @@ -360,11 +473,25 @@ bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &genera } // add item to list of work items to schedule on node update -bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) { return static_cast(node_handle)->register_callback(callback_sub); } +bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + return static_cast(node_handle)->register_callback(callback_sub); +} // remove item from list of work items -void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) { static_cast(node_handle)->unregister_callback(callback_sub); } +void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + static_cast(node_handle)->unregister_callback(callback_sub); +} +uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +{ + if (node_handle) { + return static_cast(node_handle)->get_instance(); + } + + return -1; +} int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) { diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 92c7cb2cd3..a0cc29d691 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -37,7 +37,8 @@ #include "uORBDeviceNode.hpp" #include "uORBCommon.hpp" #include "uORBDeviceMaster.hpp" - +#include // For ORB_ID enum +#include #include #ifdef __PX4_NUTTX @@ -58,6 +59,100 @@ class Manager; class SubscriptionCallback; } + +/* + * IOCTLs for manager to access device nodes using + * a handle + * + * This is WIP; handle is just a pointer to kernel side object, so this is a + * security hole. + * + * The handle in the user side should just be a file descriptor, and IOCTL go + * directly to the device nodes. + * But for now, publisher doesn't even keep the descriptors open. + * This needs to be addressed later! + */ + +#define ORBIOCDEVEXISTS _ORBIOC(30) +typedef struct orbiocdevexists { + const ORB_ID orb_id; + const uint8_t instance; + const bool check_advertised; + int ret; +} orbiocdevexists_t; + +#define ORBIOCDEVADVERTISE _ORBIOC(31) +typedef struct orbiocadvertise { + const struct orb_metadata *meta; + bool is_advertiser; + int *instance; + int ret; +} orbiocdevadvertise_t; + +#define ORBIOCDEVUNADVERTISE _ORBIOC(32) +typedef struct orbiocunadvertise { + void *handle; + int ret; +} orbiocdevunadvertise_t; + +#define ORBIOCDEVPUBLISH _ORBIOC(33) +typedef struct orbiocpublish { + const struct orb_metadata *meta; + orb_advert_t handle; + const void *data; + int ret; +} orbiocdevpublish_t; + +#define ORBIOCDEVADDSUBSCRIBER _ORBIOC(34) +typedef struct { + const ORB_ID orb_id; + const uint8_t instance; + unsigned *initial_generation; + void *handle; +} orbiocdevaddsubscriber_t; + +#define ORBIOCDEVREMSUBSCRIBER _ORBIOC(35) + +#define ORBIOCDEVQUEUESIZE _ORBIOC(36) +typedef struct { + const void *handle; + uint8_t size; +} orbiocdevqueuesize_t; + +#define ORBIOCDEVDATACOPY _ORBIOC(37) +typedef struct { + void *handle; + void *dst; + unsigned generation; + bool ret; +} orbiocdevdatacopy_t; + +#define ORBIOCDEVREGCALLBACK _ORBIOC(38) +typedef struct { + void *handle; + class uORB::SubscriptionCallback *callback_sub; + bool registered; +} orbiocdevregcallback_t; + +#define ORBIOCDEVUNREGCALLBACK _ORBIOC(39) +typedef struct { + void *handle; + class uORB::SubscriptionCallback *callback_sub; +} orbiocdevunregcallback_t; + +#define ORBIOCDEVGETINSTANCE _ORBIOC(40) +typedef struct { + const void *handle; + uint8_t instance; +} orbiocdevgetinstance_t; + +typedef enum { + ORB_DEVMASTER_STATUS = 0, + ORB_DEVMASTER_TOP = 1 +} orbiocdevmastercmd_t; +#define ORBIOCDEVMASTERCMD _ORBIOC(45) + + /** * This is implemented as a singleton. This class manages creating the * uORB nodes for each uORB topics and also implements the behavor of the @@ -98,6 +193,10 @@ public: */ uORB::DeviceMaster *get_device_master(); +#if defined (__PX4_NUTTX) && !defined (CONFIG_BUILD_FLAT) && defined(__KERNEL__) + static int orb_ioctl(unsigned int cmd, unsigned long arg); +#endif + // ==== uORB interface methods ==== /** * Advertise as the publisher of a topic. diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp new file mode 100644 index 0000000000..8f664b24ca --- /dev/null +++ b/platforms/common/uORB/uORBManagerUsr.cpp @@ -0,0 +1,340 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "uORBDeviceNode.hpp" +#include "uORBUtils.hpp" +#include "uORBManager.hpp" + +uORB::Manager *uORB::Manager::_Instance = nullptr; + +bool uORB::Manager::initialize() +{ + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance != nullptr; +} + +bool uORB::Manager::terminate() +{ + if (_Instance != nullptr) { + delete _Instance; + _Instance = nullptr; + return true; + } + + return false; +} + +uORB::Manager::Manager() +{ +} + +uORB::Manager::~Manager() +{ +} + +int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +{ + // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) + if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { + return PX4_ERROR; + } + + orbiocdevexists_t data = {static_cast(meta->o_id), static_cast(instance), true, PX4_ERROR}; + boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); + + return data.ret; +} + +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + unsigned int queue_size) +{ + /* open the node as an advertiser */ + int fd = node_open(meta, true, instance); + + if (fd == PX4_ERROR) { + PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); + return nullptr; + } + + /* Set the queue size. This must be done before the first publication; thus it fails if + * this is not the first advertiser. + */ + int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); + + if (result < 0 && queue_size > 1) { + PX4_WARN("orb_advertise_multi: failed to set queue size"); + } + + /* get the advertiser handle and close the node */ + orb_advert_t advertiser; + + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); + + if (result == PX4_ERROR) { + PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); + return nullptr; + } + + /* the advertiser may perform an initial publish to initialise the object */ + if (data != nullptr) { + result = orb_publish(meta, advertiser, data); + + if (result == PX4_ERROR) { + PX4_ERR("orb_publish failed %s", meta->o_name); + return nullptr; + } + } + + return advertiser; +} + +int uORB::Manager::orb_unadvertise(orb_advert_t handle) +{ + orbiocdevunadvertise_t data = {handle, PX4_ERROR}; + boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast(&data)); + + return data.ret; +} + +int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(meta, false); +} + +int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(meta, false, &inst); +} + +int uORB::Manager::orb_unsubscribe(int fd) +{ + return px4_close(fd); +} + +int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR}; + boardctl(ORBIOCDEVPUBLISH, reinterpret_cast(&d)); + + return d.ret; +} + +int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = px4_read(handle, buffer, meta->o_size); + + if (ret < 0) { + return PX4_ERROR; + } + + if (ret != (int)meta->o_size) { + errno = EIO; + return PX4_ERROR; + } + + return PX4_OK; +} + +int uORB::Manager::orb_check(int handle, bool *updated) +{ + /* Set to false here so that if `px4_ioctl` fails to false. */ + *updated = false; + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int uORB::Manager::orb_set_interval(int handle, unsigned interval) +{ + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + +int uORB::Manager::orb_get_interval(int handle, unsigned *interval) +{ + int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); + *interval /= 1000; + return ret; +} + +int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) +{ + char path[orb_maxpath]; + int fd = -1; + int ret = PX4_ERROR; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return PX4_ERROR; + } + + /* if we have an instance and are an advertiser, we will generate a new node and set the instance, + * so we do not need to open here */ + if (!instance || !advertiser) { + /* + * Generate the path to the node and try to open it. + */ + ret = uORB::Utils::node_mkpath(path, meta, instance); + + if (ret != OK) { + errno = -ret; + return PX4_ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { + *instance = 0; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + ret = PX4_ERROR; + + orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR}; + boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data); + ret = data.ret; + + /* it's OK if it already exists */ + if ((ret != PX4_OK) && (EEXIST == errno)) { + ret = PX4_OK; + } + + if (ret == PX4_OK) { + /* update the path, as it might have been updated during the node advertise call */ + ret = uORB::Utils::node_mkpath(path, meta, instance); + + /* on success, try to open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { + errno = -ret; + return PX4_ERROR; + } + } + } + + if (fd < 0) { + errno = EIO; + return PX4_ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +{ + orbiocdevexists_t data = {orb_id, instance, false, 0}; + boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); + + return data.ret == PX4_OK ? true : false; +} + +void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) +{ + orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr}; + boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast(&data)); + + return data.handle; +} + +void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) +{ + boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast(node_handle)); +} + +uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) +{ + orbiocdevqueuesize_t data = {node_handle, 0}; + boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast(&data)); + + return data.size; +} + +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation) +{ + orbiocdevdatacopy_t data = {node_handle, dst, generation, false}; + boardctl(ORBIOCDEVDATACOPY, reinterpret_cast(&data)); + generation = data.generation; + + return data.ret; +} + +bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + orbiocdevregcallback_t data = {node_handle, callback_sub, false}; + boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast(&data)); + + return data.registered; +} + +void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + orbiocdevunregcallback_t data = {node_handle, callback_sub}; + boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast(&data)); +} + +uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +{ + orbiocdevgetinstance_t data = {node_handle, 0}; + boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast(&data)); + + return data.instance; +} + +void uORB::Manager::device_master_cmd(orbiocdevmastercmd_t cmd) +{ + boardctl(ORBIOCDEVMASTERCMD, cmd); +}