diff options
Diffstat (limited to 'apps/uORB/uORB.cpp')
| -rw-r--r-- | apps/uORB/uORB.cpp | 1006 |
1 files changed, 0 insertions, 1006 deletions
diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp deleted file mode 100644 index 7abbf42ae..000000000 --- a/apps/uORB/uORB.cpp +++ /dev/null @@ -1,1006 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -/** - * @file uORB.cpp - * A lightweight object broker. - */ - -#include <nuttx/config.h> - -#include <drivers/device/device.h> - -#include <sys/types.h> -#include <stdint.h> -#include <stdbool.h> -#include <string.h> -#include <stdlib.h> -#include <fcntl.h> -#include <poll.h> -#include <errno.h> -#include <stdio.h> -#include <math.h> -#include <unistd.h> - -#include <nuttx/arch.h> -#include <nuttx/wqueue.h> -#include <nuttx/clock.h> - -#include <drivers/drv_hrt.h> - -#include <drivers/drv_orb_dev.h> - -#include "uORB.h" - -/** - * Utility functions. - */ -namespace -{ - -static const unsigned orb_maxpath = 64; - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -enum Flavor { - PUBSUB, - PARAM -}; - -int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) -{ - unsigned len; - - len = snprintf(buf, orb_maxpath, "/%s/%s", - (f == PUBSUB) ? "obj" : "param", - meta->o_name); - - if (len >= orb_maxpath) - return -ENAMETOOLONG; - - return OK; -} - -} - -/** - * Per-object device instance. - */ -class ORBDevNode : public device::CDev -{ -public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); - ~ORBDevNode(); - - virtual int open(struct file *filp); - virtual int close(struct file *filp); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); - -protected: - virtual pollevent_t poll_state(struct file *filp); - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); - -private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ - struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - }; - - const struct orb_metadata *_meta; /**< object metadata information */ - uint8_t *_data; /**< allocated object buffer */ - hrt_abstime _last_update; /**< time the object was last updated */ - volatile unsigned _generation; /**< object generation count */ - pid_t _publisher; /**< if nonzero, current publisher */ - - SubscriberData *filp_to_sd(struct file *filp) { - SubscriberData *sd = (SubscriberData *)(filp->f_priv); - return sd; - } - - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); - - /** - * Bridge from hrt_call to update_deferred - * - * void *arg ORBDevNode pointer for which the deferred update is performed. - */ - static void update_deferred_trampoline(void *arg); - - /** - * Check whether a topic appears updated to a subscriber. - * - * @param sd The subscriber for whom to check. - * @return True if the topic should appear updated to the subscriber - */ - bool appears_updated(SubscriberData *sd); -}; - -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : - CDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0) -{ - // enable debug() calls - _debug_enabled = true; -} - -ORBDevNode::~ORBDevNode() -{ - if (_data != nullptr) - delete[] _data; -} - -int -ORBDevNode::open(struct file *filp) -{ - int ret; - - /* is this a publisher? */ - if (filp->f_oflags == O_WRONLY) { - - /* become the publisher if we can */ - lock(); - - if (_publisher == 0) { - _publisher = getpid(); - ret = OK; - - } else { - ret = -EBUSY; - } - - unlock(); - - /* now complete the open */ - if (ret == OK) { - ret = CDev::open(filp); - - /* open failed - not the publisher anymore */ - if (ret != OK) - _publisher = 0; - } - - return ret; - } - - /* is this a new subscriber? */ - if (filp->f_oflags == O_RDONLY) { - - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; - - if (nullptr == sd) - return -ENOMEM; - - memset(sd, 0, sizeof(*sd)); - - /* default to no pending update */ - sd->generation = _generation; - - filp->f_priv = (void *)sd; - - ret = CDev::open(filp); - - if (ret != OK) - free(sd); - - return ret; - } - - /* can only be pub or sub, not both */ - return -EINVAL; -} - -int -ORBDevNode::close(struct file *filp) -{ - /* is this the publisher closing? */ - if (getpid() == _publisher) { - _publisher = 0; - - } else { - SubscriberData *sd = filp_to_sd(filp); - - if (sd != nullptr) - delete sd; - } - - return CDev::close(filp); -} - -ssize_t -ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) -{ - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; - - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) - return -EIO; - - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); - - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) - memcpy(buffer, _data, _meta->o_size); - - /* track the last generation that the file has seen */ - sd->generation = _generation; - - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; - - irqrestore(flags); - - return _meta->o_size; -} - -ssize_t -ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) -{ - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. - * - * Note that filp will usually be NULL. - */ - if (nullptr == _data) { - if (!up_interrupt_context()) { - - lock(); - - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; - - unlock(); - } - - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } - - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) - return -EIO; - - /* Perform an atomic copy. */ - irqstate_t flags = irqsave(); - memcpy(_data, buffer, _meta->o_size); - irqrestore(flags); - - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - _generation++; - - /* notify any poll waiters */ - poll_notify(POLLIN); - - return _meta->o_size; -} - -int -ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - SubscriberData *sd = filp_to_sd(filp); - - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return OK; - - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return OK; - - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return OK; - - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return OK; - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -ssize_t -ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) -{ - ORBDevNode *devnode = (ORBDevNode *)handle; - int ret; - - /* this is a bit risky, since we are trusting the handle in order to deref it */ - if (devnode->_meta != meta) { - errno = EINVAL; - return ERROR; - } - - /* call the devnode write method with no file pointer */ - ret = devnode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; -} - -pollevent_t -ORBDevNode::poll_state(struct file *filp) -{ - SubscriberData *sd = filp_to_sd(filp); - - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; - - return 0; -} - -void -ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events) -{ - SubscriberData *sd = filp_to_sd((struct file *)fds->priv); - - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) - CDev::poll_notify_one(fds, events); -} - -bool -ORBDevNode::appears_updated(SubscriberData *sd) -{ - /* assume it doesn't look updated */ - bool ret = false; - - /* avoid racing between interrupt and non-interrupt context calls */ - irqstate_t state = irqsave(); - - /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) { - ret = false; - goto out; - } - - /* - * If the subscriber's generation count matches the update generation - * count, there has been no update from their perspective; if they - * don't match then we might have a visible update. - */ - while (sd->generation != _generation) { - - /* - * Handle non-rate-limited subscribers. - */ - if (sd->update_interval == 0) { - ret = true; - break; - } - - /* - * If we have previously told the subscriber that there is data, - * and they have not yet collected it, continue to tell them - * that there has been an update. This mimics the non-rate-limited - * behaviour where checking / polling continues to report an update - * until the topic is read. - */ - if (sd->update_reported) { - ret = true; - break; - } - - /* - * If the interval timer is still running, the topic should not - * appear updated, even though at this point we know that it has. - * We have previously been through here, so the subscriber - * must have collected the update we reported, otherwise - * update_reported would still be true. - */ - if (!hrt_called(&sd->update_call)) - break; - - /* - * Make sure that we don't consider the topic to be updated again - * until the interval has passed once more by restarting the interval - * timer and thereby re-scheduling a poll notification at that time. - */ - hrt_call_after(&sd->update_call, - sd->update_interval, - &ORBDevNode::update_deferred_trampoline, - (void *)this); - - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; - - break; - } - -out: - irqrestore(state); - - /* consider it updated */ - return ret; -} - -void -ORBDevNode::update_deferred() -{ - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); -} - -void -ORBDevNode::update_deferred_trampoline(void *arg) -{ - ORBDevNode *node = (ORBDevNode *)arg; - - node->update_deferred(); -} - -/** - * Master control device for ObjDev. - * - * Used primarily to create new objects via the ORBIOCCREATE - * ioctl. - */ -class ORBDevMaster : public device::CDev -{ -public: - ORBDevMaster(Flavor f); - ~ORBDevMaster(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); -private: - Flavor _flavor; -}; - -ORBDevMaster::ORBDevMaster(Flavor f) : - CDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _flavor(f) -{ - // enable debug() calls - _debug_enabled = true; - -} - -ORBDevMaster::~ORBDevMaster() -{ -} - -int -ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret; - - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_metadata *meta = (const struct orb_metadata *)arg; - const char *objname; - char nodepath[orb_maxpath]; - ORBDevNode *node; - - /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta); - - if (ret != OK) - return ret; - - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) - return -ENOMEM; - - /* construct the new node */ - node = new ORBDevNode(meta, objname, nodepath); - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - if (node != nullptr) - ret = node->init(); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) - return -ENOMEM; - - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); - } - - return ret; - } - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - - -/** - * Local functions in support of the shell command. - */ - -namespace -{ - -ORBDevMaster *g_dev; - -struct orb_test { - int val; -}; - -ORB_DEFINE(orb_test, struct orb_test); - -int -test_fail(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "FAIL: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return ERROR; -} - -int -test_note(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "note: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return OK; -} - -ORB_DECLARE(sensor_combined); - -int -test() -{ - struct orb_test t, u; - int pfd, sfd; - bool updated; - - t.val = 0; - pfd = orb_advertise(ORB_ID(orb_test), &t); - - if (pfd < 0) - return test_fail("advertise failed: %d", errno); - - test_note("publish handle 0x%08x", pfd); - sfd = orb_subscribe(ORB_ID(orb_test)); - - if (sfd < 0) - return test_fail("subscribe failed: %d", errno); - - test_note("subscribe fd %d", sfd); - u.val = 1; - - if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(1) failed: %d", errno); - - if (u.val != t.val) - return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); - - if (OK != orb_check(sfd, &updated)) - return test_fail("check(1) failed"); - - if (updated) - return test_fail("spurious updated flag"); - - t.val = 2; - test_note("try publish"); - - if (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) - return test_fail("publish failed"); - - if (OK != orb_check(sfd, &updated)) - return test_fail("check(2) failed"); - - if (!updated) - return test_fail("missing updated flag"); - - if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(2) failed: %d", errno); - - if (u.val != t.val) - return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); - - orb_unsubscribe(sfd); - close(pfd); - -#if 0 - /* this is a hacky test that exploits the sensors app to test rate-limiting */ - - sfd = orb_subscribe(ORB_ID(sensor_combined)); - - hrt_abstime start, end; - unsigned count; - - start = hrt_absolute_time(); - count = 0; - - do { - orb_check(sfd, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); - - end = hrt_absolute_time(); - test_note("full-speed, 100 updates in %llu", end - start); - - orb_set_interval(sfd, 10); - - start = hrt_absolute_time(); - count = 0; - - do { - orb_check(sfd, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); - - end = hrt_absolute_time(); - test_note("100Hz, 100 updates in %llu", end - start); - - orb_unsubscribe(sfd); -#endif - - return test_note("PASS"); -} - -int -info() -{ - return OK; -} - - -} // namespace - -/* - * uORB server 'main'. - */ -extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } - -int -uorb_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) { - fprintf(stderr, "[uorb] already loaded\n"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } - - /* create the driver */ - g_dev = new ORBDevMaster(PUBSUB); - - if (g_dev == nullptr) { - fprintf(stderr, "[uorb] driver alloc failed\n"); - return -ENOMEM; - } - - if (OK != g_dev->init()) { - fprintf(stderr, "[uorb] driver init failed\n"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - printf("[uorb] ready\n"); - return OK; - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - return test(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - return info(); - - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n"); - return -EINVAL; -} - -/* - * Library functions. - */ -namespace -{ - -void debug(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - usleep(100000); -} - -/** - * Advertise a node; don't consider it an error if the node has - * already been advertised. - * - * @todo verify that the existing node is the same as the one - * we tried to advertise. - */ -int -node_advertise(const struct orb_metadata *meta) -{ - int fd = -1; - int ret = ERROR; - - /* open the control device */ - fd = open(TOPIC_MASTER_DEVICE_PATH, 0); - - if (fd < 0) - goto out; - - /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); - - /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) - ret = OK; - -out: - - if (fd >= 0) - close(fd); - - return ret; -} - -/** - * Common implementation for orb_advertise and orb_subscribe. - * - * Handles creation of the object and the initial publication for - * advertisers. - */ -int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) -{ - char path[orb_maxpath]; - int fd, ret; - - /* - * 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 ERROR; - } - - /* - * Advertiser must publish an initial value. - */ - if (advertiser && (data == nullptr)) { - errno = EINVAL; - return ERROR; - } - - /* - * Generate the path to the node and try to open it. - */ - ret = node_mkpath(path, f, meta); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); - - /* we may need to advertise the node... */ - if (fd < 0) { - - /* try to create the node */ - ret = node_advertise(meta); - - /* on success, try the open again */ - if (ret == OK) - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); - } - - if (fd < 0) { - errno = EIO; - return ERROR; - } - - /* everything has been OK, we can return the handle now */ - return fd; -} - -} // namespace - -orb_advert_t -orb_advertise(const struct orb_metadata *meta, const void *data) -{ - int result, fd; - orb_advert_t advertiser; - - /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true); - if (fd == ERROR) - return ERROR; - - /* get the advertiser handle and close the node */ - result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - close(fd); - if (result == ERROR) - return ERROR; - - /* the advertiser must perform an initial publish to initialise the object */ - result= orb_publish(meta, advertiser, data); - if (result == ERROR) - return ERROR; - - return advertiser; -} - -int -orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(PUBSUB, meta, nullptr, false); -} - -int -orb_unsubscribe(int handle) -{ - return close(handle); -} - -int -orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) -{ - return ORBDevNode::publish(meta, handle, data); -} - -int -orb_copy(const struct orb_metadata *meta, int handle, void *buffer) -{ - int ret; - - ret = read(handle, buffer, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; -} - -int -orb_check(int handle, bool *updated) -{ - return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); -} - -int -orb_stat(int handle, uint64_t *time) -{ - return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); -} - -int -orb_set_interval(int handle, unsigned interval) -{ - return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); -} - |