fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 15:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Device: Fixed code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:15:22 +02:00
parent 2154f0ca1d
commit fe216b5c5f
21 changed files with 540 additions and 413 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/device/cdev.cpp
+30 -14
View File
@@ -100,14 +100,16 @@ CDev::CDev(const char *name,
_registered(false),
_open_count(0)
{
for (unsigned i = 0; i < _max_pollwaiters; i++)
for (unsigned i = 0; i < _max_pollwaiters; i++) {
_pollset[i] = nullptr;
}
}
CDev::~CDev()
{
if (_registered)
if (_registered) {
unregister_driver(_devname);
}
}
int
@@ -124,13 +126,16 @@ CDev::register_class_devname(const char *class_devname)
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
if (ret == OK) { break; }
class_instance++;
}
if (class_instance == 4) {
return ret;
}
return class_instance;
}
@@ -148,15 +153,17 @@ CDev::init()
// base class init first
int ret = Device::init();
if (ret != OK)
if (ret != OK) {
goto out;
}
// now register the driver
if (_devname != nullptr) {
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
if (ret != OK) {
goto out;
}
_registered = true;
}
@@ -182,8 +189,9 @@ CDev::open(file_t *filp)
/* first-open callback may decline the open */
ret = open_first(filp);
if (ret != OK)
if (ret != OK) {
_open_count--;
}
}
unlock();
@@ -209,8 +217,9 @@ CDev::close(file_t *filp)
_open_count--;
/* callback cannot decline the close */
if (_open_count == 0)
if (_open_count == 0) {
ret = close_last(filp);
}
} else {
ret = -EBADF;
@@ -250,26 +259,30 @@ CDev::ioctl(file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
/* fetch a pointer to the driver's private data */
/* fetch a pointer to the driver's private data */
case DIOC_GETPRIV:
*(void **)(uintptr_t)arg = (void *)this;
return OK;
break;
case DEVIOCSPUBBLOCK:
_pub_blocked = (arg != 0);
return OK;
break;
case DEVIOCGPUBBLOCK:
return _pub_blocked;
break;
}
/* try the superclass. The different ioctl() function form
* means we need to copy arg */
unsigned arg2 = arg;
* means we need to copy arg */
unsigned arg2 = arg;
int ret = Device::ioctl(cmd, arg2);
if (ret != -ENODEV)
if (ret != -ENODEV) {
return ret;
}
return -ENOTTY;
}
@@ -305,8 +318,9 @@ CDev::poll(file_t *filp, struct pollfd *fds, bool setup)
fds->revents |= fds->events & poll_state(filp);
/* yes? post the notification */
if (fds->revents != 0)
if (fds->revents != 0) {
px4_sem_post(fds->sem);
}
}
} else {
@@ -328,8 +342,9 @@ CDev::poll_notify(pollevent_t events)
irqstate_t state = irqsave();
for (unsigned i = 0; i < _max_pollwaiters; i++)
if (nullptr != _pollset[i])
if (nullptr != _pollset[i]) {
poll_notify_one(_pollset[i], events);
}
irqrestore(state);
}
@@ -342,8 +357,9 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t 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) && (fds->sem->semcount <= 0))
if ((fds->revents != 0) && (fds->sem->semcount <= 0)) {
px4_sem_post(fds->sem);
}
}
pollevent_t
src/drivers/device/device_nuttx.cpp
+10 -5
View File
@@ -95,7 +95,7 @@ Device::Device(const char *name,
_irq_attached(false)
{
sem_init(&_lock, 0, 1);
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid = 0;
@@ -109,8 +109,9 @@ Device::~Device()
{
sem_destroy(&_lock);
if (_irq_attached)
if (_irq_attached) {
unregister_interrupt(_irq);
}
}
int
@@ -126,8 +127,9 @@ Device::init()
/* register */
ret = register_interrupt(_irq, this);
if (ret != OK)
if (ret != OK) {
goto out;
}
_irq_attached = true;
}
@@ -139,15 +141,17 @@ out:
void
Device::interrupt_enable()
{
if (_irq_attached)
if (_irq_attached) {
up_enable_irq(_irq);
}
}
void
Device::interrupt_disable()
{
if (_irq_attached)
if (_irq_attached) {
up_disable_irq(_irq);
}
}
void
@@ -224,6 +228,7 @@ Device::ioctl(unsigned operation, unsigned &arg)
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
}
src/drivers/device/device_nuttx.h
+25 -20
View File
@@ -118,7 +118,7 @@ public:
* @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);
/**
@@ -147,13 +147,13 @@ public:
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
};
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 {
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
@@ -189,14 +189,16 @@ protected:
*
* Note that we must loop as the wait may be interrupted by a signal.
*/
void lock() {
void lock()
{
do {} while (sem_wait(&_lock) != 0);
}
/**
* Release the driver lock.
*/
void unlock() {
void unlock()
{
sem_post(&_lock);
}
@@ -416,7 +418,7 @@ protected:
*/
virtual int close_last(file_t *filp);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -425,7 +427,7 @@ protected:
*/
virtual int register_class_devname(const char *class_devname);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -440,7 +442,7 @@ protected:
*
* @return the file system string of the device handle
*/
const char* get_devname() { return _devname; }
const char *get_devname() { return _devname; }
bool _pub_blocked; /**< true if publishing should be blocked */
@@ -470,8 +472,8 @@ private:
int remove_poll_waiter(struct pollfd *fds);
/* do not allow copying this class */
CDev(const CDev&);
CDev operator=(const CDev&);
CDev(const CDev &);
CDev operator=(const CDev &);
};
/**
@@ -503,7 +505,8 @@ protected:
*
* @param offset Register offset in bytes from the base address.
*/
uint32_t reg(uint32_t offset) {
uint32_t reg(uint32_t offset)
{
return *(volatile uint32_t *)(_base + offset);
}
@@ -513,7 +516,8 @@ protected:
* @param offset Register offset in bytes from the base address.
* @param value Value to write.
*/
void reg(uint32_t offset, uint32_t value) {
void reg(uint32_t offset, uint32_t value)
{
*(volatile uint32_t *)(_base + offset) = value;
}
@@ -527,7 +531,8 @@ protected:
* @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) {
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits)
{
uint32_t val = reg(offset);
val &= ~clearbits;
val |= setbits;
@@ -542,9 +547,9 @@ private:
// class instance for primary driver of each class
enum CLASS_DEVICE {
CLASS_DEVICE_PRIMARY=0,
CLASS_DEVICE_SECONDARY=1,
CLASS_DEVICE_TERTIARY=2
CLASS_DEVICE_PRIMARY = 0,
CLASS_DEVICE_SECONDARY = 1,
CLASS_DEVICE_TERTIARY = 2
};
#endif /* _DEVICE_DEVICE_H */
src/drivers/device/device_posix.cpp
+9 -8
View File
@@ -60,7 +60,7 @@ Device::Device(const char *name) :
if (ret != 0) {
PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno));
}
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid = 0;
@@ -86,23 +86,24 @@ Device::init()
int
Device::dev_read(unsigned offset, void *data, unsigned count)
{
return -ENODEV;
return -ENODEV;
}
int
Device::dev_write(unsigned offset, void *data, unsigned count)
{
return -ENODEV;
return -ENODEV;
}
int
Device::dev_ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
switch (operation) {
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
}
} // namespace device
src/drivers/device/i2c_nuttx.cpp
+18 -9
View File
@@ -69,7 +69,7 @@ I2C::I2C(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
I2C::~I2C()
@@ -92,6 +92,7 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
if (_bus_clocks[index] > 0) {
// DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
}
_bus_clocks[index] = clock_hz;
return OK;
@@ -122,7 +123,7 @@ I2C::init()
(void)up_i2cuninitialize(_dev);
_dev = nullptr;
DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",
_bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
_bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
ret = -EINVAL;
goto out;
}
@@ -162,13 +163,15 @@ I2C::init()
// tell the world where we are
DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",
_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
out:
if ((ret != OK) && (_dev != nullptr)) {
up_i2cuninitialize(_dev);
_dev = nullptr;
}
return ret;
}
@@ -208,18 +211,21 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
msgs++;
}
if (msgs == 0)
if (msgs == 0) {
return -EINVAL;
}
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
/* success */
if (ret == OK)
if (ret == OK) {
break;
}
/* if we have already retried once, or we are going to give up, then reset the bus */
if ((retry_count >= 1) || (retry_count >= _retries))
if ((retry_count >= 1) || (retry_count >= _retries)) {
up_i2creset(_dev);
}
} while (retry_count++ < _retries);
@@ -234,20 +240,23 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
unsigned retry_count = 0;
/* force the device address into the message vector */
for (unsigned i = 0; i < msgs; i++)
for (unsigned i = 0; i < msgs; i++) {
msgv[i].addr = _address;
}
do {
ret = I2C_TRANSFER(_dev, msgv, msgs);
/* success */
if (ret == OK)
if (ret == OK) {
break;
}
/* if we have already retried once, or we are going to give up, then reset the bus */
if ((retry_count >= 1) || (retry_count >= _retries))
if ((retry_count >= 1) || (retry_count >= _retries)) {
up_i2creset(_dev);
}
} while (retry_count++ < _retries);
src/drivers/device/i2c_nuttx.h
+2 -1
View File
@@ -134,7 +134,8 @@ protected:
*
* @param address The new bus address to set.
*/
void set_address(uint16_t address) {
void set_address(uint16_t address)
{
_address = address;
_device_id.devid_s.address = _address;
}
src/drivers/device/i2c_posix.cpp
+40 -28
View File
@@ -75,7 +75,7 @@ I2C::I2C(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
I2C::~I2C()
@@ -105,6 +105,7 @@ I2C::init()
}
_fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY);
if (_fd < 0) {
DEVICE_DEBUG("px4_open failed of device %s", get_devname());
return PX4_ERROR;
@@ -116,16 +117,18 @@ I2C::init()
if (simulate) {
_fd = 10000;
}
else {
} else {
#ifndef __PX4_QURT
// Open the actual I2C device and map to the virtual dev name
_fd = ::open(get_devname(), O_RDWR);
if (_fd < 0) {
warnx("could not open %s", get_devname());
px4_errno = errno;
return PX4_ERROR;
}
#endif
}
@@ -135,9 +138,9 @@ I2C::init()
int
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
{
#ifndef __PX4_LINUX
#ifndef __PX4_LINUX
return 1;
#else
#else
struct i2c_msg msgv[2];
unsigned msgs;
struct i2c_rdwr_ioctl_data packets;
@@ -145,7 +148,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
unsigned retry_count = 0;
if (_fd < 0) {
warnx("I2C device not opened");
warnx("I2C device not opened");
return 1;
}
@@ -169,8 +172,9 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
msgs++;
}
if (msgs == 0)
if (msgs == 0) {
return -EINVAL;
}
packets.msgs = msgv;
packets.nmsgs = msgs;
@@ -178,9 +182,10 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
if (simulate) {
//warnx("I2C SIM: transfer_4 on %s", get_devname());
ret = PX4_OK;
}
else {
} else {
ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets);
if (ret < 0) {
warnx("I2C transfer failed");
return 1;
@@ -188,28 +193,30 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
}
/* success */
if (ret == PX4_OK)
if (ret == PX4_OK) {
break;
}
} while (retry_count++ < _retries);
return ret;
#endif
#endif
}
int
I2C::transfer(struct i2c_msg *msgv, unsigned msgs)
{
#ifndef __PX4_LINUX
#ifndef __PX4_LINUX
return 1;
#else
#else
struct i2c_rdwr_ioctl_data packets;
int ret;
unsigned retry_count = 0;
/* force the device address into the message vector */
for (unsigned i = 0; i < msgs; i++)
for (unsigned i = 0; i < msgs; i++) {
msgv[i].addr = _address;
}
do {
packets.msgs = msgv;
@@ -218,34 +225,38 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs)
if (simulate) {
warnx("I2C SIM: transfer_2 on %s", get_devname());
ret = PX4_OK;
}
else {
} else {
ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets);
}
if (ret < 0) {
warnx("I2C transfer failed");
return 1;
}
warnx("I2C transfer failed");
return 1;
}
/* success */
if (ret == PX4_OK)
if (ret == PX4_OK) {
break;
}
} while (retry_count++ < _retries);
return ret;
#endif
#endif
}
int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
//struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg;
switch (cmd) {
#ifdef __PX4_LINUX
#ifdef __PX4_LINUX
case I2C_RDWR:
warnx("Use I2C::transfer, not ioctl");
warnx("Use I2C::transfer, not ioctl");
return 0;
#endif
#endif
default:
/* give it to the superclass */
return VDev::ioctl(filp, cmd, arg);
@@ -256,27 +267,28 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen)
{
if (simulate) {
// FIXME no idea what this should be
warnx ("2C SIM I2C::read");
warnx("2C SIM I2C::read");
return 0;
}
#ifndef __PX4_QURT
return ::read(_fd, buffer, buflen);
#else
return 0;
return 0;
#endif
}
ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen)
{
if (simulate) {
warnx ("2C SIM I2C::write");
warnx("2C SIM I2C::write");
return buflen;
}
#ifndef __PX4_QURT
return ::write(_fd, buffer, buflen);
#else
return buflen;
return buflen;
#endif
}
src/drivers/device/i2c_posix.h
+2 -2
View File
@@ -127,8 +127,8 @@ private:
int _fd;
std::string _dname;
I2C(const device::I2C&);
I2C operator=(const device::I2C&);
I2C(const device::I2C &);
I2C operator=(const device::I2C &);
};
} // namespace device
src/drivers/device/integrator.cpp
+2 -1
View File
@@ -82,7 +82,7 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
// following:
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f;
}
@@ -117,6 +117,7 @@ math::Vector<3>
Integrator::read(bool auto_reset)
{
math::Vector<3> val = _integral_read;
if (auto_reset) {
_integral_read(0) = 0.0f;
_integral_read(1) = 0.0f;
src/drivers/device/integrator.h
+4 -3
View File
@@ -43,7 +43,8 @@
#include <mathlib/mathlib.h>
class Integrator {
class Integrator
{
public:
Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
virtual ~Integrator();
@@ -91,6 +92,6 @@ private:
bool _coning_comp_on; /**< coning compensation */
/* we don't want this class to be copied */
Integrator(const Integrator&);
Integrator operator=(const Integrator&);
Integrator(const Integrator &);
Integrator operator=(const Integrator &);
};
src/drivers/device/ringbuffer.cpp
+25 -10
View File
@@ -46,15 +46,16 @@ namespace ringbuffer
RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
_num_items(num_items),
_item_size(item_size),
_buf(new char[(_num_items+1) * item_size]),
_head(_num_items),
_tail(_num_items)
_buf(new char[(_num_items + 1) * item_size]),
_head(_num_items),
_tail(_num_items)
{}
RingBuffer::~RingBuffer()
{
if (_buf != nullptr)
if (_buf != nullptr) {
delete[] _buf;
}
}
unsigned
@@ -84,20 +85,25 @@ RingBuffer::size()
void
RingBuffer::flush()
{
while (!empty())
while (!empty()) {
get(NULL);
}
}
bool
RingBuffer::put(const void *val, size_t val_size)
{
unsigned next = _next(_head);
if (next != _tail) {
if ((val_size == 0) || (val_size > _item_size))
if ((val_size == 0) || (val_size > _item_size)) {
val_size = _item_size;
}
memcpy(&_buf[_head * _item_size], val, val_size);
_head = next;
return true;
} else {
return false;
}
@@ -169,11 +175,14 @@ RingBuffer::force(const void *val, size_t val_size)
bool overwrote = false;
for (;;) {
if (put(val, val_size))
if (put(val, val_size)) {
break;
}
get(NULL);
overwrote = true;
}
return overwrote;
}
@@ -246,6 +255,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned
*a = c;
return true;
}
return false;
}
@@ -259,8 +269,9 @@ RingBuffer::get(void *val, size_t val_size)
unsigned candidate;
unsigned next;
if ((val_size == 0) || (val_size > _item_size))
if ((val_size == 0) || (val_size > _item_size)) {
val_size = _item_size;
}
do {
/* decide which element we think we're going to read */
@@ -270,13 +281,15 @@ RingBuffer::get(void *val, size_t val_size)
next = _next(candidate);
/* go ahead and read from this index */
if (val != NULL)
if (val != NULL) {
memcpy(val, &_buf[candidate * _item_size], val_size);
}
/* if the tail pointer didn't change, we got our item */
} while (!__PX4_SBCAP(&_tail, candidate, next));
return true;
} else {
return false;
}
@@ -377,10 +390,12 @@ bool
RingBuffer::resize(unsigned new_size)
{
char *old_buffer;
char *new_buffer = new char [(new_size+1) * _item_size];
char *new_buffer = new char [(new_size + 1) * _item_size];
if (new_buffer == nullptr) {
return false;
}
old_buffer = _buf;
_buf = new_buffer;
_num_items = new_size;
src/drivers/device/ringbuffer.h
+4 -3
View File
@@ -46,7 +46,8 @@
namespace ringbuffer __EXPORT
{
class RingBuffer {
class RingBuffer
{
public:
RingBuffer(unsigned ring_size, size_t entry_size);
virtual ~RingBuffer();
@@ -171,8 +172,8 @@ private:
unsigned _next(unsigned index);
/* we don't want this class to be copied */
RingBuffer(const RingBuffer&);
RingBuffer operator=(const RingBuffer&);
RingBuffer(const RingBuffer &);
RingBuffer operator=(const RingBuffer &);
};
} // namespace ringbuffer
src/drivers/device/sim.cpp
+2 -2
View File
@@ -69,7 +69,7 @@ SIM::SIM(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
SIM::~SIM()
@@ -104,7 +104,7 @@ SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
if (recv_len > 0) {
PX4_DEBUG("SIM: receiving %d bytes", recv_len);
// TODO - write data to recv;
}
src/drivers/device/sim.h
+5 -5
View File
@@ -37,7 +37,7 @@
* Base class for devices on simulation bus.
*/
#pragma once
#pragma once
#include "vdev.h"
@@ -98,14 +98,14 @@ protected:
* otherwise.
*/
virtual int transfer(const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
uint8_t *recv, unsigned recv_len);
private:
uint16_t _address;
const char * _devname;
const char *_devname;
SIM(const device::SIM&);
SIM operator=(const device::SIM&);
SIM(const device::SIM &);
SIM operator=(const device::SIM &);
};
} // namespace device
src/drivers/device/spi.cpp
+10 -6
View File
@@ -80,7 +80,7 @@ SPI::SPI(const char *name,
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = (uint8_t)device;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
_device_id.devid_s.devtype = 0;
}
SPI::~SPI()
@@ -94,8 +94,9 @@ SPI::init()
int ret = OK;
/* attach to the spi bus */
if (_dev == nullptr)
if (_dev == nullptr) {
_dev = up_spiinitialize(_bus);
}
if (_dev == nullptr) {
DEVICE_DEBUG("failed to init SPI");
@@ -141,34 +142,37 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
int result;
if ((send == nullptr) && (recv == nullptr))
if ((send == nullptr) && (recv == nullptr)) {
return -EINVAL;
}
LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
/* lock the bus as required */
switch (mode) {
default:
case LOCK_PREEMPTION:
{
case LOCK_PREEMPTION: {
irqstate_t state = irqsave();
result = _transfer(send, recv, len);
irqrestore(state);
}
break;
case LOCK_THREADS:
SPI_LOCK(_dev, true);
result = _transfer(send, recv, len);
SPI_LOCK(_dev, false);
break;
case LOCK_NONE:
result = _transfer(send, recv, len);
break;
}
return result;
}
void
void
SPI::set_frequency(uint32_t frequency)
{
_frequency = frequency;
src/drivers/device/spi.h
+4 -4
View File
@@ -109,12 +109,12 @@ protected:
* Set the SPI bus frequency
* This is used to change frequency on the fly. Some sensors
* (such as the MPU6000) need a lower frequency for setup
* registers and can handle higher frequency for sensor
* registers and can handle higher frequency for sensor
* value registers
*
* @param frequency Frequency to set (Hz)
*/
void set_frequency(uint32_t frequency);
void set_frequency(uint32_t frequency);
/**
* Locking modes supported by the driver.
@@ -134,8 +134,8 @@ private:
struct spi_dev_s *_dev;
/* this class does not allow copying */
SPI(const SPI&);
SPI operator=(const SPI&);
SPI(const SPI &);
SPI operator=(const SPI &);
protected:
int _bus;
src/drivers/device/vdev.cpp
+75 -36
View File
@@ -54,8 +54,9 @@ struct px4_dev_t {
char *name;
void *cdev;
px4_dev_t(const char *n, void *c) : cdev(c) {
name = strdup(n);
px4_dev_t(const char *n, void *c) : cdev(c)
{
name = strdup(n);
}
~px4_dev_t() { free(name); }
@@ -85,21 +86,26 @@ VDev::VDev(const char *name,
_open_count(0)
{
PX4_DEBUG("VDev::VDev");
for (unsigned i = 0; i < _max_pollwaiters; i++)
for (unsigned i = 0; i < _max_pollwaiters; i++) {
_pollset[i] = nullptr;
}
}
VDev::~VDev()
{
PX4_DEBUG("VDev::~VDev");
if (_registered)
if (_registered) {
unregister_driver(_devname);
}
}
int
VDev::register_class_devname(const char *class_devname)
{
PX4_DEBUG("VDev::register_class_devname %s", class_devname);
if (class_devname == nullptr) {
return -EINVAL;
}
@@ -111,13 +117,16 @@ VDev::register_class_devname(const char *class_devname)
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, (void *)this);
if (ret == OK) break;
if (ret == OK) { break; }
class_instance++;
}
if (class_instance == 4) {
return ret;
}
return class_instance;
}
@@ -127,17 +136,19 @@ VDev::register_driver(const char *name, void *data)
PX4_DEBUG("VDev::register_driver %s", name);
int ret = -ENOSPC;
if (name == NULL || data == NULL)
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)) {
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) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] == NULL) {
devmap[i] = new px4_dev_t(name, (void *)data);
PX4_DEBUG("Registered DEV %s", name);
@@ -145,9 +156,11 @@ VDev::register_driver(const char *name, void *data)
break;
}
}
if (ret != PX4_OK) {
PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
}
return ret;
}
@@ -157,10 +170,11 @@ VDev::unregister_driver(const char *name)
PX4_DEBUG("VDev::unregister_driver %s", name);
int ret = -EINVAL;
if (name == NULL)
if (name == NULL) {
return -EINVAL;
}
for (int i=0;i<PX4_MAX_DEV; ++i) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
delete devmap[i];
devmap[i] = NULL;
@@ -169,6 +183,7 @@ VDev::unregister_driver(const char *name)
break;
}
}
return ret;
}
@@ -178,14 +193,16 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
PX4_DEBUG("VDev::unregister_class_devname");
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) {
for (int i = 0; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
delete devmap[i];
PX4_DEBUG("Unregistered class DEV %s", name);
devmap[i] = NULL;
return PX4_OK;
}
}
return -EINVAL;
}
@@ -197,15 +214,17 @@ VDev::init()
// base class init first
int ret = Device::init();
if (ret != PX4_OK)
if (ret != PX4_OK) {
goto out;
}
// now register the driver
if (_devname != nullptr) {
ret = register_driver(_devname, (void *)this);
if (ret != PX4_OK)
if (ret != PX4_OK) {
goto out;
}
_registered = true;
}
@@ -232,8 +251,9 @@ VDev::open(file_t *filep)
/* first-open callback may decline the open */
ret = open_first(filep);
if (ret != PX4_OK)
if (ret != PX4_OK) {
_open_count--;
}
}
unlock();
@@ -261,8 +281,9 @@ VDev::close(file_t *filep)
_open_count--;
/* callback cannot decline the close */
if (_open_count == 0)
if (_open_count == 0) {
ret = close_last(filep);
}
} else {
ret = -EBADF;
@@ -309,22 +330,26 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg)
switch (cmd) {
/* fetch a pointer to the driver's private data */
/* fetch a pointer to the driver's private data */
case DIOC_GETPRIV:
*(void **)(uintptr_t)arg = (void *)this;
ret = PX4_OK;
break;
case DEVIOCSPUBBLOCK:
_pub_blocked = (arg != 0);
ret = PX4_OK;
break;
case DEVIOCGPUBBLOCK:
ret = _pub_blocked;
break;
case DEVIOCGDEVICEID:
ret = (int)_device_id.devid;
case DEVIOCGDEVICEID:
ret = (int)_device_id.devid;
PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret);
break;
default:
break;
}
@@ -365,8 +390,10 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
fds->revents |= fds->events & poll_state(filep);
/* yes? post the notification */
if (fds->revents != 0)
if (fds->revents != 0) {
px4_sem_post(fds->sem);
}
} else {
PX4_WARN("Store Poll Waiter error.");
}
@@ -392,8 +419,9 @@ VDev::poll_notify(pollevent_t events)
lock();
for (unsigned i = 0; i < _max_pollwaiters; i++)
if (nullptr != _pollset[i])
if (nullptr != _pollset[i]) {
poll_notify_one(_pollset[i], events);
}
unlock();
}
@@ -408,7 +436,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
/* update the reported event set */
fds->revents |= fds->events & events;
PX4_DEBUG(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value);
PX4_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value);
/* if the state is now interesting, wake the waiter if it's still asleep */
/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
@@ -432,6 +460,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds)
* Look for a free slot.
*/
PX4_DEBUG("VDev::store_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (nullptr == _pollset[i]) {
@@ -449,6 +478,7 @@ int
VDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
{
PX4_DEBUG("VDev::remove_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (fds == _pollset[i]) {
@@ -465,8 +495,9 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
VDev *VDev::getDev(const char *path)
{
PX4_DEBUG("VDev::getDev");
int i=0;
for (; i<PX4_MAX_DEV; ++i) {
int i = 0;
for (; i < PX4_MAX_DEV; ++i) {
//if (devmap[i]) {
// printf("%s %s\n", devmap[i]->name, path);
//}
@@ -474,14 +505,16 @@ VDev *VDev::getDev(const char *path)
return (VDev *)(devmap[i]->cdev);
}
}
return NULL;
}
void VDev::showDevices()
{
int i=0;
int i = 0;
PX4_INFO("Devices:");
for (; i<PX4_MAX_DEV; ++i) {
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
@@ -490,9 +523,10 @@ void VDev::showDevices()
void VDev::showTopics()
{
int i=0;
int i = 0;
PX4_INFO("Devices:");
for (; i<PX4_MAX_DEV; ++i) {
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name);
}
@@ -501,11 +535,12 @@ void VDev::showTopics()
void VDev::showFiles()
{
int i=0;
int i = 0;
PX4_INFO("Files:");
for (; i<PX4_MAX_DEV; ++i) {
for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
strncmp(devmap[i]->name, "/dev/", 5) != 0) {
strncmp(devmap[i]->name, "/dev/", 5) != 0) {
PX4_INFO(" %s", devmap[i]->name);
}
}
@@ -513,17 +548,21 @@ void VDev::showFiles()
const char *VDev::topicList(unsigned int *next)
{
for (;*next<PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0)
for (; *next < PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0) {
return devmap[(*next)++]->name;
}
return NULL;
}
const char *VDev::devList(unsigned int *next)
{
for (;*next<PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0)
for (; *next < PX4_MAX_DEV; (*next)++)
if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0) {
return devmap[(*next)++]->name;
}
return NULL;
}
src/drivers/device/vdev.h
+34 -29
View File
@@ -37,7 +37,7 @@
* Definitions for the generic base classes in the virtual device framework.
*/
#pragma once
#pragma once
/*
* Includes here should only cover the needs of the framework definitions.
@@ -118,17 +118,17 @@ public:
* @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 dev_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 dev_ioctl(unsigned operation, unsigned &arg);
/**
* 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 dev_ioctl(unsigned operation, unsigned &arg);
/*
device bus types for DEVID
@@ -148,13 +148,13 @@ public:
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
};
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 {
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
@@ -179,7 +179,8 @@ protected:
*
* Note that we must loop as the wait may be interrupted by a signal.
*/
void lock() {
void lock()
{
DEVICE_DEBUG("lock");
do {} while (px4_sem_wait(&_lock) != 0);
}
@@ -187,7 +188,8 @@ protected:
/**
* Release the driver lock.
*/
void unlock() {
void unlock()
{
DEVICE_DEBUG("unlock");
px4_sem_post(&_lock);
}
@@ -330,7 +332,7 @@ public:
*
* @return the file system string of the device handle
*/
const char* get_devname() { return _devname; }
const char *get_devname() { return _devname; }
protected:
@@ -395,7 +397,7 @@ protected:
*/
virtual int close_last(file_t *filep);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -404,7 +406,7 @@ protected:
*/
virtual int register_class_devname(const char *class_devname);
/**
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
@@ -442,7 +444,7 @@ private:
int remove_poll_waiter(px4_pollfd_struct_t *fds);
/* do not allow copying this class */
VDev(const VDev&);
VDev(const VDev &);
//VDev operator=(const VDev&);
};
@@ -464,7 +466,7 @@ public:
PIO(const char *name,
const char *devname,
unsigned long base
);
);
virtual ~PIO();
virtual int init();
@@ -476,7 +478,8 @@ protected:
*
* @param offset Register offset in bytes from the base address.
*/
uint32_t reg(uint32_t offset) {
uint32_t reg(uint32_t offset)
{
return *(volatile uint32_t *)(_base + offset);
}
@@ -486,7 +489,8 @@ protected:
* @param offset Register offset in bytes from the base address.
* @param value Value to write.
*/
void reg(uint32_t offset, uint32_t value) {
void reg(uint32_t offset, uint32_t value)
{
*(volatile uint32_t *)(_base + offset) = value;
}
@@ -500,7 +504,8 @@ protected:
* @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) {
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits)
{
uint32_t val = reg(offset);
val &= ~clearbits;
val |= setbits;
@@ -516,8 +521,8 @@ private:
// class instance for primary driver of each class
enum CLASS_DEVICE {
CLASS_DEVICE_PRIMARY=0,
CLASS_DEVICE_SECONDARY=1,
CLASS_DEVICE_TERTIARY=2
CLASS_DEVICE_PRIMARY = 0,
CLASS_DEVICE_SECONDARY = 1,
CLASS_DEVICE_TERTIARY = 2
};
src/drivers/device/vdev_posix.cpp
+236 -225
View File
@@ -54,254 +54,265 @@ using namespace device;
extern "C" {
static void timer_cb(void *data)
{
px4_sem_t *p_sem = (px4_sem_t *)data;
px4_sem_post(p_sem);
PX4_DEBUG("timer_handler: Timer expired");
}
static void timer_cb(void *data)
{
px4_sem_t *p_sem = (px4_sem_t *)data;
px4_sem_post(p_sem);
PX4_DEBUG("timer_handler: Timer expired");
}
#define PX4_MAX_FD 200
static device::file_t *filemap[PX4_MAX_FD] = {};
static device::file_t *filemap[PX4_MAX_FD] = {};
int px4_errno;
int px4_errno;
inline bool valid_fd(int fd)
{
return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
}
int px4_open(const char *path, int flags, ...)
{
PX4_DEBUG("px4_open");
VDev *dev = VDev::getDev(path);
int ret = 0;
int i;
mode_t mode;
if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0 &&
strncmp(path, "/obj/", 5) != 0 &&
strncmp(path, "/dev/", 5) != 0)
inline bool valid_fd(int fd)
{
va_list p;
va_start(p, flags);
mode = va_arg(p, mode_t);
va_end(p);
return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
}
// Create the file
PX4_DEBUG("Creating virtual file %s", path);
dev = VFile::createFile(path, mode);
}
if (dev) {
for (i=0; i<PX4_MAX_FD; ++i) {
if (filemap[i] == 0) {
filemap[i] = new device::file_t(flags,dev,i);
break;
}
}
if (i < PX4_MAX_FD) {
ret = dev->open(filemap[i]);
}
else {
PX4_WARN("exceeded maximum number of file descriptors!");
ret = -ENOENT;
}
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
return -1;
}
PX4_DEBUG("px4_open fd = %d", filemap[i]->fd);
return filemap[i]->fd;
}
int px4_close(int fd)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_close fd = %d", fd);
ret = dev->close(filemap[fd]);
filemap[fd] = NULL;
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_read(int fd, void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_read fd = %d", fd);
ret = dev->read(filemap[fd], (char *)buffer, buflen);
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_write(int fd, const void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_write fd = %d", fd);
ret = dev->write(filemap[fd], (const char *)buffer, buflen);
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
int px4_ioctl(int fd, int cmd, unsigned long arg)
{
PX4_DEBUG("px4_ioctl fd = %d", fd);
int ret = 0;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
ret = dev->ioctl(filemap[fd], cmd, arg);
}
else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
}
return ret;
}
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
px4_sem_t sem;
int count = 0;
int ret = -1;
unsigned int i;
PX4_DEBUG("Called px4_poll timeout = %d", timeout);
px4_sem_init(&sem, 0, 0);
// For each fd
for (i=0; i<nfds; ++i)
int px4_open(const char *path, int flags, ...)
{
fds[i].sem = &sem;
fds[i].revents = 0;
fds[i].priv = NULL;
PX4_DEBUG("px4_open");
VDev *dev = VDev::getDev(path);
int ret = 0;
int i;
mode_t mode;
// If fd is valid
if (valid_fd(fds[i].fd))
{
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 &&
strncmp(path, "/obj/", 5) != 0 &&
strncmp(path, "/dev/", 5) != 0) {
va_list p;
va_start(p, flags);
mode = va_arg(p, mode_t);
va_end(p);
if (ret < 0)
break;
}
}
if (ret >= 0)
{
if (timeout > 0)
{
// Use a work queue task
work_s _hpwork;
hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout);
px4_sem_wait(&sem);
// Make sure timer thread is killed before sem goes
// out of scope
hrt_work_cancel(&_hpwork);
}
else if (timeout < 0)
{
px4_sem_wait(&sem);
// Create the file
PX4_DEBUG("Creating virtual file %s", path);
dev = VFile::createFile(path, mode);
}
// For each fd
for (i=0; i<nfds; ++i)
{
// If fd is valid
if (valid_fd(fds[i].fd))
{
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
if (ret < 0)
if (dev) {
for (i = 0; i < PX4_MAX_FD; ++i) {
if (filemap[i] == 0) {
filemap[i] = new device::file_t(flags, dev, i);
break;
}
}
if (fds[i].revents)
count += 1;
if (i < PX4_MAX_FD) {
ret = dev->open(filemap[i]);
} else {
PX4_WARN("exceeded maximum number of file descriptors!");
ret = -ENOENT;
}
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
return -1;
}
PX4_DEBUG("px4_open fd = %d", filemap[i]->fd);
return filemap[i]->fd;
}
int px4_close(int fd)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_close fd = %d", fd);
ret = dev->close(filemap[fd]);
filemap[fd] = NULL;
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_read(int fd, void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_read fd = %d", fd);
ret = dev->read(filemap[fd], (char *)buffer, buflen);
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
ssize_t px4_write(int fd, const void *buffer, size_t buflen)
{
int ret;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
PX4_DEBUG("px4_write fd = %d", fd);
ret = dev->write(filemap[fd], (const char *)buffer, buflen);
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
ret = PX4_ERROR;
}
return ret;
}
int px4_ioctl(int fd, int cmd, unsigned long arg)
{
PX4_DEBUG("px4_ioctl fd = %d", fd);
int ret = 0;
if (valid_fd(fd)) {
VDev *dev = (VDev *)(filemap[fd]->vdev);
ret = dev->ioctl(filemap[fd], cmd, arg);
} else {
ret = -EINVAL;
}
if (ret < 0) {
px4_errno = -ret;
}
return ret;
}
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
px4_sem_t sem;
int count = 0;
int ret = -1;
unsigned int i;
PX4_DEBUG("Called px4_poll timeout = %d", timeout);
px4_sem_init(&sem, 0, 0);
// For each fd
for (i = 0; i < nfds; ++i) {
fds[i].sem = &sem;
fds[i].revents = 0;
fds[i].priv = NULL;
// If fd is valid
if (valid_fd(fds[i].fd)) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
if (ret < 0) {
break;
}
}
}
if (ret >= 0) {
if (timeout > 0) {
// Use a work queue task
work_s _hpwork;
hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout);
px4_sem_wait(&sem);
// Make sure timer thread is killed before sem goes
// out of scope
hrt_work_cancel(&_hpwork);
} else if (timeout < 0) {
px4_sem_wait(&sem);
}
// For each fd
for (i = 0; i < nfds; ++i) {
// If fd is valid
if (valid_fd(fds[i].fd)) {
VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
if (ret < 0) {
break;
}
if (fds[i].revents) {
count += 1;
}
}
}
}
px4_sem_destroy(&sem);
return count;
}
px4_sem_destroy(&sem);
return count;
}
int px4_fsync(int fd)
{
return 0;
}
int px4_access(const char *pathname, int mode)
{
if (mode != F_OK) {
errno = EINVAL;
return -1;
int px4_fsync(int fd)
{
return 0;
}
VDev *dev = VDev::getDev(pathname);
return (dev != nullptr) ? 0 : -1;
}
void px4_show_devices()
{
VDev::showDevices();
}
int px4_access(const char *pathname, int mode)
{
if (mode != F_OK) {
errno = EINVAL;
return -1;
}
void px4_show_topics()
{
VDev::showTopics();
}
VDev *dev = VDev::getDev(pathname);
return (dev != nullptr) ? 0 : -1;
}
void px4_show_files()
{
VDev::showFiles();
}
void px4_show_devices()
{
VDev::showDevices();
}
const char * px4_get_device_names(unsigned int *handle)
{
return VDev::devList(handle);
}
void px4_show_topics()
{
VDev::showTopics();
}
const char * px4_get_topic_names(unsigned int *handle)
{
return VDev::topicList(handle);
}
void px4_show_files()
{
VDev::showFiles();
}
const char *px4_get_device_names(unsigned int *handle)
{
return VDev::devList(handle);
}
const char *px4_get_topic_names(unsigned int *handle)
{
return VDev::topicList(handle);
}
}
src/drivers/device/vfile.cpp
+1 -1
View File
@@ -48,7 +48,7 @@ VFile::VFile(const char *fname, mode_t mode) :
{
}
VFile * VFile::createFile(const char *fname, mode_t mode)
VFile *VFile::createFile(const char *fname, mode_t mode)
{
VFile *me = new VFile(fname, mode);
me->register_driver(fname, me);
src/drivers/device/vfile.h
+2 -1
View File
@@ -44,7 +44,8 @@
#include <unistd.h>
#include <stdio.h>
class VFile : public device::VDev {
class VFile : public device::VDev
{
public:
static VFile *createFile(const char *fname, mode_t mode);