ssize_t
uORB::DeviceNode::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 = px4_enter_critical_section();
if (_generation > sd->generation + _queue_size) {
/* Reader is too far behind: some messages are lost */
_lost_messages += _generation - (sd->generation + _queue_size);
sd->generation = _generation - _queue_size;
}
if (_generation == sd->generation && sd->generation > 0) {
/* The subscriber already read the latest message, but nothing new was published yet.
* Return the previous message
*/
--sd->generation;
}
/* if the caller doesn't want the data, don't give it to them */
if (nullptr != buffer) {
memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
}
if (sd->generation < _generation) {
++sd->generation;
}
/* set priority */
sd->set_priority(_priority);
/*
* Clear the flag that indicates that an update has been reported, as
* we have just collected it.
*/
sd->set_update_reported(false);
px4_leave_critical_section(flags);
return _meta->o_size;
}
int
uORB::DeviceNode::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;
/* set priority */
sd->set_priority(_priority);
filp->f_priv = (void *)sd;
ret = CDev::open(filp);
add_internal_subscriber();
if (ret != OK) {
delete sd;
}
return ret;
}
/* can only be pub or sub, not both */
return -EINVAL;
}
int
uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
{
SubscriberData *sd = filp_to_sd(filp);
switch (cmd) {
case ORBIOCLASTUPDATE: {
irqstate_t state = px4_enter_critical_section();
*(hrt_abstime *)arg = _last_update;
px4_leave_critical_section(state);
return OK;
}
case ORBIOCUPDATED:
*(bool *)arg = appears_updated(sd);
return OK;
case ORBIOCSETINTERVAL: {
int ret = PX4_OK;
lock();
if (arg == 0) {
if (sd->update_interval) {
delete(sd->update_interval);
sd->update_interval = nullptr;
}
} else {
if (sd->update_interval) {
sd->update_interval->interval = arg;
} else {
sd->update_interval = new UpdateIntervalData();
if (sd->update_interval) {
memset(&sd->update_interval->update_call, 0, sizeof(hrt_call));
sd->update_interval->interval = arg;
} else {
ret = -ENOMEM;
}
}
}
unlock();
return ret;
}
case ORBIOCGADVERTISER:
*(uintptr_t *)arg = (uintptr_t)this;
return OK;
case ORBIOCGPRIORITY:
*(int *)arg = sd->priority();
return OK;
case ORBIOCSETQUEUESIZE:
//no need for locking here, since this is used only during the advertisement call,
//and only one advertiser is allowed to open the DeviceNode at the same time.
return update_queue_size(arg);
case ORBIOCGETINTERVAL:
if (sd->update_interval) {
*(unsigned *)arg = sd->update_interval->interval;
} else {
*(unsigned *)arg = 0;
}
return OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
int
uORB::DeviceNode::open(cdev::file_t *filp)
{
int ret;
/* is this a publisher? */
if (FILE_FLAGS(filp) == PX4_F_WRONLY) {
/* become the publisher if we can */
lock();
if (_publisher == 0) {
_publisher = px4_getpid();
ret = PX4_OK;
} else {
ret = -EBUSY;
}
unlock();
/* now complete the open */
if (ret == PX4_OK) {
ret = CDev::open(filp);
/* open failed - not the publisher anymore */
if (ret != PX4_OK) {
_publisher = 0;
}
}
return ret;
}
/* is this a new subscriber? */
if (FILE_FLAGS(filp) == PX4_F_RDONLY) {
/* allocate subscriber data */
SubscriberData *sd = new SubscriberData{};
if (nullptr == sd) {
return -ENOMEM;
}
/* If there were any previous publications, allow the subscriber to read them */
sd->generation = _generation - (_queue_size < _generation ? _queue_size : _generation);
/* set priority */
sd->set_priority(_priority);
FILE_PRIV(filp) = (void *)sd;
ret = CDev::open(filp);
add_internal_subscriber();
if (ret != PX4_OK) {
PX4_ERR("CDev::open failed");
delete sd;
}
return ret;
}
if (FILE_FLAGS(filp) == 0) {
return CDev::open(filp);
}
/* can only be pub or sub, not both */
return -EINVAL;
}
int
uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
SubscriberData *sd = filp_to_sd(filp);
switch (cmd) {
case ORBIOCLASTUPDATE: {
ATOMIC_ENTER;
*(hrt_abstime *)arg = _last_update;
ATOMIC_LEAVE;
return PX4_OK;
}
case ORBIOCUPDATED:
#ifndef __PX4_NUTTX
lock();
#endif
*(bool *)arg = appears_updated(sd);
#ifndef __PX4_NUTTX
unlock();
#endif
return PX4_OK;
case ORBIOCSETINTERVAL: {
int ret = PX4_OK;
lock();
if (arg == 0) {
if (sd->update_interval) {
delete (sd->update_interval);
sd->update_interval = nullptr;
}
} else {
if (sd->update_interval) {
sd->update_interval->interval = arg;
#ifndef __PX4_NUTTX
sd->update_interval->last_update = hrt_absolute_time();
#endif
} else {
sd->update_interval = new UpdateIntervalData();
if (sd->update_interval) {
memset(&sd->update_interval->update_call, 0, sizeof(hrt_call));
sd->update_interval->interval = arg;
#ifndef __PX4_NUTTX
sd->update_interval->last_update = hrt_absolute_time();
#endif
} else {
ret = -ENOMEM;
}
}
}
unlock();
return ret;
}
case ORBIOCGADVERTISER:
*(uintptr_t *)arg = (uintptr_t)this;
return PX4_OK;
case ORBIOCGPRIORITY:
*(int *)arg = sd->priority();
return PX4_OK;
case ORBIOCSETQUEUESIZE:
//no need for locking here, since this is used only during the advertisement call,
//and only one advertiser is allowed to open the DeviceNode at the same time.
return update_queue_size(arg);
case ORBIOCGETINTERVAL:
if (sd->update_interval) {
*(unsigned *)arg = sd->update_interval->interval;
} else {
*(unsigned *)arg = 0;
}
return OK;
case ORBIOCISPUBLISHED:
*(unsigned long *)arg = _published;
return OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
