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;
}
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);
}
}
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);
switch (cmd) {
case ORBIOCLASTUPDATE:
*(hrt_abstime *)arg = _last_update;
return PX4_OK;
case ORBIOCUPDATED:
*(bool *)arg = appears_updated(sd);
return PX4_OK;
case ORBIOCSETINTERVAL:
sd->update_interval = arg;
return PX4_OK;
case ORBIOCGADVERTISER:
*(uintptr_t *)arg = (uintptr_t)this;
return PX4_OK;
case ORBIOCGPRIORITY:
*(int *)arg = sd->priority;
return PX4_OK;
default:
/* give it to the superclass */
return VDev::ioctl(filp, cmd, arg);
}
}
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);
/* 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
*/
lock();
if (_generation > sd->generation + _queue_size) {
/* Reader is too far behind: some messages are lost */
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->priority = _priority;
/*
* Clear the flag that indicates that an update has been reported, as
* we have just collected it.
*/
sd->update_reported = false;
unlock();
return _meta->o_size;
}
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);
}
void
uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
{
SubscriberData *sd = filp_to_sd((cdev::file_t *)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);
}
}
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;
}
pollevent_t
uORB::DeviceNode::poll_state(cdev::file_t *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;
}
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);
switch (cmd) {
case ORBIOCLASTUPDATE:
lock();
*(hrt_abstime *)arg = _last_update;
unlock();
return PX4_OK;
case ORBIOCUPDATED:
lock();
*(bool *)arg = appears_updated(sd);
unlock();
return PX4_OK;
case ORBIOCSETINTERVAL:
lock();
sd->update_interval = arg;
sd->last_update = hrt_absolute_time();
unlock();
return PX4_OK;
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:
*(unsigned *)arg = sd->update_interval;
return OK;
default:
/* give it to the superclass */
return VDev::ioctl(filp, cmd, arg);
}
}
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) {
hrt_cancel(&sd->update_call);
delete sd;
}
}
return CDev::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);
/* 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
*/
lock();
/* 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;
/* 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;
unlock();
return _meta->o_size;
}
int
uORB::DeviceNode::close(struct file *filp)
{
/* is this the publisher closing? */
if (getpid() == _publisher) {
_publisher = 0;
} else {
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
remove_internal_subscriber();
delete sd;
sd = nullptr;
}
}
return CDev::close(filp);
}
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;
} else {
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
remove_internal_subscriber();
delete sd;
sd = nullptr;
}
}
return VDev::close(filp);
}
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::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);
}
}
