int uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) { 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; /* 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; } ret = ERROR; /* try for topic groups */ const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; unsigned group_tries = 0; if (adv->instance) { /* for an advertiser, this will be 0, but a for subscriber that requests a certain instance, * we do not want to start with 0, but with the instance the subscriber actually requests. */ group_tries = *adv->instance; if (group_tries >= max_group_tries) { return -ENOMEM; } } SmartLock smart_lock(_lock); 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; } objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed /* driver wants a permanent copy of the path, so make one here */ devpath = strdup(nodepath); if (devpath == nullptr) { return -ENOMEM; } /* construct the new node */ node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); /* if we didn't get a device, that's bad */ if (node == nullptr) { free((void *)devpath); return -ENOMEM; } /* 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; if (ret == -EEXIST) { /* if the node exists already, get the existing one and check if * something has been published yet. */ uORB::DeviceNode *existing_node = GetDeviceNode(devpath); if ((existing_node != nullptr) && !(existing_node->is_published())) { /* nothing has been published yet, lets claim it */ ret = PX4_OK; } else { /* otherwise: data has already been published, keep looking */ } } /* also discard the name now */ free((void *)devpath); } else { // add to the node map;. _node_map[std::string(nodepath)] = node; } group_tries++; } while (ret != PX4_OK && (group_tries < max_group_tries)); if (ret != PX4_OK && group_tries >= max_group_tries) { ret = -ENOMEM; } return ret; } default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); } }
int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority) { int ret = PX4_ERROR; char nodepath[orb_maxpath]; /* construct a path to the node - this also checks the node name */ ret = uORB::Utils::node_mkpath(nodepath, meta, instance); if (ret != PX4_OK) { return ret; } ret = PX4_ERROR; /* try for topic groups */ const unsigned max_group_tries = (instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; unsigned group_tries = 0; if (instance) { /* for an advertiser, this will be 0, but a for subscriber that requests a certain instance, * we do not want to start with 0, but with the instance the subscriber actually requests. */ group_tries = *instance; if (group_tries >= max_group_tries) { return -ENOMEM; } } SmartLock smart_lock(_lock); do { /* if path is modifyable change try index */ if (instance != nullptr) { /* replace the number at the end of the string */ nodepath[strlen(nodepath) - 1] = '0' + group_tries; *instance = group_tries; } /* driver wants a permanent copy of the path, so make one here */ const char *devpath = strdup(nodepath); if (devpath == nullptr) { return -ENOMEM; } /* construct the new node */ uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority); /* if we didn't get a device, that's bad */ if (node == nullptr) { free((void *)devpath); return -ENOMEM; } /* 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; if (ret == -EEXIST) { /* if the node exists already, get the existing one and check if * something has been published yet. */ uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries); if ((existing_node != nullptr) && !(existing_node->is_published())) { /* nothing has been published yet, lets claim it */ existing_node->set_priority(priority); ret = PX4_OK; } else { /* otherwise: data has already been published, keep looking */ } } /* also discard the name now */ free((void *)devpath); } else { // add to the node map;. _node_list.add(node); } group_tries++; } while (ret != PX4_OK && (group_tries < max_group_tries)); if (ret != PX4_OK && group_tries >= max_group_tries) { ret = -ENOMEM; } return ret; }