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 <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen
2020-12-09 10:59:42 +02:00
parent f54e56d6a8
commit 92877e1f35
5 changed files with 590 additions and 3 deletions
@@ -49,6 +49,10 @@
#include <poll.h>
#endif // PX4_QURT
#if defined(__PX4_NUTTX)
#include <nuttx/mm/mm.h>
#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;
}
+13
View File
@@ -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;
+129 -2
View File
@@ -40,6 +40,10 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
#include <px4_platform/board_ctrl.h>
#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<void *>(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<DeviceNode *>(node_handle)->register_callback(callback_sub); }
bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub)
{
return static_cast<DeviceNode *>(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<DeviceNode *>(node_handle)->unregister_callback(callback_sub); }
void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub)
{
static_cast<DeviceNode *>(node_handle)->unregister_callback(callback_sub);
}
uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
{
if (node_handle) {
return static_cast<const uORB::DeviceNode *>(node_handle)->get_instance();
}
return -1;
}
int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
{
+100 -1
View File
@@ -37,7 +37,8 @@
#include "uORBDeviceNode.hpp"
#include "uORBCommon.hpp"
#include "uORBDeviceMaster.hpp"
#include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
#include <lib/cdev/CDev.hpp>
#include <stdint.h>
#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.
+340
View File
@@ -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 <sys/types.h>
#include <sys/stat.h>
#include <stdarg.h>
#include <fcntl.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <sys/boardctl.h>
#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<ORB_ID>(meta->o_id), static_cast<uint8_t>(instance), true, PX4_ERROR};
boardctl(ORBIOCDEVEXISTS, reinterpret_cast<unsigned long>(&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<unsigned long>(&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<unsigned long>(&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<unsigned long>(&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<unsigned long>(&data));
return data.handle;
}
void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
{
boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast<unsigned long>(node_handle));
}
uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle)
{
orbiocdevqueuesize_t data = {node_handle, 0};
boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast<unsigned long>(&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<unsigned long>(&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<unsigned long>(&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<unsigned long>(&data));
}
uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
{
orbiocdevgetinstance_t data = {node_handle, 0};
boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast<unsigned long>(&data));
return data.instance;
}
void uORB::Manager::device_master_cmd(orbiocdevmastercmd_t cmd)
{
boardctl(ORBIOCDEVMASTERCMD, cmd);
}