From ccd18929fc7a9d573c0a439ee9545a232efeaa66 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 15:06:15 -0700 Subject: Linux: changed CDev to VDev for virtual device implementation To avoid confusion when a real device and a virtual device is being used, changed CDev to VDev for Linux. Signed-off-by: Mark Charlebois --- src/drivers/device/device.h | 2 +- src/drivers/device/i2c_linux.cpp | 6 +-- src/drivers/device/i2c_linux.h | 27 ++--------- src/drivers/device/spi.h | 4 ++ src/drivers/device/vdev.cpp | 97 ++++++++++++++++++------------------- src/drivers/device/vdev.h | 18 ++++--- src/drivers/device/vdev_posix.cpp | 18 +++---- src/drivers/ms5611/ms5611_linux.cpp | 10 ++-- 8 files changed, 82 insertions(+), 100 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 98851edc8..4d941b752 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -36,6 +36,6 @@ #ifdef __PX4_NUTTX #include "device_nuttx.h" #elif defined (__PX4_LINUX) -#include "vdevice.h" +#include "vdev.h" #endif diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 4399626aa..4fe2c415c 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -55,7 +55,7 @@ I2C::I2C(const char *name, int bus, uint16_t address) : // base class - CDev(name, devname), + VDev(name, devname), // public // protected _retries(0), @@ -93,7 +93,7 @@ I2C::init() // way to set it from user space. // do base class init, which will create device node, etc - ret = CDev::init(); + ret = VDev::init(); if (ret != PX4_OK) { debug("cdev init failed"); @@ -231,7 +231,7 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) return 0; default: /* give it to the superclass */ - return CDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); } } diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index aae1e7f58..a49c03267 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -40,7 +40,7 @@ #ifndef _DEVICE_I2C_H #define _DEVICE_I2C_H -#include "device.h" +#include "vdev.h" #include #include @@ -53,7 +53,7 @@ namespace device __EXPORT /** * Abstract class for character device on I2C */ -class __EXPORT I2C : public CDev +class __EXPORT I2C : public VDev { public: @@ -92,12 +92,6 @@ protected: virtual ~I2C(); virtual int init(); -#if 0 - /** - * Check for the presence of the device on the bus. - */ - virtual int probe(); -#endif virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); @@ -126,25 +120,10 @@ protected: */ int transfer(struct i2c_msg *msgv, unsigned msgs); -#if 0 - /** - * Change the bus address. - * - * Most often useful during probe() when the driver is testing - * several possible bus addresses. - * - * @param address The new bus address to set. - */ - void set_address(uint16_t address) { - _address = address; - _device_id.devid_s.address = _address; - } -#endif - private: uint16_t _address; int _fd; - std::string _dname; + std::string _dname; I2C(const device::I2C&); I2C operator=(const device::I2C&); diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 482288014..2f44f3caf 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -50,7 +50,11 @@ namespace device __EXPORT /** * Abstract class for character device on SPI */ +#ifdef __PX4_NUTTX class __EXPORT SPI : public CDev +#else +class __EXPORT SPI : public VDev +#endif { protected: /** diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index da1ff0576..1e69384e6 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -38,7 +38,7 @@ */ #include "px4_posix.h" -#include "device.h" +#include "vdev.h" #include "drivers/drv_device.h" #include @@ -49,7 +49,6 @@ namespace device { int px4_errno; - struct px4_dev_t { char *name; @@ -73,7 +72,7 @@ static px4_dev_t *devmap[PX4_MAX_DEV]; * directly, so we have to bounce them through this dispatch table. */ -CDev::CDev(const char *name, +VDev::VDev(const char *name, const char *devname) : // base class Device(name), @@ -89,14 +88,14 @@ CDev::CDev(const char *name, _pollset[i] = nullptr; } -CDev::~CDev() +VDev::~VDev() { if (_registered) unregister_driver(_devname); } int -CDev::register_class_devname(const char *class_devname) +VDev::register_class_devname(const char *class_devname) { if (class_devname == nullptr) { return -EINVAL; @@ -119,7 +118,7 @@ CDev::register_class_devname(const char *class_devname) } int -CDev::register_driver(const char *name, void *data) +VDev::register_driver(const char *name, void *data) { int ret = -ENOSPC; @@ -145,7 +144,7 @@ CDev::register_driver(const char *name, void *data) } int -CDev::unregister_driver(const char *name) +VDev::unregister_driver(const char *name) { int ret = -ENOSPC; @@ -165,7 +164,7 @@ CDev::unregister_driver(const char *name) } int -CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +VDev::unregister_class_devname(const char *class_devname, unsigned class_instance) { char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); @@ -180,7 +179,7 @@ CDev::unregister_class_devname(const char *class_devname, unsigned class_instanc } int -CDev::init() +VDev::init() { // base class init first int ret = Device::init(); @@ -206,11 +205,11 @@ out: * Default implementations of the character device interface */ int -CDev::open(px4_dev_handle_t *handlep) +VDev::open(px4_dev_handle_t *handlep) { int ret = PX4_OK; - debug("CDev::open"); + debug("VDev::open"); lock(); /* increment the open count */ _open_count++; @@ -230,16 +229,16 @@ CDev::open(px4_dev_handle_t *handlep) } int -CDev::open_first(px4_dev_handle_t *handlep) +VDev::open_first(px4_dev_handle_t *handlep) { - debug("CDev::open_first"); + debug("VDev::open_first"); return PX4_OK; } int -CDev::close(px4_dev_handle_t *handlep) +VDev::close(px4_dev_handle_t *handlep) { - debug("CDev::close"); + debug("VDev::close"); int ret = PX4_OK; lock(); @@ -262,68 +261,66 @@ CDev::close(px4_dev_handle_t *handlep) } int -CDev::close_last(px4_dev_handle_t *handlep) +VDev::close_last(px4_dev_handle_t *handlep) { - debug("CDev::close_last"); + debug("VDev::close_last"); return PX4_OK; } ssize_t -CDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +VDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) { - debug("CDev::read"); + debug("VDev::read"); return -ENOSYS; } ssize_t -CDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +VDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) { - debug("CDev::write"); + debug("VDev::write"); return -ENOSYS; } off_t -CDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) +VDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) { return -ENOSYS; } int -CDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) { - debug("CDev::ioctl"); + int ret = -ENOTTY; + + debug("VDev::ioctl"); switch (cmd) { /* fetch a pointer to the driver's private data */ case PX4_DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; - return PX4_OK; + ret = PX4_OK; break; case PX4_DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); - return PX4_OK; + ret = PX4_OK; break; case PX4_DEVIOCGPUBBLOCK: - return _pub_blocked; + ret = _pub_blocked; + break; + case PX4_DEVIOCGDEVICEID: + ret = (int)_device_id.devid; + default: break; } -#if 0 - /* try the superclass. The different ioctl() function form - * means we need to copy arg */ - unsigned arg2 = arg; - int ret = Device::ioctl(cmd, arg2); - if (ret != -ENODEV) - return ret; -#endif return -ENOTTY; } int -CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) +VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) { int ret = PX4_OK; - debug("CDev::Poll %s", setup ? "setup" : "teardown"); + debug("VDev::Poll %s", setup ? "setup" : "teardown"); /* * Lock against pollnotify() (and possibly other callers) @@ -336,7 +333,7 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * benefit. */ fds->priv = (void *)handlep; - debug("CDev::poll: fds->priv = %p", handlep); + debug("VDev::poll: fds->priv = %p", handlep); /* * Handle setup requests. @@ -369,9 +366,9 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) } void -CDev::poll_notify(pollevent_t events) +VDev::poll_notify(pollevent_t events) { - debug("CDev::poll_notify events = %0x", events); + debug("VDev::poll_notify events = %0x", events); /* lock against poll() as well as other wakeups */ lock(); @@ -384,9 +381,9 @@ CDev::poll_notify(pollevent_t events) } void -CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - debug("CDev::poll_notify_one"); + debug("VDev::poll_notify_one"); int value; sem_getvalue(fds->sem, &value); @@ -402,20 +399,20 @@ CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) } pollevent_t -CDev::poll_state(px4_dev_handle_t *handlep) +VDev::poll_state(px4_dev_handle_t *handlep) { - debug("CDev::poll_notify"); + debug("VDev::poll_notify"); /* by default, no poll events to report */ return 0; } int -CDev::store_poll_waiter(px4_pollfd_struct_t *fds) +VDev::store_poll_waiter(px4_pollfd_struct_t *fds) { /* * Look for a free slot. */ - debug("CDev::store_poll_waiter"); + debug("VDev::store_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -430,9 +427,9 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds) } int -CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) +VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { - debug("CDev::remove_poll_waiter"); + debug("VDev::remove_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -446,12 +443,12 @@ CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) return -EINVAL; } -CDev *CDev::getDev(const char *path) +VDev *VDev::getDev(const char *path) { int i=0; for (; iname, path) == 0)) { - return (CDev *)(devmap[i]->cdev); + return (VDev *)(devmap[i]->cdev); } } return NULL; diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index bf823f5b5..9334c06b7 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -218,9 +218,9 @@ private: }; /** - * Abstract class for any character device + * Abstract class for any virtual character device */ -class __EXPORT CDev : public Device +class __EXPORT VDev : public Device { public: /** @@ -229,12 +229,12 @@ public: * @param name Driver name * @param devname Device node name */ - CDev(const char *name, const char *devname); + VDev(const char *name, const char *devname); /** * Destructor */ - virtual ~CDev(); + virtual ~VDev(); virtual int init(); @@ -333,7 +333,7 @@ public: */ virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); - static CDev *getDev(const char *path); + static VDev *getDev(const char *path); protected: @@ -452,14 +452,15 @@ private: int remove_poll_waiter(px4_pollfd_struct_t *fds); /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); + VDev(const VDev&); + VDev operator=(const VDev&); }; +#if 0 /** * Abstract class for character device accessed via PIO */ -class __EXPORT PIO : public CDev +class __EXPORT VPIO : public CDev { public: /** @@ -519,6 +520,7 @@ protected: private: unsigned long _base; }; +#endif } // namespace device diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index fab308ef0..bb119624c 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -84,7 +84,7 @@ inline bool valid_fd(int fd) int px4_open(const char *path, int flags) { - CDev *dev = CDev::getDev(path); + VDev *dev = VDev::getDev(path); int ret = 0; int i; @@ -117,7 +117,7 @@ int px4_close(int fd) { int ret; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_close fd = %d\n", fd); ret = dev->close(filemap[fd]); filemap[fd] = NULL; @@ -135,7 +135,7 @@ ssize_t px4_read(int fd, void *buffer, size_t buflen) { int ret; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_read fd = %d\n", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); } @@ -152,7 +152,7 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) { int ret = PX4_ERROR; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_write fd = %d\n", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); } @@ -169,7 +169,7 @@ int px4_ioctl(int fd, int cmd, unsigned long arg) { int ret = PX4_ERROR; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_ioctl fd = %d\n", fd); ret = dev->ioctl(filemap[fd], cmd, arg); } @@ -206,8 +206,8 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // If fd is valid if (valid_fd(fds[i].fd)) { - CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; - PX4_DEBUG("px4_poll: CDev->poll(setup) %d\n", fds[i].fd); + VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);; + PX4_DEBUG("px4_poll: VDev->poll(setup) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) @@ -250,8 +250,8 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // If fd is valid if (valid_fd(fds[i].fd)) { - CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; - PX4_DEBUG("px4_poll: CDev->poll(teardown) %d\n", fds[i].fd); + VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);; + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 913c1ebcd..490601b74 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -100,7 +100,7 @@ static const int ERROR = -1; #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" -class MS5611 : public device::CDev +class MS5611 : public device::VDev { public: MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); @@ -210,7 +210,7 @@ protected: extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : - CDev("MS5611", path), + VDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -259,9 +259,9 @@ MS5611::init() { int ret; - ret = CDev::init(); + ret = VDev::init(); if (ret != OK) { - debug("CDev init failed"); + debug("VDev init failed"); goto out; } @@ -505,7 +505,7 @@ MS5611::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return CDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); } void -- cgit v1.2.3