diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index a501e67193..56567e48f4 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -10,7 +10,7 @@ # # Board support modules # -#MODULES += drivers/device +MODULES += drivers/device #MODULES += modules/sensors # @@ -55,4 +55,5 @@ MODULES += lib/geo_lookup # MODULES += platforms/linux/px4_layer MODULES += platforms/linux/hello +MODULES += platforms/linux/vcdev_test diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index cf7ec47fc5..98851edc83 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -31,530 +31,11 @@ * ****************************************************************************/ -/** - * @file device.h - * - * Definitions for the generic base classes in the device framework. - */ +#pragma once -#ifndef _DEVICE_DEVICE_H -#define _DEVICE_DEVICE_H +#ifdef __PX4_NUTTX +#include "device_nuttx.h" +#elif defined (__PX4_LINUX) +#include "vdevice.h" +#endif -/* - * Includes here should only cover the needs of the framework definitions. - */ -#include - -#include -#include -#include -#include -#include - -#include - -/** - * Namespace encapsulating all device framework classes, functions and data. - */ -namespace device __EXPORT -{ - -/** - * Fundamental base class for all device drivers. - * - * This class handles the basic "being a driver" things, including - * interrupt registration and dispatch. - */ -class __EXPORT Device -{ -public: - /** - * Destructor. - * - * Public so that anonymous devices can be destroyed. - */ - virtual ~Device(); - - /** - * Interrupt handler. - */ - virtual void interrupt(void *ctx); /**< interrupt handler */ - - /* - * Direct access methods. - */ - - /** - * Initialise the driver and make it ready for use. - * - * @return OK if the driver initialized OK, negative errno otherwise; - */ - virtual int init(); - - /** - * Read directly from the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param offset The device address at which to start reading - * @param data The buffer into which the read values should be placed. - * @param count The number of items to read. - * @return The number of items read on success, negative errno otherwise. - */ - virtual int read(unsigned address, void *data, unsigned count); - - /** - * Write directly to the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param address The device address at which to start writing. - * @param data The buffer from which values should be read. - * @param count The number of items to write. - * @return The number of items written on success, negative errno otherwise. - */ - virtual int write(unsigned address, void *data, unsigned count); - - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform. - * @param arg An argument to the operation. - * @return Negative errno on error, OK or positive value on success. - */ - virtual int ioctl(unsigned operation, unsigned &arg); - - /* - device bus types for DEVID - */ - enum DeviceBusType { - DeviceBusType_UNKNOWN = 0, - DeviceBusType_I2C = 1, - DeviceBusType_SPI = 2, - DeviceBusType_UAVCAN = 3, - }; - - /* - broken out device elements. The bitfields are used to keep - the overall value small enough to fit in a float accurately, - which makes it possible to transport over the MAVLink - parameter protocol without loss of information. - */ - struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; - - union DeviceId { - struct DeviceStructure devid_s; - uint32_t devid; - }; - -protected: - const char *_name; /**< driver name */ - bool _debug_enabled; /**< if true, debug messages are printed */ - union DeviceId _device_id; /**< device identifier information */ - - /** - * Constructor - * - * @param name Driver name - * @param irq Interrupt assigned to the device. - */ - Device(const char *name, - int irq = 0); - - /** - * Enable the device interrupt - */ - void interrupt_enable(); - - /** - * Disable the device interrupt - */ - void interrupt_disable(); - - /** - * Take the driver lock. - * - * Each driver instance has its own lock/semaphore. - * - * Note that we must loop as the wait may be interrupted by a signal. - */ - void lock() { - do {} while (sem_wait(&_lock) != 0); - } - - /** - * Release the driver lock. - */ - void unlock() { - sem_post(&_lock); - } - - /** - * Log a message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void log(const char *fmt, ...); - - /** - * Print a debug message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void debug(const char *fmt, ...); - -private: - int _irq; - bool _irq_attached; - sem_t _lock; - - /** disable copy construction for this and all subclasses */ - Device(const Device &); - - /** disable assignment for this and all subclasses */ - Device &operator = (const Device &); - - /** - * Register ourselves as a handler for an interrupt - * - * @param irq The interrupt to claim - * @return OK if the interrupt was registered - */ - int dev_register_interrupt(int irq); - - /** - * Unregister ourselves as a handler for any interrupt - */ - void dev_unregister_interrupt(); - - /** - * Interrupt dispatcher - * - * @param irq The interrupt that has been triggered. - * @param context Pointer to the interrupted context. - */ - static void dev_interrupt(int irq, void *context); - -}; - -/** - * Abstract class for any character device - */ -class __EXPORT CDev : public Device -{ -public: - /** - * Constructor - * - * @param name Driver name - * @param devname Device node name - * @param irq Interrupt assigned to the device - */ - CDev(const char *name, const char *devname, int irq = 0); - - /** - * Destructor - */ - virtual ~CDev(); - - virtual int init(); - - /** - * Handle an open of the device. - * - * This function is called for every open of the device. The default - * implementation maintains _open_count and always returns OK. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the open is allowed, -errno otherwise. - */ - virtual int open(struct file *filp); - - /** - * Handle a close of the device. - * - * This function is called for every close of the device. The default - * implementation maintains _open_count and returns OK as long as it is not zero. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the close was successful, -errno otherwise. - */ - virtual int close(struct file *filp); - - /** - * Perform a read from the device. - * - * The default implementation returns -ENOSYS. - * - * @param filp Pointer to the NuttX file structure. - * @param buffer Pointer to the buffer into which data should be placed. - * @param buflen The number of bytes to be read. - * @return The number of bytes read or -errno otherwise. - */ - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - - /** - * Perform a write to the device. - * - * The default implementation returns -ENOSYS. - * - * @param filp Pointer to the NuttX file structure. - * @param buffer Pointer to the buffer from which data should be read. - * @param buflen The number of bytes to be written. - * @return The number of bytes written or -errno otherwise. - */ - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - - /** - * Perform a logical seek operation on the device. - * - * The default implementation returns -ENOSYS. - * - * @param filp Pointer to the NuttX file structure. - * @param offset The new file position relative to whence. - * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. - * @return The previous offset, or -errno otherwise. - */ - virtual off_t seek(struct file *filp, off_t offset, int whence); - - /** - * Perform an ioctl operation on the device. - * - * The default implementation handles DIOC_GETPRIV, and otherwise - * returns -ENOTTY. Subclasses should call the default implementation - * for any command they do not handle themselves. - * - * @param filp Pointer to the NuttX file structure. - * @param cmd The ioctl command value. - * @param arg The ioctl argument value. - * @return OK on success, or -errno otherwise. - */ - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Perform a poll setup/teardown operation. - * - * This is handled internally and should not normally be overridden. - * - * @param filp Pointer to the NuttX file structure. - * @param fds Poll descriptor being waited on. - * @param arg True if this is establishing a request, false if - * it is being torn down. - * @return OK on success, or -errno otherwise. - */ - virtual int poll(struct file *filp, struct pollfd *fds, bool setup); - - /** - * Test whether the device is currently open. - * - * This can be used to avoid tearing down a device that is still active. - * Note - not virtual, cannot be overridden by a subclass. - * - * @return True if the device is currently open. - */ - bool is_open() { return _open_count > 0; } - -protected: - /** - * Pointer to the default cdev file operations table; useful for - * registering clone devices etc. - */ - static const struct file_operations fops; - - /** - * Check the current state of the device for poll events from the - * perspective of the file. - * - * This function is called by the default poll() implementation when - * a poll is set up to determine whether the poll should return immediately. - * - * The default implementation returns no events. - * - * @param filp The file that's interested. - * @return The current set of poll events. - */ - virtual pollevent_t poll_state(struct file *filp); - - /** - * Report new poll events. - * - * This function should be called anytime the state of the device changes - * in a fashion that might be interesting to a poll waiter. - * - * @param events The new event(s) being announced. - */ - virtual void poll_notify(pollevent_t events); - - /** - * Internal implementation of poll_notify. - * - * @param fds A poll waiter to notify. - * @param events The event(s) to send to the waiter. - */ - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); - - /** - * Notification of the first open. - * - * This function is called when the device open count transitions from zero - * to one. The driver lock is held for the duration of the call. - * - * The default implementation returns OK. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the open should proceed, -errno otherwise. - */ - virtual int open_first(struct file *filp); - - /** - * Notification of the last close. - * - * This function is called when the device open count transitions from - * one to zero. The driver lock is held for the duration of the call. - * - * The default implementation returns OK. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the open should return OK, -errno otherwise. - */ - virtual int close_last(struct file *filp); - - /** - * Register a class device name, automatically adding device - * class instance suffix if need be. - * - * @param class_devname Device class name - * @return class_instamce Class instance created, or -errno on failure - */ - virtual int register_class_devname(const char *class_devname); - - /** - * Register a class device name, automatically adding device - * class instance suffix if need be. - * - * @param class_devname Device class name - * @param class_instance Device class instance from register_class_devname() - * @return OK on success, -errno otherwise - */ - virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char* get_devname() { return _devname; } - - bool _pub_blocked; /**< true if publishing should be blocked */ - -private: - static const unsigned _max_pollwaiters = 8; - - const char *_devname; /**< device node name */ - bool _registered; /**< true if device name was registered */ - unsigned _open_count; /**< number of successful opens */ - - struct pollfd *_pollset[_max_pollwaiters]; - - /** - * Store a pollwaiter in a slot where we can find it later. - * - * Expands the pollset as required. Must be called with the driver locked. - * - * @return OK, or -errno on error. - */ - int store_poll_waiter(struct pollfd *fds); - - /** - * Remove a poll waiter. - * - * @return OK, or -errno on error. - */ - int remove_poll_waiter(struct pollfd *fds); - - /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); -}; - -/** - * Abstract class for character device accessed via PIO - */ -class __EXPORT PIO : public CDev -{ -public: - /** - * Constructor - * - * @param name Driver name - * @param devname Device node name - * @param base Base address of the device PIO area - * @param irq Interrupt assigned to the device (or zero if none) - */ - PIO(const char *name, - const char *devname, - uint32_t base, - int irq = 0); - virtual ~PIO(); - - virtual int init(); - -protected: - - /** - * Read a register - * - * @param offset Register offset in bytes from the base address. - */ - uint32_t reg(uint32_t offset) { - return *(volatile uint32_t *)(_base + offset); - } - - /** - * Write a register - * - * @param offset Register offset in bytes from the base address. - * @param value Value to write. - */ - void reg(uint32_t offset, uint32_t value) { - *(volatile uint32_t *)(_base + offset) = value; - } - - /** - * Modify a register - * - * Note that there is a risk of a race during the read/modify/write cycle - * that must be taken care of by the caller. - * - * @param offset Register offset in bytes from the base address. - * @param clearbits Bits to clear in the register - * @param setbits Bits to set in the register - */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { - uint32_t val = reg(offset); - val &= ~clearbits; - val |= setbits; - reg(offset, val); - } - -private: - uint32_t _base; -}; - -} // namespace device - -// class instance for primary driver of each class -enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 -}; - -#endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h new file mode 100644 index 0000000000..87e5e2abe0 --- /dev/null +++ b/src/drivers/device/device_nuttx.h @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. 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 device_nuttx.h + * + * Definitions for the generic base classes in the device framework. + */ + +#ifndef _DEVICE_DEVICE_H +#define _DEVICE_DEVICE_H + +/* + * Includes here should only cover the needs of the framework definitions. + */ +#include + +#include +#include +#include +#include +#include + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +/** + * Fundamental base class for all device drivers. + * + * This class handles the basic "being a driver" things, including + * interrupt registration and dispatch. + */ +class __EXPORT Device +{ +public: + /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + + /** + * Interrupt handler. + */ + virtual void interrupt(void *ctx); /**< interrupt handler */ + + /* + * Direct access methods. + */ + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); + +#if 0 + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + virtual int read(unsigned address, void *data, unsigned count); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + virtual int write(unsigned address, void *data, unsigned count); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int ioctl(unsigned operation, unsigned &arg); +#endif + + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, + }; + + /* + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. + */ + struct DeviceStructure { + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + +protected: + const char *_name; /**< driver name */ + bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ + + /** + * Constructor + * + * @param name Driver name + * @param irq Interrupt assigned to the device. + */ + Device(const char *name, + int irq = 0); + + /** + * Enable the device interrupt + */ + void interrupt_enable(); + + /** + * Disable the device interrupt + */ + void interrupt_disable(); + + /** + * Take the driver lock. + * + * Each driver instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + */ + void lock() { + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + sem_post(&_lock); + } + + /** + * Log a message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void log(const char *fmt, ...); + + /** + * Print a debug message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void debug(const char *fmt, ...); + +private: + int _irq; + bool _irq_attached; + sem_t _lock; + + /** disable copy construction for this and all subclasses */ + Device(const Device &); + + /** disable assignment for this and all subclasses */ + Device &operator = (const Device &); + + /** + * Register ourselves as a handler for an interrupt + * + * @param irq The interrupt to claim + * @return OK if the interrupt was registered + */ + int dev_register_interrupt(int irq); + + /** + * Unregister ourselves as a handler for any interrupt + */ + void dev_unregister_interrupt(); + + /** + * Interrupt dispatcher + * + * @param irq The interrupt that has been triggered. + * @param context Pointer to the interrupted context. + */ + static void dev_interrupt(int irq, void *context); + +}; + +/** + * Abstract class for any character device + */ +class __EXPORT CDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param irq Interrupt assigned to the device + */ + CDev(const char *name, const char *devname, int irq = 0); + + /** + * Destructor + */ + virtual ~CDev(); + + virtual int init(); + + /** + * Handle an open of the device. + * + * This function is called for every open of the device. The default + * implementation maintains _open_count and always returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(struct file *filp); + + /** + * Handle a close of the device. + * + * This function is called for every close of the device. The default + * implementation maintains _open_count and returns OK as long as it is not zero. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(struct file *filp); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer into which data should be placed. + * @param buflen The number of bytes to be read. + * @return The number of bytes read or -errno otherwise. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer from which data should be read. + * @param buflen The number of bytes to be written. + * @return The number of bytes written or -errno otherwise. + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param offset The new file position relative to whence. + * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. + * @return The previous offset, or -errno otherwise. + */ + virtual off_t seek(struct file *filp, off_t offset, int whence); + + /** + * Perform an ioctl operation on the device. + * + * The default implementation handles DIOC_GETPRIV, and otherwise + * returns -ENOTTY. Subclasses should call the default implementation + * for any command they do not handle themselves. + * + * @param filp Pointer to the NuttX file structure. + * @param cmd The ioctl command value. + * @param arg The ioctl argument value. + * @return OK on success, or -errno otherwise. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Perform a poll setup/teardown operation. + * + * This is handled internally and should not normally be overridden. + * + * @param filp Pointer to the NuttX file structure. + * @param fds Poll descriptor being waited on. + * @param arg True if this is establishing a request, false if + * it is being torn down. + * @return OK on success, or -errno otherwise. + */ + virtual int poll(struct file *filp, struct pollfd *fds, bool setup); + + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } + +protected: + /** + * Pointer to the default cdev file operations table; useful for + * registering clone devices etc. + */ + static const struct file_operations fops; + + /** + * Check the current state of the device for poll events from the + * perspective of the file. + * + * This function is called by the default poll() implementation when + * a poll is set up to determine whether the poll should return immediately. + * + * The default implementation returns no events. + * + * @param filp The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(struct file *filp); + + /** + * Report new poll events. + * + * This function should be called anytime the state of the device changes + * in a fashion that might be interesting to a poll waiter. + * + * @param events The new event(s) being announced. + */ + virtual void poll_notify(pollevent_t events); + + /** + * Internal implementation of poll_notify. + * + * @param fds A poll waiter to notify. + * @param events The event(s) to send to the waiter. + */ + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + + /** + * Notification of the first open. + * + * This function is called when the device open count transitions from zero + * to one. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(struct file *filp); + + /** + * Notification of the last close. + * + * This function is called when the device open count transitions from + * one to zero. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(struct file *filp); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + + bool _pub_blocked; /**< true if publishing should be blocked */ + +private: + static const unsigned _max_pollwaiters = 8; + + const char *_devname; /**< device node name */ + bool _registered; /**< true if device name was registered */ + unsigned _open_count; /**< number of successful opens */ + + struct pollfd *_pollset[_max_pollwaiters]; + + /** + * Store a pollwaiter in a slot where we can find it later. + * + * Expands the pollset as required. Must be called with the driver locked. + * + * @return OK, or -errno on error. + */ + int store_poll_waiter(struct pollfd *fds); + + /** + * Remove a poll waiter. + * + * @return OK, or -errno on error. + */ + int remove_poll_waiter(struct pollfd *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); +}; + +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT PIO : public CDev +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param base Base address of the device PIO area + * @param irq Interrupt assigned to the device (or zero if none) + */ + PIO(const char *name, + const char *devname, + uint32_t base, + int irq = 0); + virtual ~PIO(); + + virtual int init(); + +protected: + + /** + * Read a register + * + * @param offset Register offset in bytes from the base address. + */ + unsigned long reg(uint32_t offset) { + return *(volatile uint32_t *)(_base + offset); + } + + /** + * Write a register + * + * @param offset Register offset in bytes from the base address. + * @param value Value to write. + */ + void reg(uint32_t offset, uint32_t value) { + *(volatile uint32_t *)(_base + offset) = value; + } + + /** + * Modify a register + * + * Note that there is a risk of a race during the read/modify/write cycle + * that must be taken care of by the caller. + * + * @param offset Register offset in bytes from the base address. + * @param clearbits Bits to clear in the register + * @param setbits Bits to set in the register + */ + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + uint32_t val = reg(offset); + val &= ~clearbits; + val |= setbits; + reg(offset, val); + } + +private: + unsigned long _base; +}; + +} // namespace device + +// class instance for primary driver of each class +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY=0, + CLASS_DEVICE_SECONDARY=1, + CLASS_DEVICE_TERTIARY=2 +}; + +#endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 8c716d9cdb..216fae1ab3 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -35,8 +35,15 @@ # Build the device driver framework. # -SRCS = cdev.cpp \ +#ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = \ device.cpp \ + cdev.cpp \ i2c.cpp \ pio.cpp \ spi.cpp +#endif +#ifeq ($(PX4_TARGET_OS),linux) +SRCS = vcdev.cpp \ + vdevice.cpp +#endif diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vcdev.cpp new file mode 100644 index 0000000000..174094547c --- /dev/null +++ b/src/drivers/device/vcdev.cpp @@ -0,0 +1,643 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. 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 cdev.cpp + * + * Character device base class. + */ + +//#include "px4_platform.h" +//#include "px4_device.h" +#include "px4_posix.h" +#include "device.h" +#include "drivers/drv_device.h" + +#include +#include +#include + +namespace device +{ + +int px4_errno; + + +struct px4_dev_t { + char *name; + void *cdev; + + px4_dev_t(const char *n, void *c) : cdev(c) { + name = strdup(n); + } + + ~px4_dev_t() { free(name); } + +private: + px4_dev_t() {} +}; + +#define PX4_MAX_DEV 100 +static px4_dev_t *devmap[PX4_MAX_DEV]; + +/* + * The standard NuttX operation dispatch table can't call C++ member functions + * directly, so we have to bounce them through this dispatch table. + */ + +CDev::CDev(const char *name, + const char *devname) : + // base class + Device(name), + // public + // protected + _pub_blocked(false), + // private + _devname(devname), + _registered(false), + _open_count(0) +{ + for (unsigned i = 0; i < _max_pollwaiters; i++) + _pollset[i] = nullptr; +} + +CDev::~CDev() +{ + if (_registered) + unregister_driver(_devname); +} + +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + + int class_instance = 0; + int ret = -ENOSPC; + + while (class_instance < 4) { + char name[32]; + snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); + ret = register_driver(name, (void *)this); + class_instance++; + } + + if (class_instance == 4) { + return ret; + } + return class_instance; +} + +int +CDev::register_driver(const char *name, void *data) +{ + int ret = -ENOSPC; + + if (name == NULL || data == NULL) + return -EINVAL; + + // Make sure the device does not already exist + // FIXME - convert this to a map for efficiency + for (int i=0;iname,name) == 0)) { + return -EEXIST; + } + } + for (int i=0;iname) == 0)) { + delete devmap[i]; + devmap[i] = NULL; + log("Unregistered DEV %s", name); + ret = PX4_OK; + break; + } + } + return ret; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + for (int i=0;iname,name) == 0) { + delete devmap[i]; + devmap[i] = NULL; + return PX4_OK; + } + } + return -EINVAL; +} + +int +CDev::init() +{ + // base class init first + int ret = Device::init(); + + if (ret != PX4_OK) + goto out; + + // now register the driver + if (_devname != nullptr) { + ret = register_driver(_devname, (void *)this); + + if (ret != PX4_OK) + goto out; + + _registered = true; + } + +out: + return ret; +} + +/* + * Default implementations of the character device interface + */ +int +CDev::open(px4_dev_handle_t *handlep) +{ + int ret = PX4_OK; + + debug("CDev::open"); + lock(); + /* increment the open count */ + _open_count++; + + if (_open_count == 1) { + + /* first-open callback may decline the open */ + ret = open_first(handlep); + + if (ret != PX4_OK) + _open_count--; + } + + unlock(); + + return ret; +} + +int +CDev::open_first(px4_dev_handle_t *handlep) +{ + debug("CDev::open_first"); + return PX4_OK; +} + +int +CDev::close(px4_dev_handle_t *handlep) +{ + debug("CDev::close"); + int ret = PX4_OK; + + lock(); + + if (_open_count > 0) { + /* decrement the open count */ + _open_count--; + + /* callback cannot decline the close */ + if (_open_count == 0) + ret = close_last(handlep); + + } else { + ret = -EBADF; + } + + unlock(); + + return ret; +} + +int +CDev::close_last(px4_dev_handle_t *handlep) +{ + debug("CDev::close_last"); + return PX4_OK; +} + +ssize_t +CDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + debug("CDev::read"); + return -ENOSYS; +} + +ssize_t +CDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +{ + debug("CDev::write"); + return -ENOSYS; +} + +off_t +CDev::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) +{ + debug("CDev::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; + break; + case PX4_DEVIOCSPUBBLOCK: + _pub_blocked = (arg != 0); + return PX4_OK; + break; + case PX4_DEVIOCGPUBBLOCK: + return _pub_blocked; + 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) +{ + int ret = PX4_OK; + log("CDev::Poll %s", setup ? "setup" : "teardown"); + + /* + * Lock against pollnotify() (and possibly other callers) + */ + lock(); + + if (setup) { + /* + * Save the file pointer in the pollfd for the subclass' + * benefit. + */ + fds->priv = (void *)handlep; + log("CDev::poll: fds->priv = %p", handlep); + + /* + * Handle setup requests. + */ + ret = store_poll_waiter(fds); + + if (ret == PX4_OK) { + + /* + * Check to see whether we should send a poll notification + * immediately. + */ + fds->revents |= fds->events & poll_state(handlep); + + /* yes? post the notification */ + if (fds->revents != 0) + sem_post(fds->sem); + } + + } else { + /* + * Handle a teardown request. + */ + ret = remove_poll_waiter(fds); + } + + unlock(); + + return ret; +} + +void +CDev::poll_notify(pollevent_t events) +{ + log("CDev::poll_notify"); + + /* 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); + + unlock(); + //irqrestore(state); +} + +void +CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +{ + log("CDev::poll_notify_one"); + int value; + sem_getvalue(fds->sem, &value); + + /* update the reported event set */ + fds->revents |= fds->events & events; + + /* 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)) + sem_post(fds->sem); +} + +pollevent_t +CDev::poll_state(px4_dev_handle_t *handlep) +{ + /* by default, no poll events to report */ + return 0; +} + +int +CDev::store_poll_waiter(px4_pollfd_struct_t *fds) +{ + /* + * Look for a free slot. + */ + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (nullptr == _pollset[i]) { + + /* save the pollfd */ + _pollset[i] = fds; + + return PX4_OK; + } + } + + return -ENOMEM; +} + +int +CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) +{ + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (fds == _pollset[i]) { + + _pollset[i] = nullptr; + return PX4_OK; + + } + } + + puts("poll: bad fd state"); + return -EINVAL; +} + +static CDev *get_dev(const char *path) +{ + int i=0; + for (; iname, path) == 0)) { + return (CDev *)(devmap[i]->cdev); + } + } + return NULL; +} + + +#define PX4_MAX_FD 100 +static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; + +} // namespace device + +extern "C" { + +int px4_errno; + +int +px4_open(const char *path, int flags) +{ + device::CDev *dev = device::get_dev(path); + int ret = 0; + int i; + + if (!dev) { + ret = -EINVAL; + } + else { + for (i=0; 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/vdevice.cpp b/src/drivers/device/vdevice.cpp new file mode 100644 index 0000000000..7098339649 --- /dev/null +++ b/src/drivers/device/vdevice.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. 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 device.cpp + * + * Fundamental driver base class for the virtual device framework. + */ + +#include "device.h" + +#include +#include +#include + +namespace device +{ + +Device::Device(const char *name) : + // public + // protected + _name(name), + _debug_enabled(true) +{ + sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid = 0; + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; +} + +Device::~Device() +{ + sem_destroy(&_lock); +} + +int +Device::init() +{ + int ret = OK; + + return ret; +} + +void +Device::log(const char *fmt, ...) +{ + va_list ap; + + printf("[%s] ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} + +void +Device::debug(const char *fmt, ...) +{ + va_list ap; + + if (_debug_enabled) { + printf("<%s> ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); + } +} + +} // namespace device diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdevice.h new file mode 100644 index 0000000000..b9d742118e --- /dev/null +++ b/src/drivers/device/vdevice.h @@ -0,0 +1,496 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. 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 vdevice.h + * + * Definitions for the generic base classes in the virtual device framework. + */ + +#pragma once + +/* + * Includes here should only cover the needs of the framework definitions. + */ +#include + +#include +#include +#include +#include +#include +#include + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + +struct px4_dev_handle_t { + int fd; + int flags; + void *priv; + void *cdev; + + px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), cdev(NULL) {} + px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), cdev(c) {} +}; + +/** + * Fundamental base class for all device drivers. + * + * This class handles the basic "being a driver" things, including + * interrupt registration and dispatch. + */ +class __EXPORT Device +{ +public: + /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + + /* + * Direct access methods. + */ + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); + + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, + }; + + /* + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. + */ + struct DeviceStructure { + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + +protected: + const char *_name; /**< driver name */ + bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ + + /** + * Constructor + * + * @param name Driver name + * @param irq Interrupt assigned to the device. + */ + Device(const char *name); + + /** + * Take the driver lock. + * + * Each driver instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + */ + void lock() { + log("lock"); + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + log("unlock"); + sem_post(&_lock); + } + + /** + * Log a message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void log(const char *fmt, ...); + + /** + * Print a debug message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void debug(const char *fmt, ...); + +private: + sem_t _lock; + + /** disable copy construction for this and all subclasses */ + Device(const Device &); + + /** disable assignment for this and all subclasses */ + Device &operator = (const Device &); +}; + +/** + * Abstract class for any character device + */ +class __EXPORT CDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + */ + CDev(const char *name, const char *devname); + + /** + * Destructor + */ + virtual ~CDev(); + + virtual int init(); + + /** + * Perform a poll setup/teardown operation. Based on NuttX implementation. + * + * This is handled internally and should not normally be overridden. + * + * @param handlep Pointer to the internal file structure. + * @param fds Poll descriptor being waited on. + * @param setup True if this is establishing a request, false if + * it is being torn down. + * @return OK on success, or -errno otherwise. + */ + virtual int poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup); + + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } + + /** + * Handle an open of the device. + * + * This function is called for every open of the device. The default + * implementation maintains _open_count and always returns OK. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(px4_dev_handle_t *handlep); + + /** + * Handle a close of the device. + * + * This function is called for every close of the device. The default + * implementation maintains _open_count and returns OK as long as it is not zero. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(px4_dev_handle_t *handlep); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @param handlep Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer into which data should be placed. + * @param buflen The number of bytes to be read. + * @return The number of bytes read or -errno otherwise. + */ + virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @param handlep Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer from which data should be read. + * @param buflen The number of bytes to be written. + * @return The number of bytes written or -errno otherwise. + */ + virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @param handlep Pointer to the NuttX file structure. + * @param offset The new file position relative to whence. + * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. + * @return The previous offset, or -errno otherwise. + */ + virtual off_t seek(px4_dev_handle_t *handlep, off_t offset, int whence); + + /** + * Perform an ioctl operation on the device. + * + * The default implementation handles DIOC_GETPRIV, and otherwise + * returns -ENOTTY. Subclasses should call the default implementation + * for any command they do not handle themselves. + * + * @param handlep Pointer to the NuttX file structure. + * @param cmd The ioctl command value. + * @param arg The ioctl argument value. + * @return OK on success, or -errno otherwise. + */ + virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + +protected: + + int register_driver(const char *name, void *data); + int unregister_driver(const char *name); + + /** + * Check the current state of the device for poll events from the + * perspective of the file. + * + * This function is called by the default poll() implementation when + * a poll is set up to determine whether the poll should return immediately. + * + * The default implementation returns no events. + * + * @param handlep The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(px4_dev_handle_t *handlep); + + /** + * Report new poll events. + * + * This function should be called anytime the state of the device changes + * in a fashion that might be interesting to a poll waiter. + * + * @param events The new event(s) being announced. + */ + virtual void poll_notify(pollevent_t events); + + /** + * Internal implementation of poll_notify. + * + * @param fds A poll waiter to notify. + * @param events The event(s) to send to the waiter. + */ + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + + /** + * Notification of the first open. + * + * This function is called when the device open count transitions from zero + * to one. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(px4_dev_handle_t *handlep); + + /** + * Notification of the last close. + * + * This function is called when the device open count transitions from + * one to zero. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(px4_dev_handle_t *handlep); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + + bool _pub_blocked; /**< true if publishing should be blocked */ + +private: + static const unsigned _max_pollwaiters = 8; + + const char *_devname; /**< device node name */ + bool _registered; /**< true if device name was registered */ + unsigned _open_count; /**< number of successful opens */ + + px4_pollfd_struct_t *_pollset[_max_pollwaiters]; + + /** + * Store a pollwaiter in a slot where we can find it later. + * + * Expands the pollset as required. Must be called with the driver locked. + * + * @return OK, or -errno on error. + */ + int store_poll_waiter(px4_pollfd_struct_t *fds); + + /** + * Remove a poll waiter. + * + * @return OK, or -errno on error. + */ + int remove_poll_waiter(px4_pollfd_struct_t *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); +}; + +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT PIO : public CDev +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param base Base address of the device PIO area + * @param irq Interrupt assigned to the device (or zero if none) + */ + PIO(const char *name, + const char *devname, + unsigned long base + ); + virtual ~PIO(); + + virtual int init(); + +protected: + + /** + * Read a register + * + * @param offset Register offset in bytes from the base address. + */ + uint32_t reg(uint32_t offset) { + return *(volatile uint32_t *)(_base + offset); + } + + /** + * Write a register + * + * @param offset Register offset in bytes from the base address. + * @param value Value to write. + */ + void reg(uint32_t offset, uint32_t value) { + *(volatile uint32_t *)(_base + offset) = value; + } + + /** + * Modify a register + * + * Note that there is a risk of a race during the read/modify/write cycle + * that must be taken care of by the caller. + * + * @param offset Register offset in bytes from the base address. + * @param clearbits Bits to clear in the register + * @param setbits Bits to set in the register + */ + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + uint32_t val = reg(offset); + val &= ~clearbits; + val |= setbits; + reg(offset, val); + } + +private: + unsigned long _base; +}; + +} // namespace device + +// class instance for primary driver of each class +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY=0, + CLASS_DEVICE_SECONDARY=1, + CLASS_DEVICE_TERTIARY=2 +}; + diff --git a/src/platforms/linux/vcdev_test/module.mk b/src/platforms/linux/vcdev_test/module.mk new file mode 100644 index 0000000000..72cde9e2e3 --- /dev/null +++ b/src/platforms/linux/vcdev_test/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = vcdevtest + +SRCS = vcdevtest_main.cpp \ + vcdevtest_start_linux.cpp \ + vcdevtest_example.cpp + diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp new file mode 100644 index 0000000000..dadd9a2913 --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp @@ -0,0 +1,150 @@ + +/**************************************************************************** + * + * 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 vcdevtest_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "vcdevtest_example.h" +#include "drivers/device/device.h" +#include +#include + +px4::AppMgr VCDevExample::mgr; + +using namespace device; + +#define TESTDEV "/dev/vdevex" + +class VCDevNode : public CDev { +public: + VCDevNode() : CDev("vcdevtest", TESTDEV) {}; + + ~VCDevNode() {} +}; + +VCDevExample::~VCDevExample() { + if (_node) { + delete _node; + _node = 0; + } +} + +int test_pub_block(int fd, unsigned long blocked) +{ + int ret = px4_ioctl(fd, PX4_DEVIOCSPUBBLOCK, blocked); + if (ret < 0) { + printf("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); + return -px4_errno; + } + + ret = px4_ioctl(fd, PX4_DEVIOCGPUBBLOCK, 0); + if (ret < 0) { + printf("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); + return -px4_errno; + } + printf("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + + return 0; +} + +int VCDevExample::main() +{ + mgr.setRunning(true); + + _node = new VCDevNode(); + + if (_node == 0) { + printf("Failed to allocate VCDevNode\n"); + return -ENOMEM; + } + + if (_node->init() != PX4_OK) { + printf("Failed to init VCDevNode\n"); + return 1; + } + + int fd = px4_open(TESTDEV, PX4_F_RDONLY); + + if (fd < 0) { + printf("Open failed %d %d", fd, px4_errno); + return -px4_errno; + } + + void *p = 0; + int ret = px4_ioctl(fd, PX4_DIOC_GETPRIV, (unsigned long)&p); + if (ret < 0) { + printf("ioctl PX4_DIOC_GETPRIV failed %d %d", ret, px4_errno); + return -px4_errno; + } + printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + + ret = test_pub_block(fd, 1); + if (ret < 0) + return ret; + ret = test_pub_block(fd, 0); + if (ret < 0) + return ret; + + int i=0; + px4_pollfd_struct_t fds[1]; + + while (!mgr.exitRequested() && i<5) { + sleep(2); + + printf(" polling...\n"); + fds[0].fd = fd; + fds[0].events = POLLIN; + fds[0].revents = 0; + + ret = px4_poll(fds, 1, 500); + if (ret < 0) { + printf("poll failed %d %d\n", ret, px4_errno); + px4_close(fd); + } + else if (ret == 0) + printf(" Nothing to read\n"); + else { + printf(" %d to read\n", ret); + } + printf(" Doing work...\n"); + ++i; + } + px4_close(fd); + + return 0; +} diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.h b/src/platforms/linux/vcdev_test/vcdevtest_example.h new file mode 100644 index 0000000000..ed00fd5cb9 --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 vcdevtest_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class VCDevNode; + +class VCDevExample { +public: + VCDevExample() : _node(0) {}; + + ~VCDevExample(); + + int main(); + + static px4::AppMgr mgr; /* Manage requests to terminate app */ + +private: + VCDevNode *_node; +}; diff --git a/src/platforms/linux/vcdev_test/vcdevtest_main.cpp b/src/platforms/linux/vcdev_test/vcdevtest_main.cpp new file mode 100644 index 0000000000..6d8d31a4c1 --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 vcdevtest_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include "vcdevtest_example.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "vcdevtest"); + + printf("vcdevtest\n"); + VCDevExample vcdevtest; + vcdevtest.main(); + + printf("goodbye\n"); + return 0; +} diff --git a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp new file mode 100644 index 0000000000..2f37fb05de --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 vcdevtest_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "vcdevtest_example.h" +#include +#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 */ + +//using namespace px4; + +extern "C" __EXPORT int vcdevtest_main(int argc, char *argv[]); +int vcdevtest_main(int argc, char *argv[]) +{ + + if (argc < 2) { + printf("usage: vcdevtest {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (VCDevExample::mgr.isRunning()) { + printf("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("vcdevtest", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + VCDevExample::mgr.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (VCDevExample::mgr.isRunning()) { + printf("is running\n"); + + } else { + printf("not started\n"); + } + + return 0; + } + + printf("usage: vcdevtest_main {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index d08c8d2380..88b8f4e80c 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -47,6 +47,13 @@ #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 +#define PX4_DIOC_GETPRIV 1 +#define PX4_DEVIOCSPUBBLOCK 2 +#define PX4_DEVIOCGPUBBLOCK 3 + +#define PX4_ERROR (-1) +#define PX4_OK 0 + __BEGIN_DECLS extern int px4_errno;