aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/px4io/px4io.cpp37
-rw-r--r--src/drivers/px4io/px4io_i2c.cpp94
-rw-r--r--src/drivers/px4io/px4io_serial.cpp147
3 files changed, 134 insertions, 144 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 29624018f..1b4f20de0 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -85,7 +85,9 @@
#include <modules/px4iofirmware/protocol.h>
#include "uploader.h"
-#include "interface.h"
+
+extern device::Device *PX4IO_i2c_interface() weak_function;
+extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
@@ -93,7 +95,7 @@
class PX4IO : public device::CDev
{
public:
- PX4IO(PX4IO_interface *interface);
+ PX4IO(device::Device *interface);
virtual ~PX4IO();
virtual int init();
@@ -131,7 +133,7 @@ public:
void print_status();
private:
- PX4IO_interface *_interface;
+ device::Device *_interface;
// XXX
unsigned _hardware;
@@ -316,7 +318,7 @@ PX4IO *g_dev;
}
-PX4IO::PX4IO(PX4IO_interface *interface) :
+PX4IO::PX4IO(device::Device *interface) :
CDev("px4io", "/dev/px4io"),
_interface(interface),
_hardware(0),
@@ -1129,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
return -EINVAL;
}
- int ret = _interface->set_reg(page, offset, values, num_values);
+ int ret = _interface->write((page << 8) | offset, (void *)values, num_values);
if (ret != OK)
debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno);
return ret;
@@ -1150,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
return -EINVAL;
}
- int ret = _interface->get_reg(page, offset, values, num_values);
+ int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
if (ret != OK)
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno);
return ret;
@@ -1602,12 +1604,12 @@ start(int argc, char *argv[])
if (g_dev != nullptr)
errx(1, "already loaded");
- PX4IO_interface *interface;
+ device::Device *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
- interface = io_serial_interface();
+ interface = PX4IO_serial_interface();
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
- interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+ interface = PX4IO_i2c_interface();
#else
# error Unknown board - cannot select interface.
#endif
@@ -1615,7 +1617,7 @@ start(int argc, char *argv[])
if (interface == nullptr)
errx(1, "cannot alloc interface");
- if (!interface->ok()) {
+ if (interface->probe()) {
delete interface;
errx(1, "interface init failed");
}
@@ -1739,12 +1741,12 @@ monitor(void)
void
if_test(unsigned mode)
{
- PX4IO_interface *interface;
+ device::Device *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
- interface = io_serial_interface();
+ interface = PX4IO_serial_interface();
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
- interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+ interface = PX4IO_i2c_interface();
#else
# error Unknown board - cannot select interface.
#endif
@@ -1752,20 +1754,17 @@ if_test(unsigned mode)
if (interface == nullptr)
errx(1, "cannot alloc interface");
- if (!interface->ok()) {
+ if (interface->probe()) {
delete interface;
errx(1, "interface init failed");
} else {
-
- int result = interface->test(mode);
+ int result = interface->ioctl(1, mode); /* XXX magic numbers */
delete interface;
errx(0, "test returned %d", result);
}
-
- exit(0);
}
-}
+} /* namespace */
int
px4io_main(int argc, char *argv[])
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
index 45a707883..317326e40 100644
--- a/src/drivers/px4io/px4io_i2c.cpp
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -52,109 +52,93 @@
#include <nuttx/i2c.h>
+#include <drivers/device/i2c.h>
#include <mavlink/mavlink_log.h>
#include "uploader.h"
#include <modules/px4iofirmware/protocol.h>
-#include "interface.h"
+device::Device *PX4IO_i2c_interface();
-class PX4IO_I2C : public PX4IO_interface
+class PX4IO_I2C : public device::I2C
{
public:
PX4IO_I2C(int bus, uint8_t address);
virtual ~PX4IO_I2C();
- virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
- virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
-
- virtual bool ok();
-
- virtual int test(unsigned mode);
+ virtual int probe();
+ virtual int read(unsigned offset, void *data, unsigned count = 1);
+ virtual int write(unsigned address, void *data, unsigned count = 1);
+ virtual int ioctl(unsigned operation, unsigned &arg);
private:
- static const unsigned _retries = 2;
- struct i2c_dev_s *_dev;
- uint8_t _address;
};
-PX4IO_interface *io_i2c_interface(int bus, uint8_t address)
+device::Device
+*PX4IO_i2c_interface()
{
- return new PX4IO_I2C(bus, address);
+#ifdef PX4_I2C_OBDEV_PX4IO
+ return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+#endif
+ return nullptr;
}
PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
- _dev(nullptr),
- _address(address)
+ I2C("PX4IO_i2c", nullptr, bus, address, 400000)
{
- _dev = up_i2cinitialize(bus);
- if (_dev)
- I2C_SETFREQUENCY(_dev, 400000);
+ _retries = 3;
}
PX4IO_I2C::~PX4IO_I2C()
{
- if (_dev)
- up_i2cuninitialize(_dev);
}
-bool
-PX4IO_I2C::ok()
+int
+PX4IO_I2C::probe()
{
- if (!_dev)
- return false;
-
- /* check any other status here */
+ /* XXX really should do something here */
- return true;
+ return 0;
}
int
-PX4IO_I2C::test(unsigned mode)
+PX4IO_I2C::ioctl(unsigned operation, unsigned &arg)
{
return 0;
}
int
-PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+PX4IO_I2C::write(unsigned address, void *data, unsigned count)
{
- int ret;
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
/* set up the transfer */
uint8_t addr[2] = {
page,
offset
};
+
i2c_msg_s msgv[2];
- msgv[0].addr = _address;
msgv[0].flags = 0;
msgv[0].buffer = addr;
msgv[0].length = 2;
- msgv[1].addr = _address;
msgv[1].flags = I2C_M_NORESTART;
msgv[1].buffer = (uint8_t *)values;
- msgv[1].length = num_values * sizeof(*values);
-
- unsigned tries = 0;
- do {
+ msgv[1].length = 2 * count;
- /* perform the transfer */
- ret = I2C_TRANSFER(_dev, msgv, 2);
-
- if (ret == OK)
- break;
-
- } while (tries++ < _retries);
-
- return ret;
+ return transfer(msgv, 2);
}
int
-PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+PX4IO_I2C::read(unsigned address, void *data, unsigned count)
{
- int ret;
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
/* set up the transfer */
uint8_t addr[2] = {
@@ -163,25 +147,13 @@ PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_
};
i2c_msg_s msgv[2];
- msgv[0].addr = _address;
msgv[0].flags = 0;
msgv[0].buffer = addr;
msgv[0].length = 2;
- msgv[1].addr = _address;
msgv[1].flags = I2C_M_READ;
msgv[1].buffer = (uint8_t *)values;
- msgv[1].length = num_values * sizeof(*values);
-
- unsigned tries = 0;
- do {
- /* perform the transfer */
- ret = I2C_TRANSFER(_dev, msgv, 2);
-
- if (ret == OK)
- break;
-
- } while (tries++ < _retries);
+ msgv[1].length = 2 * count;
- return ret;
+ return transfer(msgv, 2);
}
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 06b955971..c48b6ec4c 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -59,6 +59,7 @@
#include <debug.h>
+#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <drivers/boards/px4fmuv2/px4fmu_internal.h> /* XXX should really not be hardcoding v2 here */
@@ -66,7 +67,7 @@
#include <modules/px4iofirmware/protocol.h>
-#include "interface.h"
+device::Device *PX4IO_serial_interface();
/* serial register accessors */
#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
@@ -78,17 +79,16 @@
#define rCR3 REG(STM32_USART_CR3_OFFSET)
#define rGTPR REG(STM32_USART_GTPR_OFFSET)
-class PX4IO_serial : public PX4IO_interface
+class PX4IO_serial : public device::Device
{
public:
PX4IO_serial();
virtual ~PX4IO_serial();
- virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
- virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
-
- virtual bool ok();
- virtual int test(unsigned mode);
+ virtual int probe();
+ virtual int read(unsigned offset, void *data, unsigned count = 1);
+ virtual int write(unsigned address, void *data, unsigned count = 1);
+ virtual int ioctl(unsigned operation, unsigned &arg);
private:
/*
@@ -159,12 +159,14 @@ private:
IOPacket PX4IO_serial::_dma_buffer;
static PX4IO_serial *g_interface;
-PX4IO_interface *io_serial_interface()
+device::Device
+*PX4IO_serial_interface()
{
return new PX4IO_serial();
}
PX4IO_serial::PX4IO_serial() :
+ Device("PX4IO_serial"),
_tx_dma(nullptr),
_rx_dma(nullptr),
_rx_dma_status(_dma_status_inactive),
@@ -262,74 +264,87 @@ PX4IO_serial::~PX4IO_serial()
g_interface = nullptr;
}
-bool
-PX4IO_serial::ok()
+int
+PX4IO_serial::probe()
{
+ /* XXX this could try talking to IO */
+
if (_tx_dma == nullptr)
- return false;
+ return -1;
if (_rx_dma == nullptr)
- return false;
+ return -1;
- return true;
+ return 0;
}
int
-PX4IO_serial::test(unsigned mode)
+PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
{
- switch (mode) {
- case 0:
- lowsyslog("test 0\n");
+ switch (operation) {
- /* kill DMA, this is a PIO test */
- stm32_dmastop(_tx_dma);
- stm32_dmastop(_rx_dma);
- rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
+ case 1: /* XXX magic number - test operation */
+ switch (arg) {
+ case 0:
+ lowsyslog("test 0\n");
- for (;;) {
- while (!(rSR & USART_SR_TXE))
- ;
- rDR = 0x55;
- }
- return 0;
-
- case 1:
- {
- unsigned fails = 0;
- for (unsigned count = 0;; count++) {
- uint16_t value = count & 0xffff;
-
- if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0)
- fails++;
-
- if (count >= 5000) {
- lowsyslog("==== test 1 : %u failures ====\n", fails);
- perf_print_counter(_pc_txns);
- perf_print_counter(_pc_dmasetup);
- perf_print_counter(_pc_retries);
- perf_print_counter(_pc_timeouts);
- perf_print_counter(_pc_crcerrs);
- perf_print_counter(_pc_dmaerrs);
- perf_print_counter(_pc_protoerrs);
- perf_print_counter(_pc_uerrs);
- perf_print_counter(_pc_idle);
- perf_print_counter(_pc_badidle);
- count = 0;
+ /* kill DMA, this is a PIO test */
+ stm32_dmastop(_tx_dma);
+ stm32_dmastop(_rx_dma);
+ rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
+
+ for (;;) {
+ while (!(rSR & USART_SR_TXE))
+ ;
+ rDR = 0x55;
+ }
+ return 0;
+
+ case 1:
+ {
+ unsigned fails = 0;
+ for (unsigned count = 0;; count++) {
+ uint16_t value = count & 0xffff;
+
+ if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
+ fails++;
+
+ if (count >= 5000) {
+ lowsyslog("==== test 1 : %u failures ====\n", fails);
+ perf_print_counter(_pc_txns);
+ perf_print_counter(_pc_dmasetup);
+ perf_print_counter(_pc_retries);
+ perf_print_counter(_pc_timeouts);
+ perf_print_counter(_pc_crcerrs);
+ perf_print_counter(_pc_dmaerrs);
+ perf_print_counter(_pc_protoerrs);
+ perf_print_counter(_pc_uerrs);
+ perf_print_counter(_pc_idle);
+ perf_print_counter(_pc_badidle);
+ count = 0;
+ }
}
+ return 0;
}
+ case 2:
+ lowsyslog("test 2\n");
return 0;
}
- case 2:
- lowsyslog("test 2\n");
- return 0;
+ default:
+ break;
}
+
return -1;
}
int
-PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+PX4IO_serial::write(unsigned address, void *data, unsigned count)
{
- if (num_values > PKT_MAX_REGS) {
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+ if (count > PKT_MAX_REGS) {
errno = EINVAL;
return -1;
}
@@ -339,11 +354,11 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi
int result;
for (unsigned retries = 0; retries < 3; retries++) {
- _dma_buffer.count_code = num_values | PKT_CODE_WRITE;
+ _dma_buffer.count_code = count | PKT_CODE_WRITE;
_dma_buffer.page = page;
_dma_buffer.offset = offset;
- memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values));
- for (unsigned i = num_values; i < PKT_MAX_REGS; i++)
+ memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
+ for (unsigned i = count; i < PKT_MAX_REGS; i++)
_dma_buffer.regs[i] = 0x55aa;
/* XXX implement check byte */
@@ -373,9 +388,13 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi
}
int
-PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+PX4IO_serial::read(unsigned address, void *data, unsigned count)
{
- if (num_values > PKT_MAX_REGS)
+ uint8_t page = address >> 8;
+ uint8_t offset = address & 0xff;
+ uint16_t *values = reinterpret_cast<uint16_t *>(data);
+
+ if (count > PKT_MAX_REGS)
return -EINVAL;
sem_wait(&_bus_semaphore);
@@ -383,7 +402,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
int result;
for (unsigned retries = 0; retries < 3; retries++) {
- _dma_buffer.count_code = num_values | PKT_CODE_READ;
+ _dma_buffer.count_code = count | PKT_CODE_READ;
_dma_buffer.page = page;
_dma_buffer.offset = offset;
@@ -402,7 +421,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
perf_count(_pc_protoerrs);
/* compare the received count with the expected count */
- } else if (PKT_COUNT(_dma_buffer) != num_values) {
+ } else if (PKT_COUNT(_dma_buffer) != count) {
/* IO returned the wrong number of registers - no point retrying */
errno = EIO;
@@ -413,14 +432,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
} else {
/* copy back the result */
- memcpy(values, &_dma_buffer.regs[0], (2 * num_values));
+ memcpy(values, &_dma_buffer.regs[0], (2 * count));
}
break;
}
perf_count(_pc_retries);
}
-out:
+
sem_post(&_bus_semaphore);
return result;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 03:34:32 +0000