Checkpoint: interface abstraction for px4io driver

9 files changed, 845 insertions, 191 deletions

diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk
index 26c249901..0463ccd84 100644
--- a/makefiles/config_px4fmuv2_default.mk
+++ b/makefiles/config_px4fmuv2_default.mk
@@ -15,6 +15,7 @@ MODULES		+= drivers/stm32
 MODULES		+= drivers/stm32/adc
 MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/px4fmu
+MODULES		+= drivers/px4io
 MODULES		+= drivers/boards/px4fmuv2
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/lsm303d
diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h
new file mode 100644
index 000000000..834cb9e07
--- /dev/null
+++ b/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);
diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp
new file mode 100644
index 000000000..6895a7e23
--- /dev/null
+++ b/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;
+}
diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp
new file mode 100644
index 000000000..f91284c72
--- /dev/null
+++ b/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);
+}
+
+
+
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 328e5a684..d5bab6599 100644
--- a/src/drivers/px4io/module.mk
+++ b/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
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 19163cebe..ecf50c859 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/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];
-
-	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);
+	/* 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;
+	}
+
+	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");
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 90d63ea1a..0e40bff69 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,8 +36,7 @@
 /**
  * @file protocol.h
  *
- * PX4IO interface protocol
- * ========================
+ * PX4IO interface protocol.
  *
  * Communication is performed via writes to and reads from 16-bit virtual
  * registers organised into pages of 255 registers each.
@@ -46,7 +45,7 @@
  * respectively. Subsequent reads and writes increment the offset within
  * the page. 
  *
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
  *
  * Note that some pages may permit offset values greater than 255, which
  * can only be achieved by long writes. The offset does not wrap.
@@ -63,29 +62,6 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
- *
- * PX4IO I2C interface notes
- * -------------------------
- *
- * Register read/write operations are mapped directly to PX4IO register
- * read/write operations.
- *
- * PX4IO Serial interface notes
- * ----------------------------
- *
- * The MSB of the page number is used to distinguish between read and
- * write operations. If set, the operation is a write and additional 
- * data is expected to follow in the packet as for I2C writes.
- *
- * If clear, the packet is expected to contain a single byte giving the
- * number of registers to be read. PX4IO will respond with a packet containing
- * the same header (page, offset) and the requested data.
- *
- * If a read is requested when PX4IO does not have buffer space to store
- * the reply, the request will be dropped. PX4IO is always configured with
- * enough space to receive one full-sized write and one read request, and
- * to send one full-sized read response.
- *
  */
 
 #define PX4IO_CONTROL_CHANNELS			8
@@ -99,14 +75,12 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
-#define PX4IO_PAGE_WRITE		(1<<7)
-
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
-#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum packet transfer size */
+#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
@@ -168,7 +142,7 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP		64
+#define PX4IO_PAGE_SETUP			100
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
@@ -186,13 +160,13 @@
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS		65		/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD		66		/* see px4io_mixdata structure below */
+#define PX4IO_PAGE_MIXERLOAD			102
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG		67		/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -204,10 +178,10 @@
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM		68		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM		69		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 88f7f762c..fdc3edac7 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
-
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
+
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -76,13 +88,12 @@ struct hx_stream {
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static int	hx_rx_frame(hx_stream_t stream);
-static bool	hx_rx_char(hx_stream_t stream, uint8_t c);
 
 static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = true;
+		stream->tx_error = true;
 }
 
 static void
@@ -106,11 +117,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -123,11 +134,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -135,7 +146,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -151,8 +162,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -180,105 +191,135 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
 int
-hx_stream_send(hx_stream_t stream,
+hx_stream_start(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
-
-	if (resid > HX_STREAM_MAX_FRAME)
+	if (count > HX_STREAM_MAX_FRAME)
 		return -EINVAL;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
 
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
 
-	/* compute the CRC */
-	u.w = crc32(data, count);
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
 
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
 
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
 
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	int result;
+
+	result = hx_start(stream, data, count);
+	if (result != OK)
+		return result;
+
+	int c;
+	while ((c = hx_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
-static bool
-hx_rx_char(hx_stream_t stream, uint8_t c)
+void
+hx_stream_rx(hx_stream_t stream, uint8_t c)
 {
 	/* frame end? */
 	if (c == FBO) {
 		hx_rx_frame(stream);
-		return true;
+		return;
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
-		return false;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
+		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
-
-	return false;
-}
-
-void
-hx_stream_rx_char(hx_stream_t stream, uint8_t c)
-{
-	hx_rx_char(stream, c);
-}
-
-int
-hx_stream_rx(hx_stream_t stream)
-{
-	uint16_t	buf[16];
-	ssize_t		len;
-
-	/* read bytes */
-	len = read(stream->fd, buf, sizeof(buf));
-	if (len <= 0) {
-
-		/* nonblocking stream and no data */
-		if (errno == EWOULDBLOCK)
-			return 0;
-
-		/* error or EOF */
-		return -errno;
-	}
-
-	/* process received characters */
-	for (int i = 0; i < len; i++)
-		hx_rx_char(stream, buf[i]);
-
-	return 0;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index be4850f74..1f3927222 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -90,6 +92,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_errors);
 
 /**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
+/**
  * Send a frame.
  *
  * This function will block until all frame bytes are sent if
@@ -114,25 +154,9 @@ __EXPORT extern int		hx_stream_send(hx_stream_t stream,
  * @param stream	A handle returned from hx_stream_init.
  * @param c		The character to process.
  */
-__EXPORT extern void		hx_stream_rx_char(hx_stream_t stream,
+__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
 		uint8_t c);
 
-/**
- * Handle received bytes from the stream.
- *
- * Note that this interface should only be used with blocking streams
- * when it is OK for the call to block until a frame is received.
- *
- * When used with a non-blocking stream, it will typically return
- * immediately, or after handling a received frame.
- *
- * @param stream	A handle returned from hx_stream_init.
- * @return		-errno on error, nonzero if a frame
- *			has been received, or if not enough
- *			bytes are available to complete a frame.
- */
-__EXPORT extern int		hx_stream_rx(hx_stream_t stream);
-
 __END_DECLS
 
 #endif