From 9718404d7f61e501e720f83cc3c6163a443addd9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 11:49:32 -0700 Subject: [PATCH] Added new file for virt dev posix-like functions and test case Moved posix-like functions to vcdev_posix.cpp and updated the copyright notice. Added test case to make sure poll unblocks when a write occurs. Signed-off-by: Mark Charlebois --- src/drivers/device/module.mk | 3 +- src/drivers/device/vcdev.cpp | 213 ++----------- src/drivers/device/vcdev_posix.cpp | 281 ++++++++++++++++++ src/drivers/device/vdevice.cpp | 2 +- src/drivers/device/vdevice.h | 6 +- .../linux/vcdev_test/vcdevtest_example.cpp | 45 ++- .../vcdev_test/vcdevtest_start_linux.cpp | 5 - src/platforms/px4_tasks.h | 6 + 8 files changed, 355 insertions(+), 206 deletions(-) create mode 100644 src/drivers/device/vcdev_posix.cpp diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 522639dc17..5475e01e41 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -45,5 +45,6 @@ SRCS = \ endif ifeq ($(PX4_TARGET_OS),linux) SRCS = vcdev.cpp \ - vdevice.cpp + vdevice.cpp \ + vcdev_posix.cpp endif diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vcdev.cpp index 174094547c..edebe3b416 100644 --- a/src/drivers/device/vcdev.cpp +++ b/src/drivers/device/vcdev.cpp @@ -32,13 +32,11 @@ ****************************************************************************/ /** - * @file cdev.cpp + * @file vcdev.cpp * - * Character device base class. + * Virtual character device base class. */ -//#include "px4_platform.h" -//#include "px4_device.h" #include "px4_posix.h" #include "device.h" #include "drivers/drv_device.h" @@ -138,7 +136,7 @@ CDev::register_driver(const char *name, void *data) for (int i=0;iname) == 0)) { delete devmap[i]; devmap[i] = NULL; - log("Unregistered DEV %s", name); + debug("Unregistered DEV %s", name); ret = PX4_OK; break; } @@ -325,7 +323,7 @@ int CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) { int ret = PX4_OK; - log("CDev::Poll %s", setup ? "setup" : "teardown"); + debug("CDev::Poll %s", setup ? "setup" : "teardown"); /* * Lock against pollnotify() (and possibly other callers) @@ -338,7 +336,7 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * benefit. */ fds->priv = (void *)handlep; - log("CDev::poll: fds->priv = %p", handlep); + debug("CDev::poll: fds->priv = %p", handlep); /* * Handle setup requests. @@ -373,30 +371,33 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) void CDev::poll_notify(pollevent_t events) { - log("CDev::poll_notify"); + debug("CDev::poll_notify events = %0x", events); /* lock against poll() as well as other wakeups */ lock(); - //irqstate_t state = irqsave(); for (unsigned i = 0; i < _max_pollwaiters; i++) if (nullptr != _pollset[i]) poll_notify_one(_pollset[i], events); + for (unsigned i = 0; i < _max_pollwaiters; i++) + if (nullptr != _pollset[i]) + debug(" CHECK fds=%p %0x %0x",_pollset[i], _pollset[i]->revents, _pollset[i]->events); unlock(); - //irqrestore(state); } void CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - log("CDev::poll_notify_one"); + debug("CDev::poll_notify_one"); int value; sem_getvalue(fds->sem, &value); /* update the reported event set */ fds->revents |= fds->events & events; + debug(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); + /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ if ((fds->revents != 0) && (value <= 0)) @@ -406,6 +407,7 @@ CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) pollevent_t CDev::poll_state(px4_dev_handle_t *handlep) { + debug("CDev::poll_notify"); /* by default, no poll events to report */ return 0; } @@ -416,6 +418,7 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds) /* * Look for a free slot. */ + debug("CDev::store_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -432,6 +435,7 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds) int CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { + debug("CDev::remove_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -445,7 +449,7 @@ CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) return -EINVAL; } -static CDev *get_dev(const char *path) +CDev *CDev::getDev(const char *path) { int i=0; for (; iopen(device::filemap[i]); - } - else { - ret = -ENOENT; - } - } - if (ret < 0) { - px4_errno = -ret; - return -1; - } - printf("px4_open fd = %d\n", device::filemap[i]->fd); - return device::filemap[i]->fd; -} - -int -px4_close(int fd) -{ - int ret; - printf("px4_close fd = %d\n", fd); - if (fd < PX4_MAX_FD && fd >= 0) { - ret = ((device::CDev *)(device::filemap[fd]->cdev))->close(device::filemap[fd]); - device::filemap[fd] = NULL; - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - return ret; -} - -ssize_t -px4_read(int fd, void *buffer, size_t buflen) -{ - int ret; - printf("px4_read fd = %d\n", fd); - if (fd < PX4_MAX_FD && fd >= 0) - ret = ((device::CDev *)(device::filemap[fd]->cdev))->read(device::filemap[fd], (char *)buffer, buflen); - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - return ret; -} - -ssize_t -px4_write(int fd, const void *buffer, size_t buflen) -{ - int ret = PX4_ERROR; - printf("px4_write fd = %d\n", fd); - if (fd < PX4_MAX_FD && fd >= 0) - ret = ((device::CDev *)(device::filemap[fd]->cdev))->write(device::filemap[fd], (const char *)buffer, buflen); - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - return ret; -} - -int -px4_ioctl(int fd, int cmd, unsigned long arg) -{ - int ret = PX4_ERROR; - if (fd < PX4_MAX_FD && fd >= 0) - ret = ((device::CDev *)(device::filemap[fd]->cdev))->ioctl(device::filemap[fd], cmd, arg); - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - else { - px4_errno = -EINVAL; - } - 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; - - printf("Called px4_poll %d\n", timeout); - sem_init(&sem, 0, 0); - - // For each fd - for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) - { - - // TODO - get waiting FD events - - printf("Called setup CDev->poll %d\n", fds[i].fd); - ret = ((device::CDev *)(device::filemap[fds[i].fd]->cdev))->poll(device::filemap[fds[i].fd], &fds[i], true); - - if (ret < 0) - break; - } - } - - if (ret >= 0) - { - if (timeout >= 0) - { - ts.tv_sec = timeout/1000; - ts.tv_nsec = (timeout % 1000)*1000; - //log("sem_wait for %d\n", timeout); - sem_timedwait(&sem, &ts); - //log("sem_wait finished\n"); - } - else - { - printf("Blocking sem_wait\n"); - sem_wait(&sem); - printf("Blocking sem_wait finished\n"); - } - - // For each fd - for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) - { - printf("Called CDev->poll %d\n", fds[i].fd); - ret = ((device::CDev *)(device::filemap[fds[i].fd]->cdev))->poll(device::filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) - break; - } - } - } - sem_destroy(&sem); - - return count; -} - -} - diff --git a/src/drivers/device/vcdev_posix.cpp b/src/drivers/device/vcdev_posix.cpp new file mode 100644 index 0000000000..6df88a2f66 --- /dev/null +++ b/src/drivers/device/vcdev_posix.cpp @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vcdev_posix.cpp + * + * POSIX-like API for virtual character device + */ + +#include "px4_posix.h" +#include "device.h" + +#include +#include +#include +#include +#include + +#define DEBUG(...) +//#define DEBUG(...) printf(__VA_ARGS__) + +using namespace device; + +extern "C" { + +struct timerData { + sem_t &sem; + struct timespec &ts; + + timerData(sem_t &s, struct timespec &t) : sem(s), ts(t) {} + ~timerData() {} +}; + +static void *timer_handler(void *data) +{ + struct timerData *td = (struct timerData *)data; + + if (td->ts.tv_sec) { + sleep(td->ts.tv_sec); + } + usleep(td->ts.tv_nsec/1000); + sem_post(&(td->sem)); + + printf("Timer expired\n"); + return 0; +} + +#define PX4_MAX_FD 100 +static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; + +int px4_errno; + +int +px4_open(const char *path, int flags) +{ + CDev *dev = CDev::getDev(path); + int ret = 0; + int i; + + if (!dev) { + ret = -EINVAL; + } + else { + for (i=0; iopen(filemap[i]); + } + else { + ret = -ENOENT; + } + } + if (ret < 0) { + px4_errno = -ret; + return -1; + } + DEBUG("px4_open fd = %d", filemap[i]->fd); + return filemap[i]->fd; +} + +int +px4_close(int fd) +{ + int ret; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_close fd = %d\n", fd); + ret = dev->close(filemap[fd]); + filemap[fd] = NULL; + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +ssize_t +px4_read(int fd, void *buffer, size_t buflen) +{ + int ret; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_read fd = %d\n", fd); + ret = dev->read(filemap[fd], (char *)buffer, buflen); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +ssize_t +px4_write(int fd, const void *buffer, size_t buflen) +{ + int ret = PX4_ERROR; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_write fd = %d\n", fd); + ret = dev->write(filemap[fd], (const char *)buffer, buflen); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +int +px4_ioctl(int fd, int cmd, unsigned long arg) +{ + int ret = PX4_ERROR; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_ioctl fd = %d\n", fd); + ret = dev->ioctl(filemap[fd], cmd, arg); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + else { + px4_errno = -EINVAL; + } + 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; + + printf("Called px4_poll timeout = %d\n", timeout); + sem_init(&sem, 0, 0); + + // For each fd + for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) + { + CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; + DEBUG("px4_poll: CDev->poll(setup) %d\n", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) + break; + DEBUG("xxxx fds=%p fds[%d].revents = %d\n", fds, i, fds[i].revents); + } + } + + if (ret >= 0) + { + if (timeout >= 0) + { + pthread_t pt; + void *res; + + ts.tv_sec = timeout/1000; + ts.tv_nsec = (timeout % 1000)*1000; + + // 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= 0 && fds[i].fd < PX4_MAX_FD) + { + DEBUG("zzzz fds=%p fds[%d].revents = %d\n",fds, i, fds[i].revents); + + CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; + DEBUG("px4_poll: CDev->poll(teardown) %d\n", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) + break; + + DEBUG("yyyy fds=%p fds[%d].revents = %d\n", fds, i, fds[i].revents); + if (fds[i].revents) + count += 1; + } + } + } + +cleanup: + sem_destroy(&sem); + + return count; +} + +} + diff --git a/src/drivers/device/vdevice.cpp b/src/drivers/device/vdevice.cpp index 7098339649..487249aed5 100644 --- a/src/drivers/device/vdevice.cpp +++ b/src/drivers/device/vdevice.cpp @@ -50,7 +50,7 @@ Device::Device(const char *name) : // public // protected _name(name), - _debug_enabled(true) + _debug_enabled(false) { sem_init(&_lock, 0, 1); diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdevice.h index b9d742118e..f35ac9fc19 100644 --- a/src/drivers/device/vdevice.h +++ b/src/drivers/device/vdevice.h @@ -146,7 +146,7 @@ protected: * Note that we must loop as the wait may be interrupted by a signal. */ void lock() { - log("lock"); + debug("lock"); do {} while (sem_wait(&_lock) != 0); } @@ -154,7 +154,7 @@ protected: * Release the driver lock. */ void unlock() { - log("unlock"); + debug("unlock"); sem_post(&_lock); } @@ -300,6 +300,8 @@ public: */ virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + static CDev *getDev(const char *path); + protected: int register_driver(const char *name, void *data); diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp index dadd9a2913..5650b4640f 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp @@ -39,6 +39,7 @@ * @author Mark Charlebois */ +#include #include "vcdevtest_example.h" #include "drivers/device/device.h" #include @@ -50,13 +51,45 @@ using namespace device; #define TESTDEV "/dev/vdevex" +int writer_main(int argc, char *argv[]) +{ + char buf[1] = { '1' }; + + int fd = px4_open(TESTDEV, PX4_F_RDONLY); + if (fd < 0) { + printf("--- Open failed %d %d", fd, px4_errno); + return -px4_errno; + } + + // Wait for 3 seconds + printf("--- Sleeping for 3 sec\n"); + int ret = sleep(3); + if (ret < 0) { + printf("--- usleep failed %d %d\n", ret, errno); + return ret; + } + + printf("--- writing to fd\n"); + return px4_write(fd, buf, 1); +} + class VCDevNode : public CDev { public: VCDevNode() : CDev("vcdevtest", TESTDEV) {}; ~VCDevNode() {} + + virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); }; +ssize_t VCDevNode::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +{ + // ignore what was written, but let pollers know something was written + poll_notify(POLLIN); + + return buflen; +} + VCDevExample::~VCDevExample() { if (_node) { delete _node; @@ -123,6 +156,14 @@ int VCDevExample::main() int i=0; px4_pollfd_struct_t fds[1]; + // Start a task that will write something in 3 seconds + (void)px4_task_spawn_cmd("writer", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 2000, + writer_main, + (char* const*)NULL); + while (!mgr.exitRequested() && i<5) { sleep(2); @@ -131,7 +172,9 @@ int VCDevExample::main() fds[0].events = POLLIN; fds[0].revents = 0; - ret = px4_poll(fds, 1, 500); + printf(" Calling Poll\n"); + ret = px4_poll(fds, 1, 1000); + printf(" Done poll\n"); if (ret < 0) { printf("poll failed %d %d\n", ret, px4_errno); px4_close(fd); diff --git a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp index 2f37fb05de..c006136114 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp @@ -42,11 +42,6 @@ #include #include #include -#include - -#define SCHED_DEFAULT SCHED_FIFO -#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) static int daemon_task; /* Handle of deamon task / thread */ diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index ac75126c44..18a6ced79e 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -48,6 +48,12 @@ typedef int px4_task_t; #elif defined(__PX4_LINUX) #include +#include + +#define SCHED_DEFAULT SCHED_FIFO +#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) + typedef pthread_t px4_task_t; typedef struct {