int px4_open(const char *path, int flags, ...) { PX4_DEBUG("px4_open"); VDev *dev = VDev::getDev(path); int ret = 0; int i; mode_t mode; if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 && strncmp(path, "/obj/", 5) != 0 && strncmp(path, "/dev/", 5) != 0) { va_list p; va_start(p, flags); mode = va_arg(p, mode_t); va_end(p); // Create the file PX4_DEBUG("Creating virtual file %s", path); dev = VFile::createFile(path, mode); } if (dev) { pthread_mutex_lock(&filemutex); for (i = 0; i < PX4_MAX_FD; ++i) { if (filemap[i] == 0) { filemap[i] = new device::file_t(flags, dev, i); break; } } pthread_mutex_unlock(&filemutex); if (i < PX4_MAX_FD) { ret = dev->open(filemap[i]); } else { PX4_WARN("exceeded maximum number of file descriptors!"); ret = -ENOENT; } } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; return -1; } PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); return filemap[i]->fd; }
int px4_ioctl(int fd, int cmd, unsigned long arg) { PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); ret = dev->ioctl(filemap[fd], cmd, arg); } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; } return (ret == 0) ? PX4_OK : PX4_ERROR; }
ssize_t px4_write(int fd, const void *buffer, size_t buflen) { int ret; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_write fd = %d", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; ret = PX4_ERROR; } return ret; }
int px4_close(int fd) { int ret; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_close fd = %d", fd); ret = dev->close(filemap[fd]); filemap[fd] = NULL; } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; ret = PX4_ERROR; } return ret; }
int px4_ioctl(int fd, int cmd, unsigned long arg) { PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; VDev *dev = get_vdev(fd); if (dev) { ret = dev->ioctl(filemap[fd], cmd, arg); } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; } return ret; }
ssize_t px4_read(int fd, void *buffer, size_t buflen) { int ret; VDev *dev = get_vdev(fd); if (dev) { PX4_DEBUG("px4_read fd = %d", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; ret = PX4_ERROR; } return ret; }
int px4_close(int fd) { int ret; VDev *dev = get_vdev(fd); if (dev) { pthread_mutex_lock(&filemutex); ret = dev->close(filemap[fd]); filemap[fd] = nullptr; pthread_mutex_unlock(&filemutex); PX4_DEBUG("px4_close fd = %d", fd); } else { ret = -EINVAL; } if (ret < 0) { px4_errno = -ret; ret = PX4_ERROR; } return ret; }
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { sem_t sem; int count = 0; int ret; unsigned int i; struct timespec ts; PX4_DEBUG("Called px4_poll timeout = %d", timeout); sem_init(&sem, 0, 0); // For each fd for (i=0; i<nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; fds[i].priv = NULL; // If fd is valid if (valid_fd(fds[i].fd)) { VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) break; } } if (ret >= 0) { if (timeout >= 0) { pthread_t pt; void *res; ts.tv_sec = timeout/1000; ts.tv_nsec = (timeout % 1000)*1000000; // Create a timer to unblock struct timerData td(sem, ts); int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td); if (rv != 0) { count = -1; goto cleanup; } sem_wait(&sem); // Make sure timer thread is killed before sem goes // out of scope (void)pthread_cancel(pt); (void)pthread_join(pt, &res); } else { sem_wait(&sem); } // For each fd for (i=0; i<nfds; ++i) { // If fd is valid if (valid_fd(fds[i].fd)) { VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) break; if (fds[i].revents) count += 1; } } } cleanup: sem_destroy(&sem); return count; }
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { sem_t sem; int count = 0; int ret; unsigned int i; PX4_DEBUG("Called px4_poll timeout = %d", timeout); sem_init(&sem, 0, 0); // For each fd for (i=0; i<nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; fds[i].priv = NULL; // If fd is valid if (valid_fd(fds[i].fd)) { VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) break; } } if (ret >= 0) { if (timeout >= 0) { // Use a work queue task work_s _hpwork; hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout); sem_wait(&sem); // Make sure timer thread is killed before sem goes // out of scope hrt_work_cancel(&_hpwork); } else { sem_wait(&sem); } // For each fd for (i=0; i<nfds; ++i) { // If fd is valid if (valid_fd(fds[i].fd)) { VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) break; if (fds[i].revents) count += 1; } } } sem_destroy(&sem); return count; }
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { if (nfds == 0) { PX4_WARN("px4_poll with no fds"); return -1; } px4_sem_t sem; int count = 0; int ret = -1; unsigned int i; const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); if (nret || thread_name[0] == 0) { PX4_WARN("failed getting thread name"); } PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); // Go through all fds and check them for a pollable state bool fd_pollable = false; for (i = 0; i < nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; fds[i].priv = NULL; VDev *dev = get_vdev(fds[i].fd); // If fd is valid if (dev) { PX4_DEBUG("%s: px4_poll: VDev->poll(setup) %d", thread_name, fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) { PX4_WARN("%s: px4_poll() error: %s", thread_name, strerror(errno)); break; } if (ret >= 0) { fd_pollable = true; } } } // If any FD can be polled, lock the semaphore and // check for new data if (fd_pollable) { if (timeout > 0) { // Get the current time struct timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); // Calculate an absolute time in the future const unsigned billion = (1000 * 1000 * 1000); unsigned tdiff = timeout; uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000); ts.tv_sec += nsecs / billion; nsecs -= (nsecs / billion) * billion; ts.tv_nsec = nsecs; // Execute a blocking wait for that time in the future errno = 0; ret = px4_sem_timedwait(&sem, &ts); #ifndef __PX4_DARWIN ret = errno; #endif // Ensure ret is negative on failure if (ret > 0) { ret = -ret; } if (ret && ret != -ETIMEDOUT) { PX4_WARN("%s: px4_poll() sem error", thread_name); } } else if (timeout < 0) { px4_sem_wait(&sem); } // We have waited now (or not, depending on timeout), // go through all fds and count how many have data for (i = 0; i < nfds; ++i) { VDev *dev = get_vdev(fds[i].fd); // If fd is valid if (dev) { PX4_DEBUG("%s: px4_poll: VDev->poll(teardown) %d", thread_name, fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) { PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name); break; } if (fds[i].revents) { count += 1; } } } } px4_sem_destroy(&sem); // Return the positive count if present, // return the negative error number if failed return (count) ? count : ret; }