Checkpoint: interface abstraction for px4io driver

src/drivers/px4io/interface.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 interface.h
+ *
+ * PX4IO interface classes.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+
+class PX4IO_interface
+{
+public:
+	/**
+	 * Check that the interface initialised OK.
+	 *
+	 * Does not check that communication has been established.
+	 */
+	virtual bool	ok() = 0;
+
+	/**
+	 * Set PX4IO registers.
+	 *
+	 * @param page		The register page to write
+	 * @param offset	Offset of the first register to write
+	 * @param values	Pointer to values to write
+	 * @param num_values	The number of values to write
+	 * @return		Zero on success.
+	 */
+	virtual int	set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) = 0;
+
+	/**
+	 * Get PX4IO registers.
+	 *
+	 * @param page		The register page to read
+	 * @param offset	Offset of the first register to read
+	 * @param values	Pointer to store values read
+	 * @param num_values	The number of values to read
+	 * @return		Zero on success.
+	 */	
+	virtual int	get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0;
+};
+
+extern PX4IO_interface	*io_i2c_interface(int bus, uint8_t address);
+extern PX4IO_interface	*io_serial_interface(int port);

src/drivers/px4io/interface_i2c.cpp

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 interface_i2c.cpp
+  *
+  * I2C interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/i2c.h>
+
+#include <mavlink/mavlink_log.h>
+#include "uploader.h"
+#include <modules/px4iofirmware/protocol.h>
+
+#include "interface.h"
+
+class PX4IO_I2C : public PX4IO_interface
+{
+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();
+
+private:
+	static const unsigned	_retries = 2;
+
+	struct i2c_dev_s	*_dev;
+	uint8_t			_address;
+};
+
+PX4IO_interface	*io_i2c_interface(int bus, uint8_t address)
+{
+	return new PX4IO_I2C(bus, address);
+}
+
+PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
+	_dev(nullptr),
+	_address(address)
+{
+	_dev = up_i2cinitialize(bus);
+	if (_dev)
+		I2C_SETFREQUENCY(_dev, 400000);
+}
+
+PX4IO_I2C::~PX4IO_I2C()
+{
+	if (_dev)
+		up_i2cuninitialize(_dev);
+}
+
+bool
+PX4IO_I2C::ok()
+{
+	if (!_dev)
+		return false;
+
+	/* check any other status here */
+
+	return true;
+}
+
+int
+PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+	int ret;
+
+	/* 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 {
+
+		/* perform the transfer */
+		ret = I2C_TRANSFER(_dev, msgv, 2);
+
+		if (ret == OK)
+			break;
+
+	} while (tries++ < _retries);
+
+	return ret;
+}
+
+int
+PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+{
+	int ret;
+
+	/* 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_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);
+
+	return ret;
+}

src/drivers/px4io/interface_serial.cpp

@@ -0,0 +1,361 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 interface_serial.cpp
+  *
+  * Serial interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32_internal.h>
+
+#include <debug.h>
+
+#include <systemlib/hx_stream.h>
+
+#include "interface.h"
+
+class PX4IO_serial : public PX4IO_interface
+{
+public:
+	PX4IO_serial(int port);
+	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();
+
+private:
+	volatile uint32_t	*_serial_base;
+	int			_vector;
+
+	uint8_t			*_tx_buf;
+	unsigned		_tx_size;
+
+	const uint8_t		*_rx_buf;
+	unsigned		_rx_size;
+
+	hx_stream_t		_stream;
+
+	sem_t			_bus_semaphore;
+	sem_t			_completion_semaphore;
+
+	/**
+	 * Send _tx_size bytes from the buffer, then
+	 * if _rx_size is greater than zero wait for a packet
+	 * to come back.
+	 */
+	int			_wait_complete();
+
+	/**
+	 * Interrupt handler.
+	 */
+	static int		_interrupt(int irq, void *context);
+	void			_do_interrupt();
+
+	/**
+	 * Stream transmit callback
+	 */
+	static void		_tx(void *arg, uint8_t data);
+	void			_do_tx(uint8_t data);
+
+	/**
+	 * Stream receive callback
+	 */
+	static void		_rx(void *arg, const void *data, size_t length);
+	void			_do_rx(const uint8_t *data, size_t length);
+
+	/**
+	 * Serial register accessors.
+	 */
+	volatile uint32_t	&_sreg(unsigned offset)
+	{
+		return *(_serial_base + (offset / sizeof(uint32_t)));
+	}
+	volatile uint32_t	&_SR()   { return _sreg(STM32_USART_SR_OFFSET);   }
+	volatile uint32_t	&_DR()   { return _sreg(STM32_USART_DR_OFFSET);   }
+	volatile uint32_t	&_BRR()  { return _sreg(STM32_USART_BRR_OFFSET);  }
+	volatile uint32_t	&_CR1()  { return _sreg(STM32_USART_CR1_OFFSET);  }
+	volatile uint32_t	&_CR2()  { return _sreg(STM32_USART_CR2_OFFSET);  }
+	volatile uint32_t	&_CR3()  { return _sreg(STM32_USART_CR3_OFFSET);  }
+	volatile uint32_t	&_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); }
+};
+
+/* XXX hack to avoid expensive IRQ lookup */
+static PX4IO_serial *io_serial;
+
+PX4IO_interface	*io_serial_interface(int port)
+{
+	return new PX4IO_serial(port);
+}
+
+PX4IO_serial::PX4IO_serial(int port) :
+	_serial_base(0),
+	_vector(0),
+	_tx_buf(nullptr),
+	_tx_size(0),
+	_rx_size(0),
+	_stream(0)
+{
+	/* only allow one instance */
+	if (io_serial != nullptr)
+		return;
+
+	switch (port) {
+	case 5:
+		_serial_base = (volatile uint32_t *)STM32_UART5_BASE;
+		_vector = STM32_IRQ_UART5;
+		break;
+	default:
+		/* not a supported port */
+		return;
+	}
+
+	/* need space for worst-case escapes + hx protocol overhead */
+	/* XXX this is kinda gross, but hx transmits a byte at a time */
+	_tx_buf = new uint8_t[HX_STREAM_MAX_FRAME];
+
+	irq_attach(_vector, &_interrupt);
+
+	_stream = hx_stream_init(-1, _rx, this);
+
+	sem_init(&_completion_semaphore, 0, 0);
+	sem_init(&_bus_semaphore, 0, 1);
+}
+
+PX4IO_serial::~PX4IO_serial()
+{
+
+	if (_tx_buf != nullptr)
+		delete[] _tx_buf;
+
+	if (_vector)
+		irq_detach(_vector);
+
+	if (io_serial == this)
+		io_serial = nullptr;
+
+	if (_stream)
+		hx_stream_free(_stream);
+
+	sem_destroy(&_completion_semaphore);
+	sem_destroy(&_bus_semaphore);
+}
+
+bool
+PX4IO_serial::ok()
+{
+	if (_serial_base == 0)
+		return false;
+	if (_vector == 0)
+		return false;
+	if (_tx_buf == nullptr)
+		return false;
+	if (!_stream)
+		return false;
+
+	return true;
+}
+
+int
+PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+
+	unsigned count = num_values * sizeof(*values);
+	if (count > (HX_STREAM_MAX_FRAME - 2))
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	_tx_buf[0] = page;
+	_tx_buf[1] = offset;
+	memcpy(&_tx_buf[2], (void *)values, count);
+
+	_tx_size = count + 2;
+	_rx_size = 0;
+
+	/* start the transaction and wait for it to complete */
+	int result = _wait_complete();
+
+	sem_post(&_bus_semaphore);
+	return result;
+}
+
+int
+PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+{
+
+	unsigned count = num_values * sizeof(*values);
+	if (count > HX_STREAM_MAX_FRAME)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	_tx_buf[0] = page;
+	_tx_buf[1] = offset;
+	_tx_buf[2] = num_values;
+
+	_tx_size = 3;		/* this tells IO that this is a read request */
+	_rx_size = count;
+
+	/* start the transaction and wait for it to complete */
+	int result = _wait_complete();
+	if (result != OK)
+		goto out;
+
+	/* compare the received count with the expected count */
+	if (_rx_size != count) {
+		return -EIO;
+	} else {
+		/* copy back the result */
+		memcpy(values, &_tx_buf[0], count);
+	}
+out:
+	sem_post(&_bus_semaphore);
+	return OK;
+}
+
+int
+PX4IO_serial::_wait_complete()
+{
+	/* prepare the stream for transmission */
+	hx_stream_reset(_stream);
+	hx_stream_start(_stream, _tx_buf, _tx_size);
+
+	/* enable UART */
+	_CR1() |= USART_CR1_RE |
+		  USART_CR1_TE |
+		  USART_CR1_TXEIE |
+		  USART_CR1_RXNEIE |
+		  USART_CR1_UE;
+
+	/* compute the deadline for a 5ms timeout */
+	struct timespec abstime;
+	clock_gettime(CLOCK_REALTIME, &abstime);
+	abstime.tv_nsec += 5000000;	/* 5ms timeout */
+	while (abstime.tv_nsec > 1000000000) {
+		abstime.tv_sec++;
+		abstime.tv_nsec -= 1000000000;
+	}
+
+	/* wait for the transaction to complete */
+	int ret = sem_timedwait(&_completion_semaphore, &abstime);
+
+	/* disable the UART */
+	_CR1() &= ~(USART_CR1_RE |
+		    USART_CR1_TE |
+		    USART_CR1_TXEIE |
+		    USART_CR1_RXNEIE |
+		    USART_CR1_UE);
+
+	return ret;
+}
+
+int
+PX4IO_serial::_interrupt(int irq, void *context)
+{
+	/* ... because NuttX doesn't give us a handle per vector */
+	io_serial->_do_interrupt();
+	return 0;
+}
+
+void
+PX4IO_serial::_do_interrupt()
+{
+	uint32_t sr = _SR();
+
+	/* handle transmit completion */
+	if (sr & USART_SR_TXE) {
+		int c = hx_stream_send_next(_stream);
+		if (c == -1) {
+			/* transmit (nearly) done, not interested in TX-ready interrupts now */
+			_CR1() &= ~USART_CR1_TXEIE;
+
+			/* was this a tx-only operation? */
+			if (_rx_size == 0) {
+				/* wake up waiting sender */
+				sem_post(&_completion_semaphore);
+			}
+		} else {
+			_DR() = c;
+		}
+	}
+
+	if (sr & USART_SR_RXNE) {
+		uint8_t c = _DR();
+
+		hx_stream_rx(_stream, c);
+	}
+}
+
+void
+PX4IO_serial::_rx(void *arg, const void *data, size_t length)
+{
+	PX4IO_serial *pserial = reinterpret_cast<PX4IO_serial *>(arg);
+
+	pserial->_do_rx((const uint8_t *)data, length);
+}
+
+void
+PX4IO_serial::_do_rx(const uint8_t *data, size_t length)
+{
+	_rx_buf = data;
+
+	if (length < _rx_size)
+		_rx_size = length;
+
+	/* notify waiting receiver */
+	sem_post(&_completion_semaphore);
+}
+
+
+

src/drivers/px4io/module.mk

@@ -38,4 +38,9 @@
 MODULE_COMMAND		= px4io
 
 SRCS			= px4io.cpp \
-			  uploader.cpp
+			  uploader.cpp \
+			  interface_serial.cpp \
+			  interface_i2c.cpp
+
+# XXX prune to just get UART registers
+INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common

src/drivers/px4io/px4io.cpp

@@ -58,7 +58,6 @@
 #include <arch/board/board.h>
 
 #include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
@@ -83,16 +82,18 @@
 #include <debug.h>
 
 #include <mavlink/mavlink_log.h>
-#include "uploader.h"
 #include <modules/px4iofirmware/protocol.h>
 
+#include "uploader.h"
+#include "interface.h"
+
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
-class PX4IO : public device::I2C
+class PX4IO : public device::CDev
 {
 public:
-	PX4IO();
+	PX4IO(PX4IO_interface *interface);
 	virtual ~PX4IO();
 
 	virtual int		init();
@@ -130,6 +131,8 @@ public:
 	void			print_status();
 
 private:
+	PX4IO_interface		*_interface;
+
 	// XXX
 	unsigned		_max_actuators;
 	unsigned		_max_controls;
@@ -312,8 +315,9 @@ PX4IO	*g_dev;
 
 }
 
-PX4IO::PX4IO() :
-	I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+PX4IO::PX4IO(PX4IO_interface *interface) :
+	CDev("px4io", "/dev/px4io"),
+	_interface(interface),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
@@ -364,6 +368,9 @@ PX4IO::~PX4IO()
 	if (_task != -1)
 		task_delete(_task);
 
+	if (_interface != nullptr)
+		delete _interface;
+
 	g_dev = nullptr;
 }
 
@@ -375,18 +382,10 @@ PX4IO::init()
 	ASSERT(_task == -1);
 
 	/* do regular cdev init */
-	ret = I2C::init();
+	ret = CDev::init();
 	if (ret != OK)
 		return ret;
 
-	/*
-	 * Enable a couple of retries for operations to IO.
-	 *
-	 * Register read/write operations are intentionally idempotent
-	 * so this is safe as designed.
-	 */
-	_retries = 2;
-
 	/* get some parameters */
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
 	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
@@ -395,7 +394,7 @@ PX4IO::init()
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
 	if ((_max_actuators < 1) || (_max_actuators > 255) ||
 	    (_max_relays < 1)    || (_max_relays > 255)    ||
-	    (_max_transfer < 16)   || (_max_transfer > 255)    ||
+	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
 		log("failed getting parameters from PX4IO");
@@ -700,8 +699,6 @@ PX4IO::io_set_control_state()
 int
 PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 {
-	uint16_t 		regs[_max_actuators];
-
 	if (len > _max_actuators)
 		/* fail with error */
 		return E2BIG;
@@ -1114,22 +1111,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
 		return -EINVAL;
 	}
 
-	/* set up the transfer */
-	uint8_t 	addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
-
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_NORESTART;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
-
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
+	int ret =  _interface->set_reg(page, offset, values, num_values);
 	if (ret != OK)
 		debug("io_reg_set: error %d", ret);
 	return ret;
@@ -1144,22 +1126,13 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
 int
 PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
 {
-	/* set up the transfer */
-	uint8_t		addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
+	/* range check the transfer */
+	if (num_values > ((_max_transfer) / sizeof(*values))) {
+		debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+		return -EINVAL;
+	}
 
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_READ;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
-
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
+	int ret =  _interface->get_reg(page, offset, values, num_values);
 	if (ret != OK)
 		debug("io_reg_get: data error %d", ret);
 	return ret;
@@ -1603,8 +1576,26 @@ start(int argc, char *argv[])
 	if (g_dev != nullptr)
 		errx(1, "already loaded");
 
+	PX4IO_interface *interface;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+	interface = io_serial_interface(5);	/* XXX wrong port! */
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU)
+	interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+#else
+# error Unknown board - cannot select interface.
+#endif
+
+	if (interface == nullptr)
+		errx(1, "cannot alloc interface");
+
+	if (!interface->ok()) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
+
 	/* create the driver - it will set g_dev */
-	(void)new PX4IO();
+	(void)new PX4IO(interface);
 
 	if (g_dev == nullptr)
 		errx(1, "driver alloc failed");