dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added 

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-03-12 15:26:10 -07:00
parent 0553a68a1a
commit f3596e555b
13 changed files with 2235 additions and 527 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
makefiles/linux/config_linux_default.mk
View File

@ -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

531
src/drivers/device/device.h
View File

@ -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 <px4_config.h>
#include <errno.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <poll.h>
#include <nuttx/fs/fs.h>
/**
* 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 */

560
src/drivers/device/device_nuttx.h Normal file
View File

@ -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 <px4_config.h>
#include <errno.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <poll.h>
/**
* 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 */

9
src/drivers/device/module.mk
View File

@ -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

643
src/drivers/device/vcdev.cpp Normal file
View File

@ -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 <stdlib.h>
#include <stdio.h>
#include <string.h>
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;i<PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name,name) == 0)) {
return -EEXIST;
}
}
for (int i=0;i<PX4_MAX_DEV; ++i) {
if (devmap[i] == NULL) {
devmap[i] = new px4_dev_t(name, (void *)data);
log("Registered DEV %s", name);
ret = PX4_OK;
break;
}
}
return ret;
}
int
CDev::unregister_driver(const char *name)
{
int ret = -ENOSPC;
if (name == NULL)
return -EINVAL;
for (int i=0;i<PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(name, devmap[i]->name) == 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;i<PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name,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 (; i<PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(devmap[i]->name, 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; i<PX4_MAX_FD; ++i) {
if (device::filemap[i] == 0) {
device::filemap[i] = new device::px4_dev_handle_t(flags,dev,i);
break;
}
}
if (i < PX4_MAX_FD) {
ret = dev->open(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<nfds; ++i)
{
fds[i].sem = &sem;
fds[i].revents = 0;
fds[i].priv = NULL;
// If fd is valid
if (fds[i].fd >= 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<nfds; ++i)
{
fds[i].sem = &sem;
fds[i].revents = 0;
fds[i].priv = NULL;
// If fd is valid
if (fds[i].fd >= 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;
}
}

107
src/drivers/device/vdevice.cpp Normal file
View File

@ -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 <px4_defines.h>
#include <stdio.h>
#include <unistd.h>
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

496
src/drivers/device/vdevice.h Normal file
View File

@ -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 <px4_posix.h>
#include <errno.h>
#include <stdbool.h>
#include <stdio.h>
#include <stddef.h>
#include <stdint.h>
#include <semaphore.h>
/**
* 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
};

43
src/platforms/linux/vcdev_test/module.mk Normal file
View File

@ -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

150
src/platforms/linux/vcdev_test/vcdevtest_example.cpp Normal file
View File

@ -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 <charlebm@gmail.com>
*/
#include "vcdevtest_example.h"
#include "drivers/device/device.h"
#include <unistd.h>
#include <stdio.h>
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;
}

58
src/platforms/linux/vcdev_test/vcdevtest_example.h Normal file
View File

@ -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 <charlebm@gmail.com>
*/
#pragma once
#include <px4_app.h>
class VCDevNode;
class VCDevExample {
public:
VCDevExample() : _node(0) {};
~VCDevExample();
int main();
static px4::AppMgr mgr; /* Manage requests to terminate app */
private:
VCDevNode *_node;
};

55
src/platforms/linux/vcdev_test/vcdevtest_main.cpp Normal file
View File

@ -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 <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_app.h>
#include "vcdevtest_example.h"
#include <stdio.h>
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "vcdevtest");
printf("vcdevtest\n");
VCDevExample vcdevtest;
vcdevtest.main();
printf("goodbye\n");
return 0;
}

100
src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp Normal file
View File

@ -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 <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "vcdevtest_example.h"
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
#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;
}

7
src/platforms/px4_posix.h
View File

@ -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;