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) { 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) { 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; }