diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index bebb35f989..0b3ee4ded5 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -47,483 +47,499 @@ uORB::ORBMap uORB::DeviceMaster::_node_map; uORB::DeviceNode::DeviceNode ( - const struct orb_metadata *meta, - const char *name, - const char *path, - int priority - ) : - CDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0), - _priority(priority), - _IsRemoteSubscriberPresent( false ), - _subscriber_count(0) + const struct orb_metadata *meta, + const char *name, + const char *path, + int priority +) : + CDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority), + _IsRemoteSubscriberPresent(false), + _subscriber_count(0) { - // enable debug() calls - _debug_enabled = true; + // enable debug() calls + _debug_enabled = true; } uORB::DeviceNode::~DeviceNode() { - if (_data != nullptr) - delete[] _data; + if (_data != nullptr) { + delete[] _data; + } } int uORB::DeviceNode::open(struct file *filp) { - int ret; + int ret; - /* is this a publisher? */ - if (filp->f_oflags == O_WRONLY) { + /* is this a publisher? */ + if (filp->f_oflags == O_WRONLY) { - /* become the publisher if we can */ - lock(); + /* become the publisher if we can */ + lock(); - if (_publisher == 0) { - _publisher = getpid(); - ret = OK; + if (_publisher == 0) { + _publisher = getpid(); + ret = OK; - } else { - ret = -EBUSY; - } + } else { + ret = -EBUSY; + } - unlock(); + unlock(); - /* now complete the open */ - if (ret == OK) { - ret = CDev::open(filp); + /* now complete the open */ + if (ret == OK) { + ret = CDev::open(filp); - /* open failed - not the publisher anymore */ - if (ret != OK) - _publisher = 0; - } + /* open failed - not the publisher anymore */ + if (ret != OK) { + _publisher = 0; + } + } - return ret; - } + return ret; + } - /* is this a new subscriber? */ - if (filp->f_oflags == O_RDONLY) { + /* is this a new subscriber? */ + if (filp->f_oflags == O_RDONLY) { - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; - if (nullptr == sd) - return -ENOMEM; + if (nullptr == sd) { + return -ENOMEM; + } - memset(sd, 0, sizeof(*sd)); + memset(sd, 0, sizeof(*sd)); - /* default to no pending update */ - sd->generation = _generation; + /* default to no pending update */ + sd->generation = _generation; - /* set priority */ - sd->priority = _priority; + /* set priority */ + sd->priority = _priority; - filp->f_priv = (void *)sd; + filp->f_priv = (void *)sd; - ret = CDev::open(filp); + ret = CDev::open(filp); - add_internal_subscriber(); + add_internal_subscriber(); - if (ret != OK) - delete sd; + if (ret != OK) { + delete sd; + } - return ret; - } + return ret; + } - /* can only be pub or sub, not both */ - return -EINVAL; + /* can only be pub or sub, not both */ + return -EINVAL; } int uORB::DeviceNode::close(struct file *filp) { - /* is this the publisher closing? */ - if (getpid() == _publisher) { - _publisher = 0; + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; - } else { - SubscriberData *sd = filp_to_sd(filp); + } else { + SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) { - hrt_cancel(&sd->update_call); - remove_internal_subscriber(); - delete sd; - sd = nullptr; - } - } + if (sd != nullptr) { + hrt_cancel(&sd->update_call); + remove_internal_subscriber(); + delete sd; + sd = nullptr; + } + } - return CDev::close(filp); + return CDev::close(filp); } ssize_t uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen) { - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; + /* 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; + /* 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(); + /* + * 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); + /* 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; + /* track the last generation that the file has seen */ + sd->generation = _generation; - /* set priority */ - sd->priority = _priority; + /* set priority */ + sd->priority = _priority; - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; - irqrestore(flags); + irqrestore(flags); - return _meta->o_size; + return _meta->o_size; } ssize_t uORB::DeviceNode::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()) { + /* + * 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(); + lock(); - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; + /* re-check size */ + if (nullptr == _data) { + _data = new uint8_t[_meta->o_size]; + } - unlock(); - } + unlock(); + } - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } + /* 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; + /* 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); + /* 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++; + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; - /* notify any poll waiters */ - poll_notify(POLLIN); + /* notify any poll waiters */ + poll_notify(POLLIN); - return _meta->o_size; + return _meta->o_size; } int uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = filp_to_sd(filp); - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return OK; + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return OK; - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return OK; + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return OK; - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return OK; + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return OK; - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return OK; + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; - case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; - return OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } } ssize_t uORB::DeviceNode::publish ( - const orb_metadata *meta, - orb_advert_t handle, - const void *data + const orb_metadata *meta, + orb_advert_t handle, + const void *data ) { - uORB::DeviceNode *DeviceNode = (uORB::DeviceNode *)handle; - int ret; + uORB::DeviceNode *DeviceNode = (uORB::DeviceNode *)handle; + int ret; - /* this is a bit risky, since we are trusting the handle in order to deref it */ - if (DeviceNode->_meta != meta) { - errno = EINVAL; - return ERROR; - } + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (DeviceNode->_meta != meta) { + errno = EINVAL; + return ERROR; + } - /* call the DeviceNode write method with no file pointer */ - ret = DeviceNode->write(nullptr, (const char *)data, meta->o_size); + /* call the DeviceNode write method with no file pointer */ + ret = DeviceNode->write(nullptr, (const char *)data, meta->o_size); - if (ret < 0) - return ERROR; + if (ret < 0) { + return ERROR; + } - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - /* - * if the write is successful, send the data over the Multi-ORB link - */ - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( ch != nullptr ) - { - if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) - { - warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", - __LINE__, meta->o_name ); - return ERROR; - } - } - return OK; + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr) { + if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) { + warnx("[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name); + return ERROR; + } + } + + return OK; } pollevent_t uORB::DeviceNode::poll_state(struct file *filp) { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = filp_to_sd(filp); - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) { + return POLLIN; + } - return 0; + return 0; } void uORB::DeviceNode::poll_notify_one(struct pollfd *fds, pollevent_t events) { - SubscriberData *sd = filp_to_sd((struct file *)fds->priv); + 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); + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) { + CDev::poll_notify_one(fds, events); + } } bool uORB::DeviceNode::appears_updated(SubscriberData *sd) { - /* assume it doesn't look updated */ - bool ret = false; + /* assume it doesn't look updated */ + bool ret = false; - /* avoid racing between interrupt and non-interrupt context calls */ - irqstate_t state = irqsave(); + /* 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; - } + /* 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) { + /* + * 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; - } + /* + * 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 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; + /* + * 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, - &uORB::DeviceNode::update_deferred_trampoline, - (void *)this); + /* + * 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, + &uORB::DeviceNode::update_deferred_trampoline, + (void *)this); - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; - break; - } + break; + } out: - irqrestore(state); + irqrestore(state); - /* consider it updated */ - return ret; + /* consider it updated */ + return ret; } void uORB::DeviceNode::update_deferred() { - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); } void uORB::DeviceNode::update_deferred_trampoline(void *arg) { - uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + uORB::DeviceNode *node = (uORB::DeviceNode *)arg; - node->update_deferred(); + node->update_deferred(); } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void uORB::DeviceNode::add_internal_subscriber() { - _subscriber_count++; - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( ch != nullptr && _subscriber_count > 0 ) - { - ch->add_subscription( _meta->o_name, 1 ); - } + _subscriber_count++; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count > 0) { + ch->add_subscription(_meta->o_name, 1); + } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void uORB::DeviceNode::remove_internal_subscriber() { - _subscriber_count--; - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( ch != nullptr && _subscriber_count == 0 ) - { - ch->remove_subscription( _meta->o_name ); - } + _subscriber_count--; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count == 0) { + ch->remove_subscription(_meta->o_name); + } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz ) +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) { - // if there is already data in the node, send this out to - // the remote entity. - // send the data to the remote entity. - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher. - { - ch->send_message( _meta->o_name, _meta->o_size, _data); - } + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); - return OK; + if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. + ch->send_message(_meta->o_name, _meta->o_size, _data); + } + + return OK; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::DeviceNode::process_remove_subscription() { - return OK; + return OK; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data) +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) { - int16_t ret = -1; - if( length != (int32_t)(_meta->o_size) ) - { - warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", - __LINE__, _meta->o_name, (int)length, (int)_meta->o_size ); - return ERROR; - } + int16_t ret = -1; - /* call the devnode write method with no file pointer */ - ret = write(nullptr, (const char *)data, _meta->o_size); + if (length != (int32_t)(_meta->o_size)) { + warnx("[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size); + return ERROR; + } - if (ret < 0) - return ERROR; + /* call the devnode write method with no file pointer */ + ret = write(nullptr, (const char *)data, _meta->o_size); - if (ret != (int)_meta->o_size) { - errno = EIO; - return ERROR; - } + if (ret < 0) { + return ERROR; + } - return OK; + if (ret != (int)_meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; } uORB::DeviceMaster::DeviceMaster(Flavor f) : - CDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _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; + // enable debug() calls + _debug_enabled = true; } @@ -534,110 +550,111 @@ uORB::DeviceMaster::~DeviceMaster() int uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) { - int ret; + int ret; - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - const char *objname; - const char *devpath; - char nodepath[orb_maxpath]; - uORB::DeviceNode *node; + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + uORB::DeviceNode *node; - /* set instance to zero - we could allow selective multi-pubs later based on value */ - if (adv->instance != nullptr) { - *(adv->instance) = 0; - } + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath( nodepath, _flavor, meta, adv->instance); + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); - if (ret != OK) { - return ret; - } + if (ret != OK) { + return ret; + } - /* ensure that only one advertiser runs through this critical section */ - lock(); + /* ensure that only one advertiser runs through this critical section */ + lock(); - ret = ERROR; + ret = ERROR; - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } - if (objname == nullptr) { - return -ENOMEM; - } + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); - /* driver wants a permanent copy of the path, so make one here */ - devpath = strdup(nodepath); + if (objname == nullptr) { + return -ENOMEM; + } - if (devpath == nullptr) { - return -ENOMEM; - } + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); - /* construct the new node */ - node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); + if (devpath == nullptr) { + return -ENOMEM; + } - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - unlock(); - return -ENOMEM; - } + /* construct the new node */ + node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); - free((void *)devpath); - } - else - { - // add to the node map;. - _node_map.insert(nodepath, node); - } + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); - group_tries++; + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + free((void *)devpath); - } while (ret != OK && (group_tries < max_group_tries)); + } else { + // add to the node map;. + _node_map.insert(nodepath, node); + } - if (group_tries > max_group_tries) { - ret = -ENOMEM; - } + group_tries++; - /* the file handle for the driver has been created, unlock */ - unlock(); + } while (ret != OK && (group_tries < max_group_tries)); - return ret; - } + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } + /* the file handle for the driver has been created, unlock */ + unlock(); + + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } } -uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) +uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath) { - uORB::DeviceNode* rc = nullptr; - if( _node_map.find( nodepath ) ) - { - rc = _node_map.get(nodepath); - } - return rc; + uORB::DeviceNode *rc = nullptr; + + if (_node_map.find(nodepath)) { + rc = _node_map.get(nodepath); + } + + return rc; } diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 012e7893dd..4eb5f83101 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -43,8 +43,8 @@ namespace uORB { - class DeviceNode; - class DeviceMaster; +class DeviceNode; +class DeviceMaster; } /** @@ -53,166 +53,167 @@ namespace uORB class uORB::DeviceNode : public device::CDev { public: - /** - * Constructor - */ - DeviceNode - ( - const struct orb_metadata *meta, - const char *name, - const char *path, - int priority - ); + /** + * Constructor + */ + DeviceNode + ( + const struct orb_metadata *meta, + const char *name, + const char *path, + int priority + ); - /** - * Destructor - */ - ~DeviceNode(); + /** + * Destructor + */ + ~DeviceNode(); - /** - * Method to create a subscriber instance and return the struct - * pointing to the subscriber as a file pointer. - */ - virtual int open(struct file *filp); + /** + * Method to create a subscriber instance and return the struct + * pointing to the subscriber as a file pointer. + */ + virtual int open(struct file *filp); - /** - * Method to close a subscriber for this topic. - */ - virtual int close(struct file *filp); + /** + * Method to close a subscriber for this topic. + */ + virtual int close(struct file *filp); - /** - * reads data from a subscriber node to the buffer provided. - * @param filp - * The subscriber from which the data needs to be read from. - * @param buffer - * The buffer into which the data is read into. - * @param buflen - * the length of the buffer - * @return - * ssize_t the number of bytes read. - */ - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + /** + * reads data from a subscriber node to the buffer provided. + * @param filp + * The subscriber from which the data needs to be read from. + * @param buffer + * The buffer into which the data is read into. + * @param buflen + * the length of the buffer + * @return + * ssize_t the number of bytes read. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - /** - * writes the published data to the internal buffer to be read by - * subscribers later. - * @param filp - * the subscriber; this is not used. - * @param buffer - * The buffer for the input data - * @param buflen - * the length of the buffer. - * @return ssize_t - * The number of bytes that are written - */ - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + /** + * writes the published data to the internal buffer to be read by + * subscribers later. + * @param filp + * the subscriber; this is not used. + * @param buffer + * The buffer for the input data + * @param buflen + * the length of the buffer. + * @return ssize_t + * The number of bytes that are written + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - /** - * IOCTL control for the subscriber. - */ - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** + * IOCTL control for the subscriber. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - /** - * Method to publish a data to this node. - */ - static ssize_t publish - ( - const orb_metadata *meta, - orb_advert_t handle, - const void *data - ); + /** + * Method to publish a data to this node. + */ + static ssize_t publish + ( + const orb_metadata *meta, + orb_advert_t handle, + const void *data + ); - /** - * processes a request for add subscription from remote - * @param rateInHz - * Specifies the desired rate for the message. - * @return - * 0 = success - * otherwise failure. - */ - int16_t process_add_subscription( int32_t rateInHz ); + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription(int32_t rateInHz); - /** - * processes a request to remove a subscription from remote. - */ - int16_t process_remove_subscription(); + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); - /** - * processed the received data message from remote. - */ - int16_t process_received_message( int32_t length, uint8_t* data ); + /** + * processed the received data message from remote. + */ + int16_t process_received_message(int32_t length, uint8_t *data); - /** - * Add the subscriber to the node's list of subscriber. If there is - * remote proxy to which this subscription needs to be sent, it will - * done via uORBCommunicator::IChannel interface. - * @param sd - * the subscriber to be added. - */ - void add_internal_subscriber(); + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); - /** - * Removes the subscriber from the list. Also notifies the remote - * if there a uORBCommunicator::IChannel instance. - * @param sd - * the Subscriber to be removed. - */ - void remove_internal_subscriber(); + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); protected: - virtual pollevent_t poll_state(struct file *filp); - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + 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 */ - int priority; /**< priority of publisher */ - }; + 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 */ + int priority; /**< priority of publisher */ + }; - 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 */ - const int _priority; /**< priority of topic */ + 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 */ + const int _priority; /**< priority of topic */ private: // private class methods. - SubscriberData *filp_to_sd(struct file *filp) { - SubscriberData *sd = (SubscriberData *)(filp->f_priv); - return sd; - } + SubscriberData *filp_to_sd(struct file *filp) + { + SubscriberData *sd = (SubscriberData *)(filp->f_priv); + return sd; + } - bool _IsRemoteSubscriberPresent; - int32_t _subscriber_count; + bool _IsRemoteSubscriberPresent; + int32_t _subscriber_count; - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); + /** + * 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); + /** + * 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); + /** + * 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); - // disable copy and assignment operators - DeviceNode( const DeviceNode& ); - DeviceNode& operator=( const DeviceNode& ); + // disable copy and assignment operators + DeviceNode(const DeviceNode &); + DeviceNode &operator=(const DeviceNode &); }; /** @@ -224,14 +225,14 @@ private: // private class methods. class uORB::DeviceMaster : public device::CDev { public: - DeviceMaster(Flavor f); - virtual ~DeviceMaster(); + DeviceMaster(Flavor f); + virtual ~DeviceMaster(); - static uORB::DeviceNode* GetDeviceNode( const char * node_name ); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + static uORB::DeviceNode *GetDeviceNode(const char *node_name); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: - Flavor _flavor; - static ORBMap _node_map; + Flavor _flavor; + static ORBMap _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 95a2374611..2fce0e8efb 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -43,504 +43,521 @@ #include "uORBCommunicator.hpp" #include -std::map uORB::DeviceMaster::_node_map; +std::map uORB::DeviceMaster::_node_map; uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp) { - uORB::DeviceNode::SubscriberData *sd; - if (filp) { - sd = (uORB::DeviceNode::SubscriberData *)(filp->priv); - } - else { - sd = 0; - } - return sd; + uORB::DeviceNode::SubscriberData *sd; + + if (filp) { + sd = (uORB::DeviceNode::SubscriberData *)(filp->priv); + + } else { + sd = 0; + } + + return sd; } uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : - VDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0), - _priority(priority), - _subscriber_count(0) + VDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority), + _subscriber_count(0) { - // enable debug() calls - //_debug_enabled = true; + // enable debug() calls + //_debug_enabled = true; } uORB::DeviceNode::~DeviceNode() { - if (_data != nullptr) - delete[] _data; + if (_data != nullptr) { + delete[] _data; + } } int uORB::DeviceNode::open(device::file_t *filp) { - int ret; + int ret; - /* is this a publisher? */ - if (filp->flags == PX4_F_WRONLY) { + /* is this a publisher? */ + if (filp->flags == PX4_F_WRONLY) { - /* become the publisher if we can */ - lock(); + /* become the publisher if we can */ + lock(); - if (_publisher == 0) { - _publisher = px4_getpid(); - ret = PX4_OK; + if (_publisher == 0) { + _publisher = px4_getpid(); + ret = PX4_OK; - } else { - ret = -EBUSY; - } + } else { + ret = -EBUSY; + } - unlock(); + unlock(); - /* now complete the open */ - if (ret == PX4_OK) { - ret = VDev::open(filp); + /* now complete the open */ + if (ret == PX4_OK) { + ret = VDev::open(filp); - /* open failed - not the publisher anymore */ - if (ret != PX4_OK) - _publisher = 0; - } + /* open failed - not the publisher anymore */ + if (ret != PX4_OK) { + _publisher = 0; + } + } - return ret; - } + return ret; + } - /* is this a new subscriber? */ - if (filp->flags == PX4_F_RDONLY) { + /* is this a new subscriber? */ + if (filp->flags == PX4_F_RDONLY) { - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; - if (nullptr == sd) - return -ENOMEM; + if (nullptr == sd) { + return -ENOMEM; + } - memset(sd, 0, sizeof(*sd)); + memset(sd, 0, sizeof(*sd)); - /* default to no pending update */ - sd->generation = _generation; + /* default to no pending update */ + sd->generation = _generation; - /* set priority */ - sd->priority = _priority; + /* set priority */ + sd->priority = _priority; - filp->priv = (void *)sd; + filp->priv = (void *)sd; - ret = VDev::open(filp); + ret = VDev::open(filp); - add_internal_subscriber(); + add_internal_subscriber(); - if (ret != PX4_OK) { - warnx("ERROR: VDev::open failed\n"); - delete sd; - } + if (ret != PX4_OK) { + warnx("ERROR: VDev::open failed\n"); + delete sd; + } - //warnx("uORB::DeviceNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); - return ret; - } + //warnx("uORB::DeviceNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); + return ret; + } - /* can only be pub or sub, not both */ - return -EINVAL; + /* can only be pub or sub, not both */ + return -EINVAL; } int uORB::DeviceNode::close(device::file_t *filp) { - //warnx("uORB::DeviceNode::close fd = %d", filp->fd); - /* is this the publisher closing? */ - if (px4_getpid() == _publisher) { - _publisher = 0; + //warnx("uORB::DeviceNode::close fd = %d", filp->fd); + /* is this the publisher closing? */ + if (px4_getpid() == _publisher) { + _publisher = 0; - } else { - SubscriberData *sd = filp_to_sd(filp); + } else { + SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) { - hrt_cancel(&sd->update_call); - remove_internal_subscriber(); - delete sd; - sd = nullptr; - } - } + if (sd != nullptr) { + hrt_cancel(&sd->update_call); + remove_internal_subscriber(); + delete sd; + sd = nullptr; + } + } - return VDev::close(filp); + return VDev::close(filp); } ssize_t uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen) { - //warnx("uORB::DeviceNode::read fd = %d\n", filp->fd); - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + //warnx("uORB::DeviceNode::read fd = %d\n", filp->fd); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; + /* 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; + /* 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 - */ - lock(); + /* + * Perform an atomic copy & state update + */ + lock(); - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) - memcpy(buffer, _data, _meta->o_size); + /* 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; + /* track the last generation that the file has seen */ + sd->generation = _generation; - /* set priority */ - sd->priority = _priority; + /* set priority */ + sd->priority = _priority; - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; - unlock(); + unlock(); - return _meta->o_size; + return _meta->o_size; } ssize_t uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen) { - //warnx("uORB::DeviceNode::write filp = %p (null is normal)", filp); - /* - * 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) { - lock(); + //warnx("uORB::DeviceNode::write filp = %p (null is normal)", filp); + /* + * 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) { + lock(); - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; + /* re-check size */ + if (nullptr == _data) { + _data = new uint8_t[_meta->o_size]; + } - unlock(); + unlock(); - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } + /* 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; + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) { + return -EIO; + } - /* Perform an atomic copy. */ - lock(); - memcpy(_data, buffer, _meta->o_size); - unlock(); + /* Perform an atomic copy. */ + lock(); + memcpy(_data, buffer, _meta->o_size); + unlock(); - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - _generation++; + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; - /* notify any poll waiters */ - poll_notify(POLLIN); + /* notify any poll waiters */ + poll_notify(POLLIN); - return _meta->o_size; + return _meta->o_size; } int uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - //warnx("uORB::DeviceNode::ioctl fd = %d cmd = %d", filp->fd, cmd); - SubscriberData *sd = filp_to_sd(filp); + //warnx("uORB::DeviceNode::ioctl fd = %d cmd = %d", filp->fd, cmd); + SubscriberData *sd = filp_to_sd(filp); - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return PX4_OK; + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return PX4_OK; - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return PX4_OK; + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return PX4_OK; - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return PX4_OK; + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return PX4_OK; - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return PX4_OK; + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return PX4_OK; - case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; - return PX4_OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return PX4_OK; - default: - /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); - } + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } } ssize_t -uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data ) +uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) { - //warnx("uORB::DeviceNode::publish meta = %p", meta); + //warnx("uORB::DeviceNode::publish meta = %p", meta); - if (handle == nullptr) { - warnx("uORB::DeviceNode::publish called with invalid handle"); - errno = EINVAL; - return ERROR; - } + if (handle == nullptr) { + warnx("uORB::DeviceNode::publish called with invalid handle"); + errno = EINVAL; + return ERROR; + } - uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; - int ret; + uORB::DeviceNode *devnode = (uORB::DeviceNode *)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; - } + /* 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); + /* 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 < 0) { + return ERROR; + } - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } - /* - * if the write is successful, send the data over the Multi-ORB link - */ - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( ch != nullptr ) - { - if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) - { - warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", - __LINE__, meta->o_name ); - return ERROR; - } - } - return PX4_OK; + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr) { + if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) { + warnx("[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name); + return ERROR; + } + } + + return PX4_OK; } pollevent_t uORB::DeviceNode::poll_state(device::file_t *filp) { - //warnx("uORB::DeviceNode::poll_state fd = %d", filp->fd); - SubscriberData *sd = filp_to_sd(filp); + //warnx("uORB::DeviceNode::poll_state fd = %d", filp->fd); + SubscriberData *sd = filp_to_sd(filp); - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) { + return POLLIN; + } - return 0; + return 0; } void uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - //warnx("uORB::DeviceNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); - SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); + //warnx("uORB::DeviceNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); + SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) - VDev::poll_notify_one(fds, events); + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) { + VDev::poll_notify_one(fds, events); + } } bool uORB::DeviceNode::appears_updated(SubscriberData *sd) { - //warnx("uORB::DeviceNode::appears_updated sd = %p", sd); - /* assume it doesn't look updated */ - bool ret = false; + //warnx("uORB::DeviceNode::appears_updated sd = %p", sd); + /* assume it doesn't look updated */ + bool ret = false; - /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) { - ret = false; - goto out; - } + /* 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) { + /* + * 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; - } + /* + * 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 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; + } // FIXME - the calls to hrt_called and hrt_call_after seem not to work in the // POSIX build #ifndef __PX4_POSIX - /* - * 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, - &uORB::DeviceNode::update_deferred_trampoline, - (void *)this); + /* + * 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, + &uORB::DeviceNode::update_deferred_trampoline, + (void *)this); #endif - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; - break; - } + break; + } out: - /* consider it updated */ - return ret; + /* consider it updated */ + return ret; } void uORB::DeviceNode::update_deferred() { - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); } void uORB::DeviceNode::update_deferred_trampoline(void *arg) { - uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + uORB::DeviceNode *node = (uORB::DeviceNode *)arg; - node->update_deferred(); + node->update_deferred(); } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void uORB::DeviceNode::add_internal_subscriber() { - _subscriber_count++; - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( ch != nullptr && _subscriber_count > 0 ) - { - ch->add_subscription( _meta->o_name, 1 ); - } + _subscriber_count++; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count > 0) { + ch->add_subscription(_meta->o_name, 1); + } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void uORB::DeviceNode::remove_internal_subscriber() { - _subscriber_count--; - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( ch != nullptr && _subscriber_count == 0 ) - { - ch->remove_subscription( _meta->o_name ); - } + _subscriber_count--; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count == 0) { + ch->remove_subscription(_meta->o_name); + } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz ) +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) { - // if there is already data in the node, send this out to - // the remote entity. - // send the data to the remote entity. - uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher. - { - ch->send_message( _meta->o_name, _meta->o_size, _data); - } + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); - return 0; + if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. + ch->send_message(_meta->o_name, _meta->o_size, _data); + } + + return 0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::DeviceNode::process_remove_subscription() { - return 0; + return 0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data) +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) { - int16_t ret = -1; - if( length != (int32_t)(_meta->o_size) ) - { - warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", - __LINE__, _meta->o_name, (int)length, (int)_meta->o_size ); - return ERROR; - } + int16_t ret = -1; - /* call the devnode write method with no file pointer */ - ret = write(nullptr, (const char *)data, _meta->o_size); + if (length != (int32_t)(_meta->o_size)) { + warnx("[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size); + return ERROR; + } - if (ret < 0) - return ERROR; + /* call the devnode write method with no file pointer */ + ret = write(nullptr, (const char *)data, _meta->o_size); - if (ret != (int)_meta->o_size) { - errno = EIO; - return ERROR; - } + if (ret < 0) { + return ERROR; + } - return PX4_OK; + if (ret != (int)_meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; } uORB::DeviceMaster::DeviceMaster(Flavor f) : - VDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _flavor(f) + VDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) { - // enable debug() calls - //_debug_enabled = true; + // enable debug() calls + //_debug_enabled = true; } @@ -551,114 +568,115 @@ uORB::DeviceMaster::~DeviceMaster() int uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - int ret; + int ret; - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - const char *objname; - const char *devpath; - char nodepath[orb_maxpath]; - uORB::DeviceNode *node; + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + uORB::DeviceNode *node; - /* set instance to zero - we could allow selective multi-pubs later based on value */ - if (adv->instance != nullptr) { - *(adv->instance) = 0; - } + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); - if (ret != PX4_OK) { - return ret; - } + if (ret != PX4_OK) { + return ret; + } - /* ensure that only one advertiser runs through this critical section */ - lock(); + /* ensure that only one advertiser runs through this critical section */ + lock(); - ret = ERROR; + ret = ERROR; - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } - if (objname == nullptr) { - return -ENOMEM; - } + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); - /* driver wants a permanent copy of the path, so make one here */ - devpath = strdup(nodepath); + if (objname == nullptr) { + return -ENOMEM; + } - if (devpath == nullptr) { - // FIXME - looks like we leaked memory here for objname - return -ENOMEM; - } + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); - /* construct the new node */ - node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); + if (devpath == nullptr) { + // FIXME - looks like we leaked memory here for objname + return -ENOMEM; + } - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - unlock(); + /* construct the new node */ + node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); - // FIXME - looks like we leaked memory here for devpath and objname - return -ENOMEM; - } + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); + // FIXME - looks like we leaked memory here for devpath and objname + return -ENOMEM; + } - /* if init failed, discard the node and its name */ - if (ret != PX4_OK) { - delete node; - free((void *)objname); - free((void *)devpath); - } - else - { - // add to the node map;. - _node_map[std::string(nodepath)] = node; - } + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != PX4_OK) { + delete node; + free((void *)objname); + free((void *)devpath); + + } else { + // add to the node map;. + _node_map[std::string(nodepath)] = node; + } - group_tries++; + group_tries++; - } while (ret != PX4_OK && (group_tries < max_group_tries)); + } while (ret != PX4_OK && (group_tries < max_group_tries)); - if (group_tries > max_group_tries) { - ret = -ENOMEM; - } + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } - /* the file handle for the driver has been created, unlock */ - unlock(); + /* the file handle for the driver has been created, unlock */ + unlock(); - return ret; - } + return ret; + } - default: - /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); - } + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } } -uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) +uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath) { - uORB::DeviceNode* rc = nullptr; - std::string np(nodepath); - if( _node_map.find( np ) != _node_map.end() ) - { - rc = _node_map[np]; - } - return rc; + uORB::DeviceNode *rc = nullptr; + std::string np(nodepath); + + if (_node_map.find(np) != _node_map.end()) { + rc = _node_map[np]; + } + + return rc; } diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 044622c4af..1ac0897381 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -41,110 +41,110 @@ namespace uORB { - class DeviceNode; - class DeviceMaster; +class DeviceNode; +class DeviceMaster; } class uORB::DeviceNode : public device::VDev { public: - DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); - ~DeviceNode(); + DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); + ~DeviceNode(); - virtual int open(device::file_t *filp); - virtual int close(device::file_t *filp); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual int open(device::file_t *filp); + virtual int close(device::file_t *filp); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data ); + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); - /** - * processes a request for add subscription from remote - * @param rateInHz - * Specifies the desired rate for the message. - * @return - * 0 = success - * otherwise failure. - */ - int16_t process_add_subscription( int32_t rateInHz ); + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription(int32_t rateInHz); - /** - * processes a request to remove a subscription from remote. - */ - int16_t process_remove_subscription(); + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); - /** - * processed the received data message from remote. - */ - int16_t process_received_message( int32_t length, uint8_t* data ); + /** + * processed the received data message from remote. + */ + int16_t process_received_message(int32_t length, uint8_t *data); - /** - * Add the subscriber to the node's list of subscriber. If there is - * remote proxy to which this subscription needs to be sent, it will - * done via uORBCommunicator::IChannel interface. - * @param sd - * the subscriber to be added. - */ - void add_internal_subscriber(); + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); - /** - * Removes the subscriber from the list. Also notifies the remote - * if there a uORBCommunicator::IChannel instance. - * @param sd - * the Subscriber to be removed. - */ - void remove_internal_subscriber(); + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); protected: - virtual pollevent_t poll_state(device::file_t *filp); - virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + virtual pollevent_t poll_state(device::file_t *filp); + virtual void poll_notify_one(px4_pollfd_struct_t *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 */ - int priority; /**< priority of publisher */ - }; + 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 */ + int priority; /**< priority of publisher */ + }; - 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 */ - unsigned long _publisher; /**< if nonzero, current publisher */ - const int _priority; /**< priority of topic */ + 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 */ + unsigned long _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ - SubscriberData *filp_to_sd(device::file_t *filp); + SubscriberData *filp_to_sd(device::file_t *filp); - int32_t _subscriber_count; + int32_t _subscriber_count; - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); + /** + * 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); + /** + * 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); + /** + * 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); - // disable copy and assignment operators - DeviceNode( const DeviceNode& ); - DeviceNode& operator=( const DeviceNode& ); + // disable copy and assignment operators + DeviceNode(const DeviceNode &); + DeviceNode &operator=(const DeviceNode &); }; /** @@ -156,15 +156,15 @@ private: class uORB::DeviceMaster : public device::VDev { public: - DeviceMaster(Flavor f); - ~DeviceMaster(); + DeviceMaster(Flavor f); + ~DeviceMaster(); - static uORB::DeviceNode* GetDeviceNode( const char *node_name ); + static uORB::DeviceNode *GetDeviceNode(const char *node_name); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: - Flavor _flavor; - static std::map _node_map; + Flavor _flavor; + static std::map _node_map; }; #endif /* _uORBDeviceNode_posix.hpp */