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;
}
void
ORBDevNode::update_deferred_trampoline(void *arg)
{
ORBDevNode *node = (ORBDevNode *)arg;
node->update_deferred();
}
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);
}
}
int
ORBDevMaster::ioctl(struct file *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];
ORBDevNode *node;
/* 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 = node_mkpath(nodepath, _flavor, meta, adv->instance);
if (ret != OK) {
return ret;
}
/* ensure that only one advertiser runs through this critical section */
lock();
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;
}
/* driver wants a permanent copy of the node name, so make one here */
objname = strdup(meta->o_name);
if (objname == nullptr) {
return -ENOMEM;
}
/* 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 ORBDevNode(meta, objname, devpath, adv->priority);
/* if we didn't get a device, that's bad */
if (node == nullptr) {
unlock();
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 != OK) {
delete node;
free((void *)objname);
free((void *)devpath);
}
group_tries++;
} while (ret != OK && (group_tries < max_group_tries));
if (group_tries > max_group_tries) {
ret = -ENOMEM;
}
/* 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);
}
}
