Moved all drivers to new world, PX4IO completely in new world

30 files changed, 7345 insertions, 2 deletions

diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io/module.mk
new file mode 100644
index 000000000..2601a1c15
--- /dev/null
+++ b/src/drivers/boards/px4io/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IO
+#
+
+SRCS	= px4io_init.c \
+	  px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c
new file mode 100644
index 000000000..14e8dc13a
--- /dev/null
+++ b/src/drivers/boards/px4io/px4io_init.c
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4io_init.c
+ *
+ * PX4IO-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+
+#include "stm32_internal.h"
+#include "px4io_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_pwm_output.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure GPIOs */
+	stm32_configgpio(GPIO_ACC1_PWR_EN);
+	stm32_configgpio(GPIO_ACC2_PWR_EN);
+	stm32_configgpio(GPIO_SERVO_PWR_EN);
+	stm32_configgpio(GPIO_RELAY1_EN);
+	stm32_configgpio(GPIO_RELAY2_EN);
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+	stm32_configgpio(GPIO_ACC_OC_DETECT);
+	stm32_configgpio(GPIO_SERVO_OC_DETECT);
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	stm32_configgpio(GPIO_ADC_VBATT);
+	stm32_configgpio(GPIO_ADC_IN5);
+}
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h
new file mode 100644
index 000000000..44bb98513
--- /dev/null
+++ b/src/drivers/boards/px4io/px4io_internal.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 PX4IO hardware definitions.
+ */
+
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+#include <stm32_internal.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* PX4IO GPIOs **********************************************************************/
+/* LEDs */
+
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+
+/* Safety switch button *************************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ************************************************************/
+
+#define GPIO_ACC1_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_ACC2_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_ACC_OC_DETECT   (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
+
+#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
+
+/* Analog inputs ********************************************************************/
+
+#define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+#define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c
new file mode 100644
index 000000000..a2f73c429
--- /dev/null
+++ b/src/drivers/boards/px4io/px4io_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2012 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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b21f83e44..4c2f1736f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -80,11 +80,11 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <debug.h>
 
-#include <px4io/protocol.h>
 #include <mavlink/mavlink_log.h>
 #include "uploader.h"
-#include <debug.h>
+#include <modules/px4iofirmware/protocol.h>
 
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
new file mode 100644
index 000000000..911def943
--- /dev/null
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 adc.cpp
+ *
+ * Driver for the STM32 ADC.
+ *
+ * This is a low-rate driver, designed for sampling things like voltages
+ * and so forth. It avoids the gross complexity of the NuttX ADC driver.
+ */
+
+#include <nuttx/config.h>
+#include <drivers/device/device.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_adc.h>
+
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
+
+#define rSR		REG(STM32_ADC_SR_OFFSET)
+#define rCR1		REG(STM32_ADC_CR1_OFFSET)
+#define rCR2		REG(STM32_ADC_CR2_OFFSET)
+#define rSMPR1		REG(STM32_ADC_SMPR1_OFFSET)
+#define rSMPR2		REG(STM32_ADC_SMPR2_OFFSET)
+#define rJOFR1		REG(STM32_ADC_JOFR1_OFFSET)
+#define rJOFR2		REG(STM32_ADC_JOFR2_OFFSET)
+#define rJOFR3		REG(STM32_ADC_JOFR3_OFFSET)
+#define rJOFR4		REG(STM32_ADC_JOFR4_OFFSET)
+#define rHTR		REG(STM32_ADC_HTR_OFFSET)
+#define rLTR		REG(STM32_ADC_LTR_OFFSET)
+#define rSQR1		REG(STM32_ADC_SQR1_OFFSET)
+#define rSQR2		REG(STM32_ADC_SQR2_OFFSET)
+#define rSQR3		REG(STM32_ADC_SQR3_OFFSET)
+#define rJSQR		REG(STM32_ADC_JSQR_OFFSET)
+#define rJDR1		REG(STM32_ADC_JDR1_OFFSET)
+#define rJDR2		REG(STM32_ADC_JDR2_OFFSET)
+#define rJDR3		REG(STM32_ADC_JDR3_OFFSET)
+#define rJDR4		REG(STM32_ADC_JDR4_OFFSET)
+#define rDR		REG(STM32_ADC_DR_OFFSET)
+
+#ifdef STM32_ADC_CCR
+# define rCCR		REG(STM32_ADC_CCR_OFFSET)
+#endif
+
+class ADC : public device::CDev
+{
+public:
+	ADC(uint32_t channels);
+	~ADC();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t		read(file *filp, char *buffer, size_t len);
+
+protected:
+	virtual int		open_first(struct file *filp);
+	virtual int		close_last(struct file *filp);
+
+private:
+	static const hrt_abstime _tickrate = 10000;	/**< 100Hz base rate */
+	
+	hrt_call		_call;
+	perf_counter_t		_sample_perf;
+
+	unsigned		_channel_count;
+	adc_msg_s		*_samples;		/**< sample buffer */
+
+	/** work trampoline */
+	static void		_tick_trampoline(void *arg);
+
+	/** worker function */
+	void			_tick();
+
+	/**
+	 * Sample a single channel and return the measured value.
+	 *
+	 * @param channel		The channel to sample.
+	 * @return			The sampled value, or 0xffff if
+	 *				sampling failed.
+	 */
+	uint16_t		_sample(unsigned channel);
+
+};
+
+ADC::ADC(uint32_t channels) :
+	CDev("adc", ADC_DEVICE_PATH),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+	_channel_count(0),
+	_samples(nullptr)
+{
+	_debug_enabled = true;
+
+	/* always enable the temperature sensor */
+	channels |= 1 << 16;
+
+	/* allocate the sample array */
+	for (unsigned i = 0; i < 32; i++) {
+		if (channels & (1 << i)) {
+			_channel_count++;
+		}
+	}
+	_samples = new adc_msg_s[_channel_count];
+
+	/* prefill the channel numbers in the sample array */
+	if (_samples != nullptr) {
+		unsigned index = 0;
+		for (unsigned i = 0; i < 32; i++) {
+			if (channels & (1 << i)) {
+				_samples[index].am_channel = i;
+				_samples[index].am_data = 0;
+				index++;
+			}
+		}
+	}
+}
+
+ADC::~ADC()
+{
+	if (_samples != nullptr)
+		delete _samples;
+}
+
+int
+ADC::init()
+{
+	/* do calibration if supported */
+#ifdef ADC_CR2_CAL
+	rCR2 |= ADC_CR2_CAL;
+	usleep(100);
+	if (rCR2 & ADC_CR2_CAL)
+		return -1;
+#endif
+
+	/* arbitrarily configure all channels for 55 cycle sample time */
+	rSMPR1 = 0b00000011011011011011011011011011;
+	rSMPR2 = 0b00011011011011011011011011011011;
+
+	/* XXX for F2/4, might want to select 12-bit mode? */
+	rCR1 = 0;
+
+	/* enable the temperature sensor / Vrefint channel if supported*/
+	rCR2 = 
+#ifdef ADC_CR2_TSVREFE
+		/* enable the temperature sensor in CR2 */
+		ADC_CR2_TSVREFE |
+#endif
+		0;
+
+#ifdef ADC_CCR_TSVREFE
+	/* enable temperature sensor in CCR */
+	rCCR = ADC_CCR_TSVREFE;
+#endif
+
+	/* configure for a single-channel sequence */
+	rSQR1 = 0;
+	rSQR2 = 0;
+	rSQR3 = 0;	/* will be updated with the channel each tick */ 
+
+	/* power-cycle the ADC and turn it on */
+	rCR2 &= ~ADC_CR2_ADON;
+	usleep(10);
+	rCR2 |= ADC_CR2_ADON;
+	usleep(10);
+	rCR2 |= ADC_CR2_ADON;
+	usleep(10);
+
+	/* kick off a sample and wait for it to complete */
+	hrt_abstime now = hrt_absolute_time();
+	rCR2 |= ADC_CR2_SWSTART;
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 500) {
+			log("sample timeout");
+			return -1;
+			return 0xffff;
+		}
+	}
+
+
+	debug("init done");
+
+	/* create the device node */
+	return CDev::init();
+}
+
+int
+ADC::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	return -ENOTTY;
+}
+
+ssize_t
+ADC::read(file *filp, char *buffer, size_t len)
+{
+	const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
+
+	if (len > maxsize)
+		len = maxsize;
+
+	/* block interrupts while copying samples to avoid racing with an update */
+	irqstate_t flags = irqsave();
+	memcpy(buffer, _samples, len);
+	irqrestore(flags);
+
+	return len;
+}
+
+int
+ADC::open_first(struct file *filp)
+{
+	/* get fresh data */
+	_tick();
+
+	/* and schedule regular updates */
+	hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
+
+	return 0;
+}
+
+int
+ADC::close_last(struct file *filp)
+{
+	hrt_cancel(&_call);
+	return 0;
+}
+
+void
+ADC::_tick_trampoline(void *arg)
+{
+	((ADC *)arg)->_tick();
+}
+
+void
+ADC::_tick()
+{
+	/* scan the channel set and sample each */
+	for (unsigned i = 0; i < _channel_count; i++)
+		_samples[i].am_data = _sample(_samples[i].am_channel);
+}
+
+uint16_t
+ADC::_sample(unsigned channel)
+{
+	perf_begin(_sample_perf);
+
+	/* clear any previous EOC */
+	if (rSR & ADC_SR_EOC)
+		rSR &= ~ADC_SR_EOC;
+
+	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
+	rSQR3 = channel;
+	rCR2 |= ADC_CR2_SWSTART;
+
+	/* wait for the conversion to complete */
+	hrt_abstime now = hrt_absolute_time();
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 50) {
+			log("sample timeout");
+			return 0xffff;
+		}
+	}
+
+	/* read the result and clear EOC */
+	uint16_t result = rDR;
+
+	perf_end(_sample_perf);
+	return result;
+}
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int adc_main(int argc, char *argv[]);
+
+namespace
+{
+ADC	*g_adc;
+
+void
+test(void)
+{
+
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+	if (fd < 0)
+		err(1, "can't open ADC device");
+
+	for (unsigned i = 0; i < 50; i++) {
+		adc_msg_s data[8];
+		ssize_t count = read(fd, data, sizeof(data));
+
+		if (count < 0)
+			errx(1, "read error");
+
+		unsigned channels = count / sizeof(data[0]);
+
+		for (unsigned j = 0; j < channels; j++) {
+			printf ("%d: %u  ", data[j].am_channel, data[j].am_data);
+		}
+
+		printf("\n");
+		usleep(500000);
+	}
+
+	exit(0);
+}
+}
+
+int
+adc_main(int argc, char *argv[])
+{
+	if (g_adc == nullptr) {
+		/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+
+		if (g_adc == nullptr)
+			errx(1, "couldn't allocate the ADC driver");
+
+		if (g_adc->init() != OK) {
+			delete g_adc;
+			errx(1, "ADC init failed");
+		}
+	}
+
+	if (argc > 1) {
+		if (!strcmp(argv[1], "test"))
+			test();
+	}
+
+	exit(0);
+}
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
new file mode 100644
index 000000000..48620feea
--- /dev/null
+++ b/src/drivers/stm32/adc/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# STM32 ADC driver
+#
+
+MODULE_COMMAND	= adc
+
+SRCS		= adc.cpp
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
new file mode 100644
index 000000000..cec0c49ff
--- /dev/null
+++ b/src/drivers/stm32/drv_hrt.c
@@ -0,0 +1,908 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 drv_hrt.c
+ *
+ * High-resolution timer callouts and timekeeping.
+ *
+ * This can use any general or advanced STM32 timer.
+ *
+ * Note that really, this could use systick too, but that's
+ * monopolised by NuttX and stealing it would just be awkward.
+ *
+ * We don't use the NuttX STM32 driver per se; rather, we
+ * claim the timer and then drive it directly.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+#ifdef CONFIG_HRT_TIMER
+
+/* HRT configuration */
+#if   HRT_TIMER == 1
+# define HRT_TIMER_BASE		STM32_TIM1_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
+# if CONFIG_STM32_TIM1
+#  error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
+# endif
+#elif HRT_TIMER == 2
+# define HRT_TIMER_BASE		STM32_TIM2_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM2
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
+# if CONFIG_STM32_TIM2
+#  error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2
+# endif
+#elif HRT_TIMER == 3
+# define HRT_TIMER_BASE		STM32_TIM3_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM3EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM3
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
+# if CONFIG_STM32_TIM3
+#  error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3
+# endif
+#elif HRT_TIMER == 4
+# define HRT_TIMER_BASE		STM32_TIM4_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM4EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM4
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
+# if CONFIG_STM32_TIM4
+#  error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4
+# endif
+#elif HRT_TIMER == 5
+# define HRT_TIMER_BASE		STM32_TIM5_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM5
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
+# if CONFIG_STM32_TIM5
+#  error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5
+# endif
+#elif HRT_TIMER == 8
+# define HRT_TIMER_BASE		STM32_TIM8_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM8CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
+# if CONFIG_STM32_TIM8
+#  error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8
+# endif
+#elif HRT_TIMER == 9
+# define HRT_TIMER_BASE		STM32_TIM9_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1BRK
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
+# if CONFIG_STM32_TIM9
+#  error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9
+# endif
+#elif HRT_TIMER == 10
+# define HRT_TIMER_BASE		STM32_TIM10_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1UP
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM10_CLKIN
+# if CONFIG_STM32_TIM10
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
+# endif
+#elif HRT_TIMER == 11
+# define HRT_TIMER_BASE		STM32_TIM11_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM11_CLKIN
+# if CONFIG_STM32_TIM11
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
+# endif
+#else
+# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
+#endif
+
+/*
+ * HRT clock must be a multiple of 1MHz greater than 1MHz
+ */
+#if (HRT_TIMER_CLOCK % 1000000) != 0
+# error HRT_TIMER_CLOCK must be a multiple of 1MHz
+#endif
+#if HRT_TIMER_CLOCK <= 1000000
+# error HRT_TIMER_CLOCK must be greater than 1MHz
+#endif
+
+/*
+ * Minimum/maximum deadlines.
+ *
+ * These are suitable for use with a 16-bit timer/counter clocked
+ * at 1MHz.  The high-resolution timer need only guarantee that it
+ * not wrap more than once in the 50ms period for absolute time to
+ * be consistently maintained.
+ *
+ * The minimum deadline must be such that the time taken between
+ * reading a time and writing a deadline to the timer cannot
+ * result in missing the deadline.
+ */
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
+/*
+ * Period of the free-running counter, in microseconds.
+ */
+#define HRT_COUNTER_PERIOD	65536
+
+/*
+ * Scaling factor(s) for the free-running counter; convert an input
+ * in counts to a time in microseconds.
+ */
+#define HRT_COUNTER_SCALE(_c)	(_c)
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if HRT_TIMER_CHANNEL == 1
+# define rCCR_HRT	rCCR1			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC1IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC1IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 2
+# define rCCR_HRT	rCCR2			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC2IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC2IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 3
+# define rCCR_HRT	rCCR3			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC3IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC3IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 4
+# define rCCR_HRT	rCCR4			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC4IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC4IF		/* interrupt status for HRT */
+#else
+# error HRT_TIMER_CHANNEL must be a value between 1 and 4
+#endif
+
+/*
+ * Queue of callout entries.
+ */
+static struct sq_queue_s	callout_queue;
+
+/* latency baseline (last compare value applied) */
+static uint16_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint16_t			latency_actual;
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT	8
+static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+/* timer-specific functions */
+static void		hrt_tim_init(void);
+static int		hrt_tim_isr(int irq, void *context);
+static void		hrt_latency_update(void);
+
+/* callout list manipulation */
+static void		hrt_call_internal(struct hrt_call *entry,
+		hrt_abstime deadline,
+		hrt_abstime interval,
+		hrt_callout callout,
+		void *arg);
+static void		hrt_call_enter(struct hrt_call *entry);
+static void		hrt_call_reschedule(void);
+static void		hrt_call_invoke(void);
+
+/*
+ * Specific registers and bits used by PPM sub-functions
+ */
+#ifdef CONFIG_HRT_PPM
+/* 
+ * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
+ */
+# ifndef GTIM_CCER_CC1NP
+#  define GTIM_CCER_CC1NP 0
+#  define GTIM_CCER_CC2NP 0
+#  define GTIM_CCER_CC3NP 0
+#  define GTIM_CCER_CC4NP 0
+#  define PPM_EDGE_FLIP
+# endif
+
+# if HRT_PPM_CHANNEL == 1
+#  define rCCR_PPM	rCCR1			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC1IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC1IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC1OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	1			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC1P
+# elif HRT_PPM_CHANNEL == 2
+#  define rCCR_PPM	rCCR2			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC2IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC2IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC2OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	2			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC2P
+# elif HRT_PPM_CHANNEL == 3
+#  define rCCR_PPM	rCCR3			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC3IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC3IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC3OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	1			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC3P
+# elif HRT_PPM_CHANNEL == 4
+#  define rCCR_PPM	rCCR4			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC4IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC4IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC4OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	2			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
+#  define CCER_PPM_FLIP	GTIM_CCER_CC4P
+# else
+#  error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
+# endif
+
+/*
+ * PPM decoder tuning parameters
+ */
+# define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+# define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* decoded PPM buffer */
+#define PPM_MAX_CHANNELS	12
+__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT unsigned ppm_decoded_channels = 0;
+__EXPORT uint64_t ppm_last_valid_decode = 0;
+
+/* PPM edge history */
+__EXPORT uint16_t ppm_edge_history[32];
+unsigned ppm_edge_next;
+
+/* PPM pulse history */
+__EXPORT uint16_t ppm_pulse_history[32];
+unsigned ppm_pulse_next;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+static void	hrt_ppm_decode(uint32_t status);
+
+#else
+/* disable the PPM configuration */
+# define rCCR_PPM	0
+# define DIER_PPM	0
+# define SR_INT_PPM	0
+# define SR_OVF_PPM	0
+# define CCMR1_PPM	0
+# define CCMR2_PPM	0
+# define CCER_PPM	0
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Initialise the timer we are going to use.
+ *
+ * We expect that we'll own one of the reduced-function STM32 general
+ * timers, and that we can use channel 1 in compare mode.
+ */
+static void
+hrt_tim_init(void)
+{
+	/* claim our interrupt vector */
+	irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
+
+	/* clock/power on our timer */
+	modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
+
+	/* disable and configure the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = DIER_HRT | DIER_PPM;
+	rCCER = 0;		/* unlock CCMR* registers */
+	rCCMR1 = CCMR1_PPM;
+	rCCMR2 = CCMR2_PPM;
+	rCCER = CCER_PPM;
+	rDCR = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC = (HRT_TIMER_CLOCK / 1000000) - 1;	/* this really only works for whole-MHz clocks */
+
+	/* run the full span of the counter */
+	rARR = 0xffff;
+
+	/* set an initial capture a little ways off */
+	rCCR_HRT = 1000;
+
+	/* generate an update event; reloads the counter, all registers */
+	rEGR = GTIM_EGR_UG;
+
+	/* enable the timer */
+	rCR1 = GTIM_CR1_CEN;
+
+	/* enable interrupts */
+	up_enable_irq(HRT_TIMER_VECTOR);
+}
+
+#ifdef CONFIG_HRT_PPM
+/*
+ * Handle the PPM decoder state machine.
+ */
+static void
+hrt_ppm_decode(uint32_t status)
+{
+	uint16_t count = rCCR_PPM;
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (status & SR_OVF_PPM)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+	ppm.last_edge = count;
+
+	ppm_edge_history[ppm_edge_next++] = width;
+
+	if (ppm_edge_next >= 32)
+		ppm_edge_next = 0;
+
+	/*
+	 * if this looks like a start pulse, then push the last set of values
+	 * and reset the state machine
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/* export the last set of samples if we got something sensible */
+		if (ppm.next_channel > 4) {
+			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
+				ppm_buffer[i] = ppm_temp_buffer[i];
+
+			ppm_decoded_channels = i;
+			ppm_last_valid_decode = hrt_absolute_time();
+
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+		return;
+
+	case ACTIVE:
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		ppm_pulse_history[ppm_pulse_next++] = interval;
+
+		if (ppm_pulse_next >= 32)
+			ppm_pulse_next = 0;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+
+}
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Handle the compare interupt by calling the callout dispatcher
+ * and then re-scheduling the next deadline.
+ */
+static int
+hrt_tim_isr(int irq, void *context)
+{
+	uint32_t status;
+
+	/* grab the timer for latency tracking purposes */
+	latency_actual = rCNT;
+
+	/* copy interrupt status */
+	status = rSR;
+
+	/* ack the interrupts we just read */
+	rSR = ~status;
+
+#ifdef CONFIG_HRT_PPM
+
+	/* was this a PPM edge? */
+	if (status & (SR_INT_PPM | SR_OVF_PPM)) {
+		/* if required, flip edge sensitivity */
+# ifdef PPM_EDGE_FLIP
+		rCCER ^= CCER_PPM_FLIP;
+# endif
+
+		hrt_ppm_decode(status);
+	}
+#endif
+
+	/* was this a timer tick? */
+	if (status & SR_INT_HRT) {
+
+		/* do latency calculations */
+		hrt_latency_update();
+
+		/* run any callouts that have met their deadline */
+		hrt_call_invoke();
+
+		/* and schedule the next interrupt */
+		hrt_call_reschedule();
+	}
+
+	return OK;
+}
+
+/*
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime	abstime;
+	uint32_t	count;
+	irqstate_t	flags;
+
+	/*
+	 * Counter state.  Marked volatile as they may change
+	 * inside this routine but outside the irqsave/restore
+	 * pair.  Discourage the compiler from moving loads/stores
+	 * to these outside of the protected range.
+	 */
+	static volatile hrt_abstime base_time;
+	static volatile uint32_t last_count;
+
+	/* prevent re-entry */
+	flags = irqsave();
+
+	/* get the current counter value */
+	count = rCNT;
+
+	/*
+	 * Determine whether the counter has wrapped since the
+	 * last time we're called.
+	 *
+	 * This simple test is sufficient due to the guarantee that
+	 * we are always called at least once per counter period.
+	 */
+	if (count < last_count)
+		base_time += HRT_COUNTER_PERIOD;
+
+	/* save the count for next time */
+	last_count = count;
+
+	/* compute the current time */
+	abstime = HRT_COUNTER_SCALE(base_time + count);
+
+	irqrestore(flags);
+
+	return abstime;
+}
+
+/*
+ * Convert a timespec to absolute time
+ */
+hrt_abstime
+ts_to_abstime(struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/*
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/*
+ * Compare a time value with the current time.
+ */
+hrt_abstime
+hrt_elapsed_time(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	irqrestore(flags);
+
+	return delta;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion
+ */
+hrt_abstime
+hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime ts = hrt_absolute_time();
+
+	irqrestore(flags);
+
+	return ts;
+}
+
+/*
+ * Initalise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	sq_init(&callout_queue);
+	hrt_tim_init();
+
+#ifdef CONFIG_HRT_PPM
+	/* configure the PPM input pin */
+	stm32_configgpio(GPIO_PPM_IN);
+#endif
+}
+
+/*
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+/*
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	irqstate_t flags = irqsave();
+
+	/* if the entry is currently queued, remove it */
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+
+	irqrestore(flags);
+}
+
+/*
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	irqstate_t flags = irqsave();
+
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+
+	irqrestore(flags);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+static void
+hrt_call_invoke(void)
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	while (true) {
+		/* get the current time */
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == NULL)
+			break;
+
+		if (call->deadline > now)
+			break;
+
+		sq_rem(&call->link, &callout_queue);
+		//lldbg("call pop\n");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline;
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* invoke the callout (if there is one) */
+		if (call->callout) {
+			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			call->callout(call->arg);
+		}
+
+		/* if the callout has a non-zero period, it has to be re-entered */
+		if (call->period != 0) {
+			call->deadline = deadline + call->period;
+			hrt_call_enter(call);
+		}
+	}
+}
+
+/*
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+
+	//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
+
+	/* set the new compare value and remember it for latency tracking */
+	rCCR_HRT = latency_baseline = deadline & 0xffff;
+}
+
+static void
+hrt_latency_update(void)
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	/* bounded buckets */
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	/* catch-all at the end */
+	latency_counters[index]++;
+}
+
+
+#endif /* CONFIG_HRT_TIMER */
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
new file mode 100644
index 000000000..c1efb8515
--- /dev/null
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -0,0 +1,332 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 drv_pwm_servo.c
+ *
+ * Servo driver supporting PWM servos connected to STM32 timer blocks.
+ *
+ * Works with any of the 'generic' or 'advanced' STM32 timers that
+ * have output pins, does not require an interrupt.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "drv_pwm_servo.h"
+
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#define REG(_tmr, _reg)	(*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
+
+#define rCR1(_tmr)    	REG(_tmr, STM32_GTIM_CR1_OFFSET)
+#define rCR2(_tmr)    	REG(_tmr, STM32_GTIM_CR2_OFFSET)
+#define rSMCR(_tmr)   	REG(_tmr, STM32_GTIM_SMCR_OFFSET)
+#define rDIER(_tmr)   	REG(_tmr, STM32_GTIM_DIER_OFFSET)
+#define rSR(_tmr)     	REG(_tmr, STM32_GTIM_SR_OFFSET)
+#define rEGR(_tmr)    	REG(_tmr, STM32_GTIM_EGR_OFFSET)
+#define rCCMR1(_tmr)  	REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2(_tmr)  	REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
+#define rCCER(_tmr)   	REG(_tmr, STM32_GTIM_CCER_OFFSET)
+#define rCNT(_tmr)    	REG(_tmr, STM32_GTIM_CNT_OFFSET)
+#define rPSC(_tmr)    	REG(_tmr, STM32_GTIM_PSC_OFFSET)
+#define rARR(_tmr)    	REG(_tmr, STM32_GTIM_ARR_OFFSET)
+#define rCCR1(_tmr)   	REG(_tmr, STM32_GTIM_CCR1_OFFSET)
+#define rCCR2(_tmr)   	REG(_tmr, STM32_GTIM_CCR2_OFFSET)
+#define rCCR3(_tmr)   	REG(_tmr, STM32_GTIM_CCR3_OFFSET)
+#define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
+#define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
+#define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+
+static void		pwm_timer_init(unsigned timer);
+static void		pwm_timer_set_rate(unsigned timer, unsigned rate);
+static void		pwm_channel_init(unsigned channel);
+
+static void
+pwm_timer_init(unsigned timer)
+{
+	/* enable the timer clock before we try to talk to it */
+	modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit);
+
+	/* disable and configure the timer */
+	rCR1(timer) = 0;
+	rCR2(timer) = 0;
+	rSMCR(timer) = 0;
+	rDIER(timer) = 0;
+	rCCER(timer) = 0;
+	rCCMR1(timer) = 0;
+	rCCMR2(timer) = 0;
+	rCCER(timer) = 0;
+	rDCR(timer) = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
+
+	/* default to updating at 50Hz */
+	pwm_timer_set_rate(timer, 50);
+
+	/* note that the timer is left disabled - arming is performed separately */
+}
+
+static void
+pwm_timer_set_rate(unsigned timer, unsigned rate)
+{
+	/* configure the timer to update at the desired rate */
+	rARR(timer) = 1000000 / rate;
+
+	/* generate an update event; reloads the counter and all registers */
+	rEGR(timer) = GTIM_EGR_UG;
+}
+
+static void
+pwm_channel_init(unsigned channel)
+{
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* configure the GPIO first */
+	stm32_configgpio(pwm_channels[channel].gpio);
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
+		rCCR1(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC1E;
+		break;
+
+	case 2:
+		rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
+		rCCR2(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC2E;
+		break;
+
+	case 3:
+		rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
+		rCCR3(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC3E;
+		break;
+
+	case 4:
+		rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
+		rCCR4(timer) = pwm_channels[channel].default_value;
+		rCCER(timer) |= GTIM_CCER_CC4E;
+		break;
+	}
+}
+
+int
+up_pwm_servo_set(unsigned channel, servo_position_t value)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS)
+		return -1;
+
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].gpio == 0))
+		return -1;
+
+	/* configure the channel */
+	if (value > 0)
+		value--;
+
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		rCCR1(timer) = value;
+		break;
+
+	case 2:
+		rCCR2(timer) = value;
+		break;
+
+	case 3:
+		rCCR3(timer) = value;
+		break;
+
+	case 4:
+		rCCR4(timer) = value;
+		break;
+
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+servo_position_t
+up_pwm_servo_get(unsigned channel)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS)
+		return 0;
+
+	unsigned timer = pwm_channels[channel].timer_index;
+	servo_position_t value = 0;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].timer_channel == 0))
+		return 0;
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+	case 1:
+		value = rCCR1(timer);
+		break;
+
+	case 2:
+		value = rCCR2(timer);
+		break;
+
+	case 3:
+		value = rCCR3(timer);
+		break;
+
+	case 4:
+		value = rCCR4(timer);
+		break;
+	}
+
+	return value + 1;
+}
+
+int
+up_pwm_servo_init(uint32_t channel_mask)
+{
+	/* do basic timer initialisation first */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			pwm_timer_init(i);
+	}
+
+	/* now init channels */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		/* don't do init for disabled channels; this leaves the pin configs alone */
+		if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0))
+			pwm_channel_init(i);
+	}
+
+	return OK;
+}
+
+void
+up_pwm_servo_deinit(void)
+{
+	/* disable the timers */
+	up_pwm_servo_arm(false);
+}
+
+int
+up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
+{
+	/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
+	if (rate < 1)
+		return -ERANGE;
+	if (rate > 10000)
+		return -ERANGE;
+
+	if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
+		return ERROR;
+
+	pwm_timer_set_rate(group, rate);
+
+	return OK;
+}
+
+int
+up_pwm_servo_set_rate(unsigned rate)
+{
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
+		up_pwm_servo_set_rate_group_update(i, rate);
+
+	return 0;
+}
+
+uint32_t
+up_pwm_servo_get_rate_group(unsigned group)
+{
+	unsigned channels = 0;
+
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group))
+			channels |= (1 << i);
+	}
+	return channels;
+}
+
+void
+up_pwm_servo_arm(bool armed)
+{
+	/* iterate timers and arm/disarm appropriately */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0) {
+			if (armed) {
+				/* force an update to preload all registers */
+				rEGR(i) = GTIM_EGR_UG;
+
+				/* arm requires the timer be enabled */
+				rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
+
+			} else {
+				// XXX This leads to FMU PWM being still active
+				// but uncontrollable. Just disable the timer
+				// and risk a runt.
+				///* on disarm, just stop auto-reload so we don't generate runts */
+				//rCR1(i) &= ~GTIM_CR1_ARPE;
+				rCR1(i) = 0;
+			}
+		}
+	}
+}
diff --git a/src/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h
new file mode 100644
index 000000000..5dd4cf70c
--- /dev/null
+++ b/src/drivers/stm32/drv_pwm_servo.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 drv_pwm_servo.h
+ *
+ * stm32-specific PWM output data.
+ */
+
+#pragma once
+
+#include <drivers/drv_pwm_output.h>
+
+/* configuration limits */
+#define PWM_SERVO_MAX_TIMERS	4
+#define PWM_SERVO_MAX_CHANNELS	8
+
+/* array of timers dedicated to PWM servo use */
+struct pwm_servo_timer {
+	uint32_t	base;
+	uint32_t	clock_register;
+	uint32_t	clock_bit;
+	uint32_t	clock_freq;
+};
+
+/* array of channels in logical order */
+struct pwm_servo_channel {
+	uint32_t	gpio;
+	uint8_t		timer_index;
+	uint8_t		timer_channel;
+	servo_position_t default_value;
+};
+
+/* supplied by board-specific code */
+__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
+__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];
diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk
new file mode 100644
index 000000000..bb751c7f6
--- /dev/null
+++ b/src/drivers/stm32/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# STM32 driver support code
+#
+# Modules in this directory are compiled for all STM32 targets.
+#
+
+SRCS		= drv_hrt.c \
+		  drv_pwm_servo.c
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
new file mode 100644
index 000000000..827cf30b2
--- /dev/null
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# Tone alarm driver
+#
+
+MODULE_COMMAND	= tone_alarm
+
+SRCS		= tone_alarm.cpp
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
new file mode 100644
index 000000000..ac5511e60
--- /dev/null
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -0,0 +1,1018 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Driver for the PX4 audio alarm port, /dev/tone_alarm.
+ *
+ * The tone_alarm driver supports a set of predefined "alarm"
+ * tunes and one user-supplied tune.
+ *
+ * The TONE_SET_ALARM ioctl can be used to select a predefined
+ * alarm tune, from 1 - <TBD>.  Selecting tune zero silences
+ * the alarm.
+ *
+ * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY
+ * statement, with some exceptions and extensions.
+ *
+ * From Wikibooks:
+ *
+ * PLAY "[string expression]"
+ * 
+ * Used to play notes and a score ... The tones are indicated by letters A through G.
+ * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) 
+ * immediately after the note letter. See this example:
+ * 
+ *   PLAY "C C# C C#"
+ *
+ * Whitespaces are ignored inside the string expression. There are also codes that
+ * set the duration, octave and tempo. They are all case-insensitive. PLAY executes 
+ * the commands or notes the order in which they appear in the string. Any indicators 
+ * that change the properties are effective for the notes following that indicator.
+ *
+ * Ln     Sets the duration (length) of the notes. The variable n does not indicate an actual duration
+ *        amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc.
+ *        (L8, L16, L32, L64, ...). By default, n = 4.
+ *        For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively.
+ *        The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB"
+ *        can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes.
+ * On     Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B.
+ *        Remember that C- is equivalent to B. 
+ * < >    Changes the current octave respectively down or up one level.
+ * Nn     Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.)
+ *        Cannot use with sharp and flat. Cannot use with the shorthand notation neither.
+ * MN     Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode.
+ * ML     Stand for Music Legato. Note duration is full length of that indicated by Ln.
+ * MS     Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln.
+ * Pn     Causes a silence (pause) for the length of note indicated (same as Ln). 
+ * Tn     Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. 
+ * .      When placed after a note, it causes the duration of the note to be 3/2 of the set duration.
+ *        This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note.
+ *        It can be used for a pause as well.
+ *
+ * Extensions/variations:
+ *
+ * MB MF  The MF command causes the tune to play once and then stop. The MB command causes the
+ *        tune to repeat when it ends.
+ *
+ */
+
+#include <nuttx/config.h>
+#include <debug.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <math.h>
+#include <ctype.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include <arch/stm32/chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <systemlib/err.h>
+
+#ifndef CONFIG_HRT_TIMER
+# error This driver requires CONFIG_HRT_TIMER
+#endif
+
+/* Tone alarm configuration */
+#if   TONE_ALARM_TIMER == 2
+# define TONE_ALARM_BASE		STM32_TIM2_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM2_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM2EN
+# ifdef CONFIG_STM32_TIM2
+#  error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2
+# endif
+#elif TONE_ALARM_TIMER == 3
+# define TONE_ALARM_BASE		STM32_TIM3_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM3_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM3EN
+# ifdef CONFIG_STM32_TIM3
+#  error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3
+# endif
+#elif TONE_ALARM_TIMER == 4
+# define TONE_ALARM_BASE		STM32_TIM4_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM4_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM4EN
+# ifdef CONFIG_STM32_TIM4
+#  error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4
+# endif
+#elif TONE_ALARM_TIMER == 5
+# define TONE_ALARM_BASE		STM32_TIM5_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM5_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM5EN
+# ifdef CONFIG_STM32_TIM5
+#  error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
+# endif
+#elif TONE_ALARM_TIMER == 9
+# define TONE_ALARM_BASE		STM32_TIM9_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM9_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM9EN
+# ifdef CONFIG_STM32_TIM9
+#  error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9
+# endif
+#elif TONE_ALARM_TIMER == 10
+# define TONE_ALARM_BASE		STM32_TIM10_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM10_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM10EN
+# ifdef CONFIG_STM32_TIM10
+#  error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10
+# endif
+#elif TONE_ALARM_TIMER == 11
+# define TONE_ALARM_BASE		STM32_TIM11_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM11_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM11EN
+# ifdef CONFIG_STM32_TIM11
+#  error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
+# endif
+#else
+# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver.
+#endif
+
+#if TONE_ALARM_CHANNEL == 1
+# define TONE_CCMR1	(3 << 4)
+# define TONE_CCMR2	0
+# define TONE_CCER	(1 << 0)
+# define TONE_rCCR	rCCR1
+#elif TONE_ALARM_CHANNEL == 2
+# define TONE_CCMR1	(3 << 12)
+# define TONE_CCMR2	0
+# define TONE_CCER	(1 << 4)
+# define TONE_rCCR	rCCR2
+#elif TONE_ALARM_CHANNEL == 3
+# define TONE_CCMR1	0
+# define TONE_CCMR2	(3 << 4)
+# define TONE_CCER	(1 << 8)
+# define TONE_rCCR	rCCR3
+#elif TONE_ALARM_CHANNEL == 4
+# define TONE_CCMR1	0
+# define TONE_CCMR2	(3 << 12)
+# define TONE_CCER	(1 << 12)
+# define TONE_rCCR	rCCR4
+#else
+# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver.
+#endif
+
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+class ToneAlarm : public device::CDev
+{
+public:
+	ToneAlarm();
+	~ToneAlarm();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
+
+private:
+	static const unsigned	_tune_max = 1024; // be reasonable about user tunes
+	static const char	* const _default_tunes[];
+	static const unsigned	_default_ntunes;
+	static const uint8_t	_note_tab[];
+
+	const char		*_user_tune;
+
+	const char		*_tune;		// current tune string
+	const char		*_next;		// next note in the string
+
+	unsigned		_tempo;
+	unsigned		_note_length;
+	enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode;
+	unsigned		_octave;
+	unsigned		_silence_length; // if nonzero, silence before next note
+	bool			_repeat;	// if true, tune restarts at end
+
+	hrt_call		_note_call;	// HRT callout for note completion
+
+	// Convert a note value in the range C1 to B7 into a divisor for
+	// the configured timer's clock.
+	//
+	unsigned		note_to_divisor(unsigned note);
+
+	// Calculate the duration in microseconds of play and silence for a 
+	// note given the current tempo, length and mode and the number of 
+	// dots following in the play string.
+	//
+	unsigned		note_duration(unsigned &silence, unsigned note_length, unsigned dots);
+
+	// Calculate the duration in microseconds of a rest corresponding to
+	// a given note length.
+	//
+	unsigned		rest_duration(unsigned rest_length, unsigned dots);
+
+	// Start playing the note
+	//
+	void			start_note(unsigned note);
+
+	// Stop playing the current note and make the player 'safe'
+	//
+	void			stop_note();
+
+	// Start playing the tune
+	//
+	void			start_tune(const char *tune);
+
+	// Parse the next note out of the string and play it
+	//
+	void			next_note();
+
+	// Find the next character in the string, discard any whitespace and
+	// return the canonical (uppercase) version.
+	//
+	int			next_char();
+
+	// Extract a number from the string, consuming all the digit characters.
+	//
+	unsigned		next_number();
+
+	// Consume dot characters from the string, returning the number consumed.
+	//
+	unsigned		next_dots();
+
+	// hrt_call trampoline for next_note
+	//
+	static void		next_trampoline(void *arg);
+
+};
+
+// predefined tune array
+const char * const ToneAlarm::_default_tunes[] = {
+	"MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc",		// startup tune
+	"MBT200a8a8a8PaaaP",					// ERROR tone
+	"MFT200e8a8a",						// NotifyPositive tone
+	"MFT200e8e",						// NotifyNeutral tone
+	"MFT200e8c8e8c8e8c8",					// NotifyNegative tone
+	"MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge!
+	"MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16",	// dixie
+	"MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8",		// cucuracha
+	"MNT150L8O2GGABGBADGGABL4GL8F+",			// yankee
+	"MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy
+	"T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8"	// william tell
+	"O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16"
+	"O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16"
+	"O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8"
+	"P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8"
+	"O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16"
+	"O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4"
+	"O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8"
+	"O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16"
+	"O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8"
+	"O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16"
+	"O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8"
+	"O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8"
+	"O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
+	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
+	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
+	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
+	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16"
+	"O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
+	"O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8"
+	"O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16"
+	"O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16"
+	"O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16"
+	"O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16"
+	"O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8"
+	"O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8"
+	"O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16"
+	"O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64"
+	"O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16"
+	"O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16"
+	"O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16"
+	"O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16"
+	"O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16"
+	"O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
+	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
+	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
+	"O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16"
+	"O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16"
+	"O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16"
+	"O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16"
+	"O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16"
+	"O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16"
+	"O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16"
+	"O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16"
+	"O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16"
+	"O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16"
+	"O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16"
+	"O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16"
+	"O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16"
+	"O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16"
+	"O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16"
+	"O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16"
+	"O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16"
+	"O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16"
+	"O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16"
+	"O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16"
+	"O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16"
+	"O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16"
+	"O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16"
+	"O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16"
+	"O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
+	"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
+	"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
+	"O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16"
+	"O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4."
+	"O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16"
+	"O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16"
+	"O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16"
+	"O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16"
+	"O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8"
+	"O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16"
+	"O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
+	"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
+	"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
+	"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
+	"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8"
+	"O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
+	"O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8"
+	"O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8"
+	"O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16"
+	"O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16"
+	"O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8"
+	"O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8"
+	"O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16"
+	"O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16"
+	"O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16"
+	"O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16"
+	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
+	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16"
+	"O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16"
+	"O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16"
+	"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
+	"O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16"
+	"O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16"
+	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
+	"P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16"
+	"O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8"
+	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16"
+	"O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16"
+	"O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16"
+	"O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16"
+	"O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16"
+	"P16O4E16P16D+16P16C+16P16O3B16P16A+16P16"
+	"O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16"
+	"O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16"
+	"O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16"
+	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
+	"O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16"
+	"O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16"
+	"O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
+	"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
+	"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
+	"P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16"
+	"O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16"
+	"O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16"
+	"O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16"
+	"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8"
+	"O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16"
+	"O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16"
+	"O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8"
+	"O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16"
+	"O4E16P16D+16P16C+16P16O3B16P16A+16P16A16"
+	"P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16"
+	"O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8"
+	"O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8"
+	"O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16"
+	"O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16"
+	"O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16"
+	"O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16"
+	"O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16"
+	"O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8"
+	"O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16"
+	"O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16"
+	"O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8"
+	"O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8"
+	"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
+	"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
+	"O2E2P64",
+};
+
+const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
+
+// semitone offsets from C for the characters 'A'-'G'
+const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
+
+
+ToneAlarm::ToneAlarm() :
+	CDev("tone_alarm", "/dev/tone_alarm"),
+	_user_tune(nullptr),
+	_tune(nullptr),
+	_next(nullptr)
+{
+	// enable debug() calls
+	//_debug_enabled = true;
+}
+
+ToneAlarm::~ToneAlarm()
+{
+}
+
+int
+ToneAlarm::init()
+{
+	int ret;
+
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	/* configure the GPIO to the idle state */
+	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
+
+	/* clock/power on our timer */
+	modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
+
+	/* initialise the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = 0;
+	rCCER &= TONE_CCER;		/* unlock CCMR* registers */
+	rCCMR1 = TONE_CCMR1;
+	rCCMR2 = TONE_CCMR2;
+	rCCER = TONE_CCER;
+	rDCR = 0;
+
+	/* toggle the CC output each time the count passes 1 */
+	TONE_rCCR = 1;
+
+	/* default the timer to a prescale value of 1; playing notes will change this */
+	rPSC = 0;
+
+	/* make sure the timer is running */
+	rCR1 = GTIM_CR1_CEN;
+
+	debug("ready");
+	return OK;
+}
+
+unsigned
+ToneAlarm::note_to_divisor(unsigned note)
+{
+	// compute the frequency first (Hz)
+	float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f);
+
+	float period = 0.5f / freq;
+
+	// and the divisor, rounded to the nearest integer
+	unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f;
+
+	return divisor;
+}
+
+unsigned
+ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots)
+{
+	unsigned whole_note_period = (60 * 1000000 * 4) / _tempo;
+
+	if (note_length == 0)
+		note_length = 1;
+	unsigned note_period = whole_note_period / note_length;
+
+	switch (_note_mode) {
+	case MODE_NORMAL:
+		silence = note_period / 8;
+		break;
+	case MODE_STACCATO:
+		silence = note_period / 4;
+		break;
+	default:
+	case MODE_LEGATO:
+		silence = 0;
+		break;
+	}
+	note_period -= silence;
+
+	unsigned dot_extension = note_period / 2;
+	while (dots--) {
+		note_period += dot_extension;
+		dot_extension /= 2;
+	}
+
+	return note_period;
+}
+
+unsigned
+ToneAlarm::rest_duration(unsigned rest_length, unsigned dots)
+{
+	unsigned whole_note_period = (60 * 1000000 * 4) / _tempo;
+
+	if (rest_length == 0)
+		rest_length = 1;
+
+	unsigned rest_period = whole_note_period / rest_length;
+
+	unsigned dot_extension = rest_period / 2;
+	while (dots--) {
+		rest_period += dot_extension;
+		dot_extension /= 2;
+	}
+
+	return rest_period;
+}
+
+void
+ToneAlarm::start_note(unsigned note)
+{
+	// compute the divisor
+	unsigned divisor = note_to_divisor(note);
+
+	// pick the lowest prescaler value that we can use
+	// (note that the effective prescale value is 1 greater)
+	unsigned prescale = divisor / 65536;
+
+	// calculate the timer period for the selected prescaler value
+	unsigned period = (divisor / (prescale + 1)) - 1;
+
+	rPSC = prescale;	// load new prescaler
+	rARR = period;		// load new toggle period
+	rEGR = GTIM_EGR_UG;	// force a reload of the period
+	rCCER |= TONE_CCER;	// enable the output
+
+	// configure the GPIO to enable timer output
+	stm32_configgpio(GPIO_TONE_ALARM);
+}
+
+void
+ToneAlarm::stop_note()
+{
+	/* stop the current note */
+	rCCER &= ~TONE_CCER;
+
+	/*
+	 * Make sure the GPIO is not driving the speaker.
+	 */
+	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
+}
+
+void
+ToneAlarm::start_tune(const char *tune)
+{
+	// kill any current playback
+	hrt_cancel(&_note_call);
+
+	// record the tune
+	_tune = tune;
+	_next = tune;
+
+	// initialise player state
+	_tempo = 120;
+	_note_length = 4;
+	_note_mode = MODE_NORMAL;
+	_octave = 4;
+	_silence_length = 0;
+	_repeat = false;		// otherwise command-line tunes repeat forever...
+
+	// schedule a callback to start playing
+	hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this);
+}
+
+void
+ToneAlarm::next_note()
+{
+	// do we have an inter-note gap to wait for?
+	if (_silence_length > 0) {
+		stop_note();
+		hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this);
+		_silence_length = 0;
+		return;
+	}
+
+	// make sure we still have a tune - may be removed by the write / ioctl handler
+	if ((_next == nullptr) || (_tune == nullptr)) {
+		stop_note();
+		return;
+	}
+
+	// parse characters out of the string until we have resolved a note
+	unsigned note = 0;
+	unsigned note_length = _note_length;
+	unsigned duration;
+
+	while (note == 0) {
+		// we always need at least one character from the string
+		int c = next_char();
+		if (c == 0)
+			goto tune_end;
+		_next++;
+
+		switch (c) {
+		case 'L':	// select note length
+			_note_length = next_number();
+			if (_note_length < 1)
+				goto tune_error;
+			break;
+
+		case 'O':	// select octave
+			_octave = next_number();
+			if (_octave > 6)
+				_octave = 6;
+			break;
+
+		case '<':	// decrease octave
+			if (_octave > 0)
+				_octave--;
+			break;
+
+		case '>':	// increase octave
+			if (_octave < 6)
+				_octave++;
+			break;
+
+		case 'M':	// select inter-note gap
+			c = next_char();
+			if (c == 0)
+				goto tune_error;
+			_next++;
+			switch (c) {
+			case 'N':
+				_note_mode = MODE_NORMAL;
+				break;
+			case 'L':
+				_note_mode = MODE_LEGATO;
+				break;
+			case 'S':
+				_note_mode = MODE_STACCATO;
+				break;
+			case 'F':
+				_repeat = false;
+				break;
+			case 'B':
+				_repeat = true;
+				break;
+			default:
+				goto tune_error;
+			}
+			break;
+
+		case 'P':	// pause for a note length
+			stop_note();
+			hrt_call_after(&_note_call, 
+				(hrt_abstime)rest_duration(next_number(), next_dots()),
+				(hrt_callout)next_trampoline, 
+				this);
+			return;
+
+		case 'T': {	// change tempo
+			unsigned nt = next_number();
+
+			if ((nt >= 32) && (nt <= 255)) {
+				_tempo = nt;
+			} else {
+				goto tune_error;
+			}
+			break;
+		}
+
+		case 'N':	// play an arbitrary note
+			note = next_number();
+			if (note > 84)
+				goto tune_error;
+			if (note == 0) {
+				// this is a rest - pause for the current note length
+				hrt_call_after(&_note_call,
+					(hrt_abstime)rest_duration(_note_length, next_dots()),
+					(hrt_callout)next_trampoline, 
+					this);
+				return;				
+			}
+			break;
+
+		case 'A'...'G':	// play a note in the current octave
+			note = _note_tab[c - 'A'] + (_octave * 12) + 1;
+			c = next_char();
+			switch (c) {
+			case '#':	// up a semitone
+			case '+':
+				if (note < 84)
+					note++;
+				_next++;
+				break;
+			case '-':	// down a semitone
+				if (note > 1)
+					note--;
+				_next++;
+				break;
+			default:
+				// 0 / no next char here is OK
+				break;
+			}
+			// shorthand length notation
+			note_length = next_number();
+			if (note_length == 0)
+				note_length = _note_length;
+			break;
+
+		default:
+			goto tune_error;
+		}
+	}
+
+	// compute the duration of the note and the following silence (if any)
+	duration = note_duration(_silence_length, note_length, next_dots());
+
+	// start playing the note
+	start_note(note);
+
+	// and arrange a callback when the note should stop
+	hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this);
+	return;
+
+	// tune looks bad (unexpected EOF, bad character, etc.)
+tune_error:
+	lowsyslog("tune error\n");
+	_repeat = false;		// don't loop on error
+
+	// stop (and potentially restart) the tune
+tune_end:
+	stop_note();
+	if (_repeat)
+		start_tune(_tune);
+	return;
+}
+
+int
+ToneAlarm::next_char()
+{
+	while (isspace(*_next)) {
+		_next++;
+	}
+	return toupper(*_next);
+}
+
+unsigned
+ToneAlarm::next_number()
+{
+	unsigned number = 0;
+	int c;
+
+	for (;;) {
+		c = next_char();
+		if (!isdigit(c))
+			return number;
+		_next++;
+		number = (number * 10) + (c - '0');
+	}
+}
+
+unsigned
+ToneAlarm::next_dots()
+{
+	unsigned dots = 0;
+
+	while (next_char() == '.') {
+		_next++;
+		dots++;
+	}
+	return dots;
+}
+
+void
+ToneAlarm::next_trampoline(void *arg)
+{
+	ToneAlarm *ta = (ToneAlarm *)arg;
+
+	ta->next_note();
+}
+
+
+int
+ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int result = OK;
+
+	debug("ioctl %i %u", cmd, arg);
+
+//	irqstate_t flags = irqsave();
+
+	/* decide whether to increase the alarm level to cmd or leave it alone */
+	switch (cmd) {
+	case TONE_SET_ALARM:
+		debug("TONE_SET_ALARM %u", arg);
+
+		if (arg <= _default_ntunes) {
+			if (arg == 0) {
+				// stop the tune
+				_tune = nullptr;
+				_next = nullptr;
+			} else {
+				// play the selected tune
+				start_tune(_default_tunes[arg - 1]);
+			}
+		} else {
+			result = -EINVAL;
+		}
+
+		break;
+
+	default:
+		result = -ENOTTY;
+		break;
+	}
+
+//	irqrestore(flags);
+
+	/* give it to the superclass if we didn't like it */
+	if (result == -ENOTTY)
+		result = CDev::ioctl(filp, cmd, arg);
+
+	return result;
+}
+
+int
+ToneAlarm::write(file *filp, const char *buffer, size_t len)
+{
+	// sanity-check the buffer for length and nul-termination
+	if (len > _tune_max)
+		return -EFBIG;
+
+	// if we have an existing user tune, free it
+	if (_user_tune != nullptr) {
+
+		// if we are playing the user tune, stop
+		if (_tune == _user_tune) {
+			_tune = nullptr;
+			_next = nullptr;
+		}
+
+		// free the old user tune
+		free((void *)_user_tune);
+		_user_tune = nullptr;
+	}
+
+	// if the new tune is empty, we're done
+	if (buffer[0] == '\0')
+		return OK;
+
+	// allocate a copy of the new tune
+	_user_tune = strndup(buffer, len);
+	if (_user_tune == nullptr)
+		return -ENOMEM;
+
+	// and play it
+	start_tune(_user_tune);
+
+	return len;
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace
+{
+
+ToneAlarm	*g_dev;
+
+int
+play_tune(unsigned tune)
+{
+	int	fd, ret;
+
+	fd = open("/dev/tone_alarm", 0);
+
+	if (fd < 0)
+		err(1, "/dev/tone_alarm");
+
+	ret = ioctl(fd, TONE_SET_ALARM, tune);
+	close(fd);
+
+	if (ret != 0)
+		err(1, "TONE_SET_ALARM");
+
+	exit(0);
+}
+
+int
+play_string(const char *str)
+{
+	int	fd, ret;
+
+	fd = open("/dev/tone_alarm", O_WRONLY);
+
+	if (fd < 0)
+		err(1, "/dev/tone_alarm");
+
+	ret = write(fd, str, strlen(str) + 1);
+	close(fd);
+
+	if (ret < 0)
+		err(1, "play tune");
+	exit(0);
+}
+
+} // namespace
+
+int
+tone_alarm_main(int argc, char *argv[])
+{
+	unsigned tune;
+
+	/* start the driver lazily */
+	if (g_dev == nullptr) {
+		g_dev = new ToneAlarm;
+
+		if (g_dev == nullptr)
+			errx(1, "couldn't allocate the ToneAlarm driver");
+
+		if (g_dev->init() != OK) {
+			delete g_dev;
+			errx(1, "ToneAlarm init failed");
+		}
+	}
+
+	if ((argc > 1) && !strcmp(argv[1], "start"))
+		play_tune(1);
+
+	if ((argc > 1) && !strcmp(argv[1], "stop"))
+		play_tune(0);
+
+	if ((tune = strtol(argv[1], nullptr, 10)) != 0)
+		play_tune(tune);
+
+	/* if it looks like a PLAY string... */
+	if (strlen(argv[1]) > 2) {
+		const char *str = argv[1];
+		if (str[0] == 'M') {
+			play_string(str);
+		}
+	}
+
+	errx(1, "unrecognised command, try 'start', 'stop' or an alarm number");
+}
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
new file mode 100644
index 000000000..670b8d635
--- /dev/null
+++ b/src/modules/px4iofirmware/adc.c
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 adc.c
+ *
+ * Simple ADC support for PX4IO on STM32.
+ */
+#include <nuttx/config.h>
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+
+#define DEBUG
+#include "px4io.h"
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
+
+#define rSR		REG(STM32_ADC_SR_OFFSET)
+#define rCR1		REG(STM32_ADC_CR1_OFFSET)
+#define rCR2		REG(STM32_ADC_CR2_OFFSET)
+#define rSMPR1		REG(STM32_ADC_SMPR1_OFFSET)
+#define rSMPR2		REG(STM32_ADC_SMPR2_OFFSET)
+#define rJOFR1		REG(STM32_ADC_JOFR1_OFFSET)
+#define rJOFR2		REG(STM32_ADC_JOFR2_OFFSET)
+#define rJOFR3		REG(STM32_ADC_JOFR3_OFFSET)
+#define rJOFR4		REG(STM32_ADC_JOFR4_OFFSET)
+#define rHTR		REG(STM32_ADC_HTR_OFFSET)
+#define rLTR		REG(STM32_ADC_LTR_OFFSET)
+#define rSQR1		REG(STM32_ADC_SQR1_OFFSET)
+#define rSQR2		REG(STM32_ADC_SQR2_OFFSET)
+#define rSQR3		REG(STM32_ADC_SQR3_OFFSET)
+#define rJSQR		REG(STM32_ADC_JSQR_OFFSET)
+#define rJDR1		REG(STM32_ADC_JDR1_OFFSET)
+#define rJDR2		REG(STM32_ADC_JDR2_OFFSET)
+#define rJDR3		REG(STM32_ADC_JDR3_OFFSET)
+#define rJDR4		REG(STM32_ADC_JDR4_OFFSET)
+#define rDR		REG(STM32_ADC_DR_OFFSET)
+
+perf_counter_t		adc_perf;
+
+int
+adc_init(void)
+{
+	adc_perf = perf_alloc(PC_ELAPSED, "adc");
+
+	/* do calibration if supported */
+#ifdef ADC_CR2_CAL
+	rCR2 |= ADC_CR2_RSTCAL;
+	up_udelay(1);
+
+	if (rCR2 & ADC_CR2_RSTCAL)
+		return -1;
+
+	rCR2 |= ADC_CR2_CAL;
+	up_udelay(100);
+
+	if (rCR2 & ADC_CR2_CAL)
+		return -1;
+
+#endif
+
+	/* arbitrarily configure all channels for 55 cycle sample time */
+	rSMPR1 = 0b00000011011011011011011011011011;
+	rSMPR2 = 0b00011011011011011011011011011011;
+
+	/* XXX for F2/4, might want to select 12-bit mode? */
+	rCR1 = 0;
+
+	/* enable the temperature sensor / Vrefint channel if supported*/
+	rCR2 =
+#ifdef ADC_CR2_TSVREFE
+		/* enable the temperature sensor in CR2 */
+		ADC_CR2_TSVREFE |
+#endif
+		0;
+
+#ifdef ADC_CCR_TSVREFE
+	/* enable temperature sensor in CCR */
+	rCCR = ADC_CCR_TSVREFE;
+#endif
+
+	/* configure for a single-channel sequence */
+	rSQR1 = 0;
+	rSQR2 = 0;
+	rSQR3 = 0;	/* will be updated with the channel each tick */
+
+	/* power-cycle the ADC and turn it on */
+	rCR2 &= ~ADC_CR2_ADON;
+	up_udelay(10);
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+
+	return 0;
+}
+
+uint16_t
+adc_measure(unsigned channel)
+{
+	perf_begin(adc_perf);
+
+	/* clear any previous EOC */
+	if (rSR & ADC_SR_EOC)
+		rSR &= ~ADC_SR_EOC;
+
+	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
+	rSQR3 = channel;
+	rCR2 |= ADC_CR2_ADON;
+
+	/* wait for the conversion to complete */
+	hrt_abstime now = hrt_absolute_time();
+
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* never spin forever - this will give a bogus result though */
+		if (hrt_elapsed_time(&now) > 1000) {
+			debug("adc timeout");
+			break;
+		}
+	}
+
+	/* read the result and clear EOC */
+	uint16_t result = rDR;
+
+	perf_end(adc_perf);
+	return result;
+}
\ No newline at end of file
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
new file mode 100644
index 000000000..dc36f6c93
--- /dev/null
+++ b/src/modules/px4iofirmware/controls.c
@@ -0,0 +1,332 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 controls.c
+ *
+ * R/C inputs and servo outputs.
+ */
+
+#include <nuttx/config.h>
+#include <stdbool.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/ppm_decode.h>
+
+#include "px4io.h"
+
+#define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
+#define RC_CHANNEL_HIGH_THRESH		5000
+#define RC_CHANNEL_LOW_THRESH		-5000
+
+static bool	ppm_input(uint16_t *values, uint16_t *num_values);
+
+static perf_counter_t c_gather_dsm;
+static perf_counter_t c_gather_sbus;
+static perf_counter_t c_gather_ppm;
+
+void
+controls_init(void)
+{
+	/* DSM input */
+	dsm_init("/dev/ttyS0");
+
+	/* S.bus input */
+	sbus_init("/dev/ttyS2");
+
+	/* default to a 1:1 input map, all enabled */
+	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN]        = 1000;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER]     = 1500;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX]        = 2000;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE]   = 30;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
+		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+	}
+
+	c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
+	c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
+	c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
+}
+
+void
+controls_tick() {
+
+	/*
+	 * Gather R/C control inputs from supported sources.
+	 *
+	 * Note that if you're silly enough to connect more than
+	 * one control input source, they're going to fight each
+	 * other.  Don't do that.
+	 */
+
+	perf_begin(c_gather_dsm);
+	bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
+	if (dsm_updated)
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+	perf_end(c_gather_dsm);
+
+	perf_begin(c_gather_sbus);
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
+	if (sbus_updated)
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+	perf_end(c_gather_sbus);
+
+	/*
+	 * XXX each S.bus frame will cause a PPM decoder interrupt
+	 * storm (lots of edges).  It might be sensible to actually
+	 * disable the PPM decoder completely if we have S.bus signal.
+	 */
+	perf_begin(c_gather_ppm);
+	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
+	if (ppm_updated)
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+	perf_end(c_gather_ppm);
+
+	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+
+	/*
+	 * In some cases we may have received a frame, but input has still
+	 * been lost.
+	 */
+	bool rc_input_lost = false;
+
+	/*
+	 * If we received a new frame from any of the RC sources, process it.
+	 */
+	if (dsm_updated || sbus_updated || ppm_updated) {
+
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp = hrt_absolute_time();
+
+		/* record a bitmask of channels assigned */
+		unsigned assigned_channels = 0;
+
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+				uint16_t raw = r_raw_rc_values[i];
+
+				int16_t scaled;
+
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+					raw = conf[PX4IO_P_RC_CONFIG_MIN];
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}
+
+				/* invert channel if requested */
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+					scaled = -scaled;
+
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				ASSERT(mapped < MAX_CONTROL_CHANNELS);
+
+				/* invert channel if pitch - pulling the lever down means pitching up by convention */
+				if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+					scaled = -scaled;
+
+				r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+				assigned_channels |= (1 << mapped);
+			}
+		}
+
+		/* set un-assigned controls to zero */
+		for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+			if (!(assigned_channels & (1 << i)))
+				r_rc_values[i] = 0;
+		}
+
+		/*
+		 * If we got an update with zero channels, treat it as 
+		 * a loss of input.
+		 *
+		 * This might happen if a protocol-based receiver returns an update
+		 * that contains no channels that we have mapped.
+		 */
+		if (assigned_channels == 0) {
+			rc_input_lost = true;
+		} else {
+			/* set RC OK flag and clear RC lost alarm */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+			r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
+		}
+
+		/*
+		 * Export the valid channel bitmap
+		 */
+		r_rc_valid = assigned_channels;
+	}
+
+	/*
+	 * If we haven't seen any new control data in 200ms, assume we
+	 * have lost input.
+	 */
+	if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+		rc_input_lost = true;
+
+		/* clear the input-kind flags here */
+		r_status_flags &= ~(
+			PX4IO_P_STATUS_FLAGS_RC_PPM |
+			PX4IO_P_STATUS_FLAGS_RC_DSM |
+			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+	}
+
+	/*
+	 * Handle losing RC input
+	 */
+	if (rc_input_lost) {
+
+		/* Clear the RC input status flag, clear manual override flag */
+		r_status_flags &= ~(
+			PX4IO_P_STATUS_FLAGS_OVERRIDE |
+			PX4IO_P_STATUS_FLAGS_RC_OK);
+
+		/* Set the RC_LOST alarm */
+		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+
+		/* Mark the arrays as empty */
+		r_raw_rc_count = 0;
+		r_rc_valid = 0;
+	}
+
+	/*
+	 * Check for manual override.
+	 *
+	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
+	 * must have R/C input.
+	 * Override is enabled if either the hardcoded channel / value combination
+	 * is selected, or the AP has requested it.
+	 */
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+		bool override = false;
+
+		/*
+		 * Check mapped channel 5 (can be any remote channel,
+		 * depends on RC_MAP_OVER parameter);
+		 * If the value is 'high' then the pilot has
+		 * requested override.
+		 *
+		 */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+			override = true;
+
+		if (override) {
+
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
+
+			/* mix new RC input control values to servos */
+			if (dsm_updated || sbus_updated || ppm_updated)
+				mixer_tick();
+
+		} else {
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		}
+	}
+}
+
+static bool
+ppm_input(uint16_t *values, uint16_t *num_values)
+{
+	bool result = false;
+
+	/* avoid racing with PPM updates */
+	irqstate_t state = irqsave();
+
+	/*
+	 * If we have received a new PPM frame within the last 200ms, accept it
+	 * and then invalidate it.
+	 */
+	if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
+
+		/* PPM data exists, copy it */
+		*num_values = ppm_decoded_channels;
+		if (*num_values > MAX_CONTROL_CHANNELS)
+			*num_values = MAX_CONTROL_CHANNELS;
+
+		for (unsigned i = 0; i < *num_values; i++)
+			values[i] = ppm_buffer[i];
+
+		/* clear validity */
+		ppm_last_valid_decode = 0;
+
+		/* good if we got any channels */
+		result = (*num_values > 0);
+	}
+
+	irqrestore(state);
+
+	return result;
+}
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
new file mode 100644
index 000000000..ea35e5513
--- /dev/null
+++ b/src/modules/px4iofirmware/dsm.c
@@ -0,0 +1,356 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 dsm.c
+ *
+ * Serial protocol decoder for the Spektrum DSM* family of protocols.
+ *
+ * Decodes into the global PPM buffer and updates accordingly.
+ */
+
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <drivers/drv_hrt.h>
+
+#define DEBUG
+
+#include "px4io.h"
+
+#define DSM_FRAME_SIZE		16
+#define DSM_FRAME_CHANNELS	7
+
+static int dsm_fd = -1;
+
+static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
+
+static uint8_t	frame[DSM_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+static unsigned	channel_shift;
+
+unsigned dsm_frame_drops;
+
+static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
+static void dsm_guess_format(bool reset);
+static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
+
+int
+dsm_init(const char *device)
+{
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
+
+	if (dsm_fd >= 0) {
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		partial_frame_count = 0;
+		last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+		debug("DSM: open failed");
+	}
+
+	return dsm_fd;
+}
+
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 * The DSM* protocol doesn't provide any explicit framing,
+	 * so we detect frame boundaries by the inter-frame delay.
+	 *
+	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
+	 * frame transmission time is ~1.4ms.
+	 *
+	 * We expect to only be called when bytes arrive for processing,
+	 * and if an interval of more than 5ms passes between calls,
+	 * the first byte we read will be the first byte of a frame.
+	 *
+	 * In the case where byte(s) are dropped from a frame, this also
+	 * provides a degree of protection. Of course, it would be better
+	 * if we didn't drop bytes...
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - last_rx_time) > 5000) {
+		if (partial_frame_count > 0) {
+			dsm_frame_drops++;
+			partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current frame.
+	 */
+	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	last_rx_time = now;
+
+	/*
+	 * Add bytes to the current frame
+	 */
+	partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full frame, return
+	 */
+	if (partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}
+
+static bool
+dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
+{
+
+	if (raw == 0xffff)
+		return false;
+
+	*channel = (raw >> shift) & 0xf;
+
+	uint16_t data_mask = (1 << shift) - 1;
+	*value = raw & data_mask;
+
+	//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
+
+	return true;
+}
+
+static void
+dsm_guess_format(bool reset)
+{
+	static uint32_t	cs10;
+	static uint32_t	cs11;
+	static unsigned samples;
+
+	/* reset the 10/11 bit sniffed channel masks */
+	if (reset) {
+		cs10 = 0;
+		cs11 = 0;
+		samples = 0;
+		channel_shift = 0;
+		return;
+	}
+
+	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
+
+		uint8_t *dp = &frame[2 + (2 * i)];
+		uint16_t raw = (dp[0] << 8) | dp[1];
+		unsigned channel, value;
+
+		/* if the channel decodes, remember the assigned number */
+		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
+			cs10 |= (1 << channel);
+
+		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
+			cs11 |= (1 << channel);
+
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+	}
+
+	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	if (samples++ < 5)
+		return;
+
+	/*
+	 * Iterate the set of sensible sniffed channel sets and see whether
+	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 *
+	 * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
+	 *     stream, we may want to sniff for longer in some cases when we think we
+	 *     are talking to a DSM2 receiver in high-resolution mode (so that we can
+	 *     reject it, ideally).
+	 *     See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
+	 *     of this issue.
+	 */
+	static uint32_t masks[] = {
+		0x3f,	/* 6 channels (DX6) */
+		0x7f,	/* 7 channels (DX7) */
+		0xff,	/* 8 channels (DX8) */
+		0x1ff,	/* 9 channels (DX9, etc.) */
+		0x3ff,	/* 10 channels (DX10) */
+		0x3fff	/* 18 channels (DX10) */
+	};
+	unsigned votes10 = 0;
+	unsigned votes11 = 0;
+
+	for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {
+
+		if (cs10 == masks[i])
+			votes10++;
+
+		if (cs11 == masks[i])
+			votes11++;
+	}
+
+	if ((votes11 == 1) && (votes10 == 0)) {
+		channel_shift = 11;
+		debug("DSM: 11-bit format");
+		return;
+	}
+
+	if ((votes10 == 1) && (votes11 == 0)) {
+		channel_shift = 10;
+		debug("DSM: 10-bit format");
+		return;
+	}
+
+	/* call ourselves to reset our state ... we have to try again */
+	debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
+	dsm_guess_format(true);
+}
+
+static bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+{
+
+	/*
+		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	*/
+	/*
+	 * If we have lost signal for at least a second, reset the
+	 * format guessing heuristic.
+	 */
+	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+		dsm_guess_format(true);
+
+	/* we have received something we think is a frame */
+	last_frame_time = frame_time;
+
+	/* if we don't know the frame format, update the guessing state machine */
+	if (channel_shift == 0) {
+		dsm_guess_format(false);
+		return false;
+	}
+
+	/*
+	 * The encoding of the first two bytes is uncertain, so we're
+	 * going to ignore them for now.
+	 *
+	 * Each channel is a 16-bit unsigned value containing either a 10-
+	 * or 11-bit channel value and a 4-bit channel number, shifted
+	 * either 10 or 11 bits. The MSB may also be set to indicate the
+	 * second frame in variants of the protocol where more than
+	 * seven channels are being transmitted.
+	 */
+
+	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
+
+		uint8_t *dp = &frame[2 + (2 * i)];
+		uint16_t raw = (dp[0] << 8) | dp[1];
+		unsigned channel, value;
+
+		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+			continue;
+
+		/* ignore channels out of range */
+		if (channel >= PX4IO_INPUT_CHANNELS)
+			continue;
+
+		/* update the decoded channel count */
+		if (channel >= *num_values)
+			*num_values = channel + 1;
+
+		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
+		if (channel_shift == 11)
+			value /= 2;
+
+		value += 998;
+
+		/*
+		 * Store the decoded channel into the R/C input buffer, taking into
+		 * account the different ideas about channel assignement that we have.
+		 *
+		 * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
+		 * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
+		 */
+		switch (channel) {
+		case 0:
+			channel = 2;
+			break;
+
+		case 1:
+			channel = 0;
+			break;
+
+		case 2:
+			channel = 1;
+
+		default:
+			break;
+		}
+
+		values[channel] = value;
+	}
+
+	/*
+	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
+	 */
+	return true;
+}
diff --git a/src/modules/px4iofirmware/hx_stream.c b/src/modules/px4iofirmware/hx_stream.c
new file mode 100644
index 000000000..8d77f14a8
--- /dev/null
+++ b/src/modules/px4iofirmware/hx_stream.c
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 hx_stream.c
+ *
+ * A simple serial line framing protocol based on HDLC
+ * with 32-bit CRC protection.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <crc32.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "perf_counter.h"
+
+#include "hx_stream.h"
+
+
+struct hx_stream {
+	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		frame_bytes;
+	bool			escaped;
+	bool			txerror;
+
+	int			fd;
+	hx_stream_rx_callback	callback;
+	void			*callback_arg;
+
+	perf_counter_t		pc_tx_frames;
+	perf_counter_t		pc_rx_frames;
+	perf_counter_t		pc_rx_errors;
+};
+
+/*
+ * Protocol magic numbers, straight out of HDLC.
+ */
+#define FBO	0x7e	/**< Frame Boundary Octet */
+#define CEO	0x7c	/**< Control Escape Octet */
+
+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 void
+hx_tx_raw(hx_stream_t stream, uint8_t c)
+{
+	if (write(stream->fd, &c, 1) != 1)
+		stream->txerror = true;
+}
+
+static void
+hx_tx_byte(hx_stream_t stream, uint8_t c)
+{
+	switch (c) {
+	case FBO:
+	case CEO:
+		hx_tx_raw(stream, CEO);
+		c ^= 0x20;
+		break;
+	}
+
+	hx_tx_raw(stream, c);
+}
+
+static int
+hx_rx_frame(hx_stream_t stream)
+{
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} u;
+	unsigned length = stream->frame_bytes;
+
+	/* reset the stream */
+	stream->frame_bytes = 0;
+	stream->escaped = false;
+
+	/* not a real frame - too short */
+	if (length < 4) {
+		if (length > 1)
+			perf_count(stream->pc_rx_errors);
+
+		return 0;
+	}
+
+	length -= 4;
+
+	/* compute expected CRC */
+	u.w = crc32(&stream->buf[0], length);
+
+	/* compare computed and actual CRC */
+	for (unsigned i = 0; i < 4; i++) {
+		if (u.b[i] != stream->buf[length + i]) {
+			perf_count(stream->pc_rx_errors);
+			return 0;
+		}
+	}
+
+	/* frame is good */
+	perf_count(stream->pc_rx_frames);
+	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	return 1;
+}
+
+hx_stream_t
+hx_stream_init(int fd,
+	       hx_stream_rx_callback callback,
+	       void *arg)
+{
+	hx_stream_t stream;
+
+	stream = (hx_stream_t)malloc(sizeof(struct hx_stream));
+
+	if (stream != NULL) {
+		memset(stream, 0, sizeof(struct hx_stream));
+		stream->fd = fd;
+		stream->callback = callback;
+		stream->callback_arg = arg;
+	}
+
+	return stream;
+}
+
+void
+hx_stream_free(hx_stream_t stream)
+{
+	/* free perf counters (OK if they are NULL) */
+	perf_free(stream->pc_tx_frames);
+	perf_free(stream->pc_rx_frames);
+	perf_free(stream->pc_rx_errors);
+
+	free(stream);
+}
+
+void
+hx_stream_set_counters(hx_stream_t stream,
+		       perf_counter_t tx_frames,
+		       perf_counter_t rx_frames,
+		       perf_counter_t rx_errors)
+{
+	stream->pc_tx_frames = tx_frames;
+	stream->pc_rx_frames = rx_frames;
+	stream->pc_rx_errors = rx_errors;
+}
+
+int
+hx_stream_send(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)
+		return -EINVAL;
+
+	/* start the frame */
+	hx_tx_raw(stream, FBO);
+
+	/* transmit the data */
+	while (resid--)
+		hx_tx_byte(stream, *p++);
+
+	/* compute the CRC */
+	u.w = crc32(data, count);
+
+	/* send the CRC */
+	p = &u.b[0];
+	resid = 4;
+
+	while (resid--)
+		hx_tx_byte(stream, *p++);
+
+	/* and the trailing frame separator */
+	hx_tx_raw(stream, FBO);
+
+	/* check for transmit error */
+	if (stream->txerror) {
+		stream->txerror = false;
+		return -EIO;
+	}
+
+	perf_count(stream->pc_tx_frames);
+	return 0;
+}
+
+void
+hx_stream_rx(hx_stream_t stream, uint8_t c)
+{
+	/* frame end? */
+	if (c == FBO) {
+		hx_rx_frame(stream);
+		return;
+	}
+
+	/* escaped? */
+	if (stream->escaped) {
+		stream->escaped = false;
+		c ^= 0x20;
+
+	} else if (c == CEO) {
+		/* now escaped, ignore the byte */
+		stream->escaped = true;
+		return;
+	}
+
+	/* save for later */
+	if (stream->frame_bytes < sizeof(stream->buf))
+		stream->buf[stream->frame_bytes++] = c;
+}
diff --git a/src/modules/px4iofirmware/hx_stream.h b/src/modules/px4iofirmware/hx_stream.h
new file mode 100644
index 000000000..128689953
--- /dev/null
+++ b/src/modules/px4iofirmware/hx_stream.h
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 hx_stream.h
+ *
+ * A simple serial line framing protocol based on HDLC
+ * with 32-bit CRC protection.
+ */
+
+#ifndef _SYSTEMLIB_HX_STREAM_H
+#define _SYSTEMLIB_HX_STREAM_H
+
+#include <sys/types.h>
+
+#include "perf_counter.h"
+
+struct hx_stream;
+typedef struct hx_stream *hx_stream_t;
+
+#define HX_STREAM_MAX_FRAME	64
+
+typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length);
+
+__BEGIN_DECLS
+
+/**
+ * Allocate a new hx_stream object.
+ *
+ * @param fd		The file handle over which the protocol will
+ *			communicate.
+ * @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
+ *			not be allocated.
+ */
+__EXPORT extern hx_stream_t	hx_stream_init(int fd,
+		hx_stream_rx_callback callback,
+		void *arg);
+
+/**
+ * Free a hx_stream object.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_free(hx_stream_t stream);
+
+/**
+ * Set performance counters for the stream.
+ *
+ * Any counter may be set to NULL to disable counting that datum.
+ *
+ * @param tx_frames	Counter for transmitted frames.
+ * @param rx_frames	Counter for received frames.
+ * @param rx_errors	Counter for short and corrupt received frames.
+ */
+__EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
+		perf_counter_t tx_frames,
+		perf_counter_t rx_frames,
+		perf_counter_t rx_errors);
+
+/**
+ * Send a frame.
+ *
+ * This function will block until all frame bytes are sent if
+ * the descriptor passed to hx_stream_init is marked blocking,
+ * otherwise it will return -1 (but may transmit a
+ * runt frame at the same time).
+ *
+ * @todo Handling of non-blocking streams needs to be better.
+ *
+ * @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_send(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Handle a byte from the stream.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param c		The character to process.
+ */
+__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
+		uint8_t c);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
new file mode 100644
index 000000000..4485daa5b
--- /dev/null
+++ b/src/modules/px4iofirmware/i2c.c
@@ -0,0 +1,340 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 i2c.c
+ *
+ * I2C communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <stm32_i2c.h>
+#include <stm32_dma.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+/*
+ * I2C register definitions.
+ */
+#define I2C_BASE	STM32_I2C1_BASE
+
+#define REG(_reg)	(*(volatile uint32_t *)(I2C_BASE + _reg))
+
+#define rCR1		REG(STM32_I2C_CR1_OFFSET)
+#define rCR2		REG(STM32_I2C_CR2_OFFSET)
+#define rOAR1		REG(STM32_I2C_OAR1_OFFSET)
+#define rOAR2		REG(STM32_I2C_OAR2_OFFSET)
+#define rDR		REG(STM32_I2C_DR_OFFSET)
+#define rSR1		REG(STM32_I2C_SR1_OFFSET)
+#define rSR2		REG(STM32_I2C_SR2_OFFSET)
+#define rCCR		REG(STM32_I2C_CCR_OFFSET)
+#define rTRISE		REG(STM32_I2C_TRISE_OFFSET)
+
+static int		i2c_interrupt(int irq, void *context);
+static void		i2c_rx_setup(void);
+static void		i2c_tx_setup(void);
+static void		i2c_rx_complete(void);
+static void		i2c_tx_complete(void);
+
+static DMA_HANDLE	rx_dma;
+static DMA_HANDLE	tx_dma;
+
+static uint8_t		rx_buf[68];
+static unsigned		rx_len;
+
+static const uint8_t	junk_buf[] = { 0xff, 0xff, 0xff, 0xff };
+
+static const uint8_t	*tx_buf = junk_buf;
+static unsigned		tx_len = sizeof(junk_buf);
+unsigned		tx_count;
+
+static uint8_t		selected_page;
+static uint8_t		selected_offset;
+
+enum {
+	DIR_NONE = 0,
+	DIR_TX = 1,
+	DIR_RX = 2
+} direction;
+
+void
+i2c_init(void)
+{
+	debug("i2c init");
+
+	/* allocate DMA handles and initialise DMA */
+	rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX);
+	i2c_rx_setup();
+	tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX);
+	i2c_tx_setup();
+
+	/* enable the i2c block clock and reset it */
+	modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN);
+	modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
+	modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
+
+	/* configure the i2c GPIOs */
+	stm32_configgpio(GPIO_I2C1_SCL);
+	stm32_configgpio(GPIO_I2C1_SDA);
+
+	/* soft-reset the block */
+	rCR1 |= I2C_CR1_SWRST;
+	rCR1 = 0;
+
+	/* set for DMA operation */
+	rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+	/* set the frequency value in CR2 */
+	rCR2 &= ~I2C_CR2_FREQ_MASK;
+	rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+	/* set divisor and risetime for fast mode */
+	uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+	if (result < 1)
+		result = 1;
+	result = 3;
+	rCCR &= ~I2C_CCR_CCR_MASK;
+	rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+	rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+	/* set our device address */
+	rOAR1 = 0x1a << 1;
+
+	/* enable event interrupts */
+	irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt);
+	irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt);
+	up_enable_irq(STM32_IRQ_I2C1EV);
+	up_enable_irq(STM32_IRQ_I2C1ER);
+
+	/* and enable the I2C port */
+	rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+
+#ifdef DEBUG
+	i2c_dump();
+#endif
+}
+
+
+/*
+  reset the I2C bus
+  used to recover from lockups
+ */
+void i2c_reset(void)
+{
+	rCR1 |= I2C_CR1_SWRST;
+	rCR1 = 0;
+
+	/* set for DMA operation */
+	rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+	/* set the frequency value in CR2 */
+	rCR2 &= ~I2C_CR2_FREQ_MASK;
+	rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+	/* set divisor and risetime for fast mode */
+	uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+	if (result < 1)
+		result = 1;
+	result = 3;
+	rCCR &= ~I2C_CCR_CCR_MASK;
+	rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+	rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+	/* set our device address */
+	rOAR1 = 0x1a << 1;
+
+	/* and enable the I2C port */
+	rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+}
+
+static int
+i2c_interrupt(int irq, FAR void *context)
+{
+	uint16_t sr1 = rSR1;
+
+	if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) {
+
+		if (sr1 & I2C_SR1_STOPF) {
+			/* write to CR1 to clear STOPF */
+			(void)rSR1;		/* as recommended, re-read SR1 */
+			rCR1 |= I2C_CR1_PE;
+		}
+
+		/* DMA never stops, so we should do that now */
+		switch (direction) {
+		case DIR_TX:
+			i2c_tx_complete();
+			break;
+		case DIR_RX:
+			i2c_rx_complete();
+			break;
+		default:
+			/* not currently transferring - must be a new txn */
+			break;
+		}
+		direction = DIR_NONE;
+	}
+
+	if (sr1 & I2C_SR1_ADDR) {
+
+		/* clear ADDR to ack our selection and get direction */
+		(void)rSR1;		/* as recommended, re-read SR1 */
+		uint16_t sr2 = rSR2;
+
+		if (sr2 & I2C_SR2_TRA) {
+			/* we are the transmitter */
+
+			direction = DIR_TX;
+
+		} else {
+			/* we are the receiver */
+
+			direction = DIR_RX;
+		}
+	}
+
+	/* clear any errors that might need it (this handles AF as well */
+	if (sr1 & I2C_SR1_ERRORMASK)
+		rSR1 = 0;
+
+	return 0;
+}
+
+static void
+i2c_rx_setup(void)
+{
+	/*
+	 * Note that we configure DMA in circular mode; this means that a too-long
+	 * transfer will overwrite the buffer, but that avoids us having to deal with
+	 * bailing out of a transaction while the master is still babbling at us.
+	 */
+	rx_len = 0;
+	stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf),
+		DMA_CCR_CIRC |
+		DMA_CCR_MINC |
+		DMA_CCR_PSIZE_32BITS |
+		DMA_CCR_MSIZE_8BITS |
+		DMA_CCR_PRIMED);
+
+	stm32_dmastart(rx_dma, NULL, NULL, false);			
+}
+
+static void
+i2c_rx_complete(void)
+{
+	rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma);
+	stm32_dmastop(rx_dma);
+
+	if (rx_len >= 2) {
+		selected_page = rx_buf[0];
+		selected_offset = rx_buf[1];
+
+		/* work out how many registers are being written */
+		unsigned count = (rx_len - 2) / 2;
+		if (count > 0) {
+			registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
+		} else {
+			/* no registers written, must be an address cycle */
+			uint16_t *regs;
+			unsigned reg_count;
+
+			/* work out which registers are being addressed */
+			int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
+			if (ret == 0) {
+				tx_buf = (uint8_t *)regs;
+				tx_len = reg_count * 2;
+			} else {
+				tx_buf = junk_buf;
+				tx_len = sizeof(junk_buf);
+			}
+
+			/* disable interrupts while reconfiguring DMA for the selected registers */
+			irqstate_t flags = irqsave();
+
+			stm32_dmastop(tx_dma);
+			i2c_tx_setup();
+
+			irqrestore(flags);
+		}
+	}
+
+	/* prepare for the next transaction */
+	i2c_rx_setup();
+}
+
+static void
+i2c_tx_setup(void)
+{
+	/*
+	 * Note that we configure DMA in circular mode; this means that a too-long
+	 * transfer will copy the buffer more than once, but that avoids us having
+	 * to deal with bailing out of a transaction while the master is still 
+	 * babbling at us.
+	 */
+	stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len,
+		DMA_CCR_DIR |
+		DMA_CCR_CIRC |
+		DMA_CCR_MINC |
+		DMA_CCR_PSIZE_8BITS |
+		DMA_CCR_MSIZE_8BITS |
+		DMA_CCR_PRIMED);
+
+	stm32_dmastart(tx_dma, NULL, NULL, false);
+}
+
+static void
+i2c_tx_complete(void)
+{
+	tx_count = tx_len - stm32_dmaresidual(tx_dma);
+	stm32_dmastop(tx_dma);
+
+	/* for debug purposes, save the length of the last transmit as seen by the DMA */
+
+	/* leave tx_buf/tx_len alone, so that a retry will see the same data */
+
+	/* prepare for the next transaction */
+	i2c_tx_setup();
+}
+
+void
+i2c_dump(void)
+{
+	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
+	debug("OAR1  0x%08x  OAR2  0x%08x", rOAR1, rOAR2);
+	debug("CCR   0x%08x  TRISE 0x%08x", rCCR,  rTRISE);
+	debug("SR1   0x%08x  SR2   0x%08x", rSR1,  rSR2);
+}
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
new file mode 100644
index 000000000..f38593d2a
--- /dev/null
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -0,0 +1,311 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer.cpp
+ *
+ * Control channel input/output mixer and failsafe.
+ */
+
+#include <nuttx/config.h>
+#include <syslog.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/mixer/mixer.h>
+
+extern "C" {
+//#define DEBUG
+#include "px4io.h"
+}
+
+/*
+ * Maximum interval in us before FMU signal is considered lost
+ */
+#define FMU_INPUT_DROP_LIMIT_US		200000
+
+/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
+#define ROLL     0
+#define PITCH    1
+#define YAW      2
+#define THROTTLE 3
+#define OVERRIDE 4
+
+/* current servo arm/disarm state */
+static bool mixer_servos_armed = false;
+
+/* selected control values and count for mixing */
+enum mixer_source {
+	MIX_NONE,
+	MIX_FMU,
+	MIX_OVERRIDE,
+	MIX_FAILSAFE
+};
+static mixer_source source;
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+static MixerGroup mixer_group(mixer_callback, 0);
+
+void
+mixer_tick(void)
+{
+	/* check that we are receiving fresh data from the FMU */
+	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+
+		/* too long without FMU input, time to go to failsafe */
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
+			isr_debug(1, "AP RX timeout");
+		}
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
+
+	} else {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+		r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
+	}
+
+	source = MIX_FAILSAFE;
+
+	/*
+	 * Decide which set of controls we're using.
+	 */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+		/* don't actually mix anything - we already have raw PWM values or
+		 not a valid mixer. */
+		source = MIX_NONE;
+
+	} else {
+
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+			/* mix from FMU controls */
+			source = MIX_FMU;
+		}
+
+		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+		 	/* if allowed, mix from RC inputs directly */
+			source = MIX_OVERRIDE;
+		}
+	}
+
+	/*
+	 * Run the mixers.
+	 */
+	if (source == MIX_FAILSAFE) {
+
+		/* copy failsafe values to the servo outputs */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			r_page_servos[i] = r_page_servo_failsafe[i];
+
+	} else if (source != MIX_NONE) {
+
+		float	outputs[IO_SERVO_COUNT];
+		unsigned mixed;
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+		/* scale to PWM and update the servo outputs as required */
+		for (unsigned i = 0; i < mixed; i++) {
+
+			/* save actuator values for FMU readback */
+			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+
+			/* scale to servo output */
+			r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+
+		}
+		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+			r_page_servos[i] = 0;
+	}
+
+	/*
+	 * Decide whether the servos should be armed right now.
+	 *
+	 * We must be armed, and we must have a PWM source; either raw from
+	 * FMU or from the mixer.
+	 *
+	 * XXX correct behaviour for failsafe may require an additional case
+	 * here.
+	 */
+	bool should_arm = (
+	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
+		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
+		/* FMU is available or FMU is not available but override is an option */
+		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+	);
+
+	if (should_arm && !mixer_servos_armed) {
+		/* need to arm, but not armed */
+		up_pwm_servo_arm(true);
+		mixer_servos_armed = true;
+
+	} else if (!should_arm && mixer_servos_armed) {
+		/* armed but need to disarm */
+		up_pwm_servo_arm(false);
+		mixer_servos_armed = false;
+	}
+
+	if (mixer_servos_armed) {
+		/* update the servo outputs. */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servos[i]);
+	}
+}
+
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
+{
+	if (control_group != 0)
+		return -1;
+
+	switch (source) {
+	case MIX_FMU:
+		if (control_index < PX4IO_CONTROL_CHANNELS) {
+			control = REG_TO_FLOAT(r_page_controls[control_index]);
+			break;
+		}
+		return -1;
+
+	case MIX_OVERRIDE:
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			break;
+		}
+		return -1;
+
+	case MIX_FAILSAFE:
+	case MIX_NONE:
+		/* XXX we could allow for configuration of per-output failsafe values */
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * XXX error handling here should be more aggressive; currently it is
+ * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
+ * not loaded faithfully.
+ */
+
+static char mixer_text[256];		/* large enough for one mixer */
+static unsigned mixer_text_length = 0;
+
+void
+mixer_handle_text(const void *buffer, size_t length)
+{
+	/* do not allow a mixer change while fully armed */
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		return;
+	}
+
+	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
+
+	isr_debug(2, "mix txt %u", length);
+
+	if (length < sizeof(px4io_mixdata))
+		return;
+
+	unsigned	text_length = length - sizeof(px4io_mixdata);
+
+	switch (msg->action) {
+	case F2I_MIXER_ACTION_RESET:
+		isr_debug(2, "reset");
+
+		/* FIRST mark the mixer as invalid */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+		/* THEN actually delete it */
+		mixer_group.reset();
+		mixer_text_length = 0;
+
+		/* FALLTHROUGH */
+	case F2I_MIXER_ACTION_APPEND:
+		isr_debug(2, "append %d", length);
+
+		/* check for overflow - this is really fatal */
+		/* XXX could add just what will fit & try to parse, then repeat... */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+			return;
+		}
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		isr_debug(2, "buflen %u", mixer_text_length);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+
+			/* ideally, this should test resid == 0 ? */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
+			isr_debug(2, "used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		break;
+	}
+}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
new file mode 100644
index 000000000..085697fbb
--- /dev/null
+++ b/src/modules/px4iofirmware/module.mk
@@ -0,0 +1,4 @@
+
+
+SRCS = adc.c controls.c dsm.c i2c.c mixer.cpp px4io.c registers.c safety.c sbus.c \
+       up_cxxinitialize.c hx_stream.c perf_counter.c
diff --git a/src/modules/px4iofirmware/perf_counter.c b/src/modules/px4iofirmware/perf_counter.c
new file mode 100644
index 000000000..879f4715a
--- /dev/null
+++ b/src/modules/px4iofirmware/perf_counter.c
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 perf_counter.c
+ *
+ * @brief Performance measuring tools.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/queue.h>
+#include <drivers/drv_hrt.h>
+
+#include "perf_counter.h"
+
+/**
+ * Header common to all counters.
+ */
+struct perf_ctr_header {
+	sq_entry_t		link;	/**< list linkage */
+	enum perf_counter_type	type;	/**< counter type */
+	const char		*name;	/**< counter name */
+};
+
+/**
+ * PC_EVENT counter.
+ */
+struct perf_ctr_count {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+};
+
+/**
+ * PC_ELAPSED counter.
+ */
+struct perf_ctr_elapsed {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_start;
+	uint64_t		time_total;
+	uint64_t		time_least;
+	uint64_t		time_most;
+};
+
+/**
+ * PC_INTERVAL counter.
+ */
+struct perf_ctr_interval {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_event;
+	uint64_t		time_first;
+	uint64_t		time_last;
+	uint64_t		time_least;
+	uint64_t		time_most;
+
+};
+
+/**
+ * List of all known counters.
+ */
+static sq_queue_t	perf_counters;
+
+
+perf_counter_t
+perf_alloc(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t ctr = NULL;
+
+	switch (type) {
+	case PC_COUNT:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1);
+		break;
+
+	case PC_ELAPSED:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
+		break;
+
+	case PC_INTERVAL:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctr != NULL) {
+		ctr->type = type;
+		ctr->name = name;
+		sq_addfirst(&ctr->link, &perf_counters);
+	}
+
+	return ctr;
+}
+
+void
+perf_free(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	sq_rem(&handle->link, &perf_counters);
+	free(handle);
+}
+
+void
+perf_count(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count++;
+		break;
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		hrt_abstime now = hrt_absolute_time();
+
+		switch (pci->event_count) {
+		case 0:
+			pci->time_first = now;
+			break;
+		case 1:
+			pci->time_least = now - pci->time_last;
+			pci->time_most = now - pci->time_last;
+			break;
+		default: {
+			hrt_abstime interval = now - pci->time_last;
+			if (interval < pci->time_least)
+				pci->time_least = interval;
+			if (interval > pci->time_most)
+				pci->time_most = interval;
+			break;
+		}
+		}
+		pci->time_last = now;
+		pci->event_count++;
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_begin(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED:
+		((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_end(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+
+			pce->event_count++;
+			pce->time_total += elapsed;
+
+			if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				pce->time_least = elapsed;
+
+			if (pce->time_most < elapsed)
+				pce->time_most = elapsed;
+		}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_reset(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count = 0;
+		break;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		pce->event_count = 0;
+		pce->time_start = 0;
+		pce->time_total = 0;
+		pce->time_least = 0;
+		pce->time_most = 0;
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		pci->event_count = 0;
+		pci->time_event = 0;
+		pci->time_first = 0;
+		pci->time_last = 0;
+		pci->time_least = 0;
+		pci->time_most = 0;
+		break;
+	}
+	}
+}
+
+void
+perf_print_counter(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		printf("%s: %llu events\n",
+		       handle->name,
+		       ((struct perf_ctr_count *)handle)->event_count);
+		break;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
+		       handle->name,
+		       pce->event_count,
+		       pce->time_total,
+		       pce->time_least,
+		       pce->time_most);
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+
+		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		       handle->name,
+		       pci->event_count,
+		       (pci->time_last - pci->time_first) / pci->event_count,
+		       pci->time_least,
+		       pci->time_most);
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_print_all(void)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_print_counter(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+}
+
+void
+perf_reset_all(void)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_reset(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+}
diff --git a/src/modules/px4iofirmware/perf_counter.h b/src/modules/px4iofirmware/perf_counter.h
new file mode 100644
index 000000000..5c2cb15b2
--- /dev/null
+++ b/src/modules/px4iofirmware/perf_counter.h
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 perf_counter.h
+ * Performance measuring tools.
+ */
+
+#ifndef _SYSTEMLIB_PERF_COUNTER_H
+#define _SYSTEMLIB_PERF_COUNTER_H value
+
+/**
+ * Counter types.
+ */
+enum perf_counter_type {
+	PC_COUNT,		/**< count the number of times an event occurs */
+	PC_ELAPSED,		/**< measure the time elapsed performing an event */
+	PC_INTERVAL		/**< measure the interval between instances of an event */
+};
+
+struct perf_ctr_header;
+typedef struct perf_ctr_header	*perf_counter_t;
+
+__BEGIN_DECLS
+
+/**
+ * Create a new counter.
+ *
+ * @param type			The type of the new counter.
+ * @param name			The counter name.
+ * @return			Handle for the new counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+__EXPORT extern void		perf_free(perf_counter_t handle);
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_count(perf_counter_t handle);
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_begin(perf_counter_t handle);
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_end(perf_counter_t handle);
+
+/**
+ * Reset a performance event.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_reset(perf_counter_t handle);
+
+/**
+ * Print one performance counter.
+ *
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter(perf_counter_t handle);
+
+/**
+ * Print all of the performance counters.
+ */
+__EXPORT extern void		perf_print_all(void);
+
+/**
+ * Reset all of the performance counters.
+ */
+__EXPORT extern void		perf_reset_all(void);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
new file mode 100644
index 000000000..8d8b7e941
--- /dev/null
+++ b/src/modules/px4iofirmware/protocol.h
@@ -0,0 +1,204 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file protocol.h
+ *
+ * PX4IO I2C interface protocol.
+ *
+ * Communication is performed via writes to and reads from 16-bit virtual
+ * registers organised into pages of 255 registers each.
+ *
+ * The first two bytes of each write select a page and offset address
+ * respectively. Subsequent reads and writes increment the offset within
+ * the page. 
+ *
+ * Most pages are readable or writable but not both.
+ *
+ * Note that some pages may permit offset values greater than 255, which
+ * can only be achieved by long writes. The offset does not wrap.
+ *
+ * Writes to unimplemented registers are ignored. Reads from unimplemented
+ * registers return undefined values.
+ *
+ * As convention, values that would be floating point in other parts of 
+ * the PX4 system are expressed as signed integer values scaled by 10000,
+ * e.g. control values range from -10000..10000.  Use the REG_TO_SIGNED and
+ * SIGNED_TO_REG macros to convert between register representation and
+ * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
+ *
+ * Note that the implementation of readable pages prefers registers within
+ * readable pages to be densely packed. Page numbers do not need to be
+ * packed.
+ */
+
+#define PX4IO_CONTROL_CHANNELS			8
+#define PX4IO_INPUT_CHANNELS			12
+#define PX4IO_RELAY_CHANNELS			4
+
+/* Per C, this is safe for all 2's complement systems */
+#define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
+#define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
+
+#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))
+
+/* 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 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 */
+#define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
+
+/* dynamic status page */
+#define PX4IO_PAGE_STATUS		1
+#define PX4IO_P_STATUS_FREEMEM			0
+#define PX4IO_P_STATUS_CPULOAD			1
+
+#define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
+#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
+#define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM		(1 << 4) /* DSM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 5) /* SBUS input is valid */
+#define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
+#define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
+#define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
+#define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
+#define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
+
+#define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
+#define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
+#define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+
+#define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
+
+/* array of post-mix actuator outputs, -10000..10000 */
+#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of PWM servo output values, microseconds */
+#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of raw RC input values, microseconds */
+#define PX4IO_PAGE_RAW_RC_INPUT		4
+#define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
+#define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
+
+/* array of scaled RC input values, -10000..10000 */
+#define PX4IO_PAGE_RC_INPUT		5
+#define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
+#define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
+
+/* array of raw ADC values */
+#define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+
+/* PWM servo information */
+#define PX4IO_PAGE_PWM_INFO		7
+#define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
+
+/* setup page */
+#define PX4IO_PAGE_SETUP			100
+#define PX4IO_P_SETUP_FEATURES			0
+
+#define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
+#define PX4IO_P_SETUP_ARMING_ARM_OK		(1 << 0) /* OK to arm */
+#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3) /* OK to perform position / vector control (= position lock) */
+#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+
+#define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
+#define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_IBATT_SCALE		7	/* battery current scaling factor (float) */
+#define PX4IO_P_SETUP_IBATT_BIAS		8	/* battery current bias value */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+
+/* autopilot control values, -10000..10000 */
+#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+
+/* raw text load to the mixer parser - ignores offset */
+#define PX4IO_PAGE_MIXERLOAD			102
+
+/* R/C channel config */
+#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 */
+#define PX4IO_P_RC_CONFIG_DEADZONE		3	/* band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4	/* mapped input value */
+#define PX4IO_P_RC_CONFIG_OPTIONS		5	/* channel options bitmask */
+#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
+#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
+#define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
+
+/* PWM output - overrides mixer */
+#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+#pragma pack(push, 1)
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET			0
+#define F2I_MIXER_ACTION_APPEND			1
+
+	char		text[0];	/* actual text size may vary */
+};
+#pragma pack(pop)
+
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
new file mode 100644
index 000000000..9de37e118
--- /dev/null
+++ b/src/modules/px4iofirmware/px4io.c
@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4io.c
+ * Top-level logic for the PX4IO module.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>	// required for task_create
+#include <stdbool.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+#include <poll.h>
+#include <signal.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <stm32_uart.h>
+
+#define DEBUG
+#include "px4io.h"
+
+__EXPORT int user_start(int argc, char *argv[]);
+
+extern void up_cxxinitialize(void);
+
+struct sys_state_s 	system_state;
+
+static struct hrt_call serial_dma_call;
+
+/* store i2c reset count XXX this should be a register, together with other error counters */
+volatile uint32_t i2c_loop_resets = 0;
+
+/*
+ * a set of debug buffers to allow us to send debug information from ISRs
+ */
+
+static volatile uint32_t msg_counter;
+static volatile uint32_t last_msg_counter;
+static volatile uint8_t msg_next_out, msg_next_in;
+
+/*
+ * WARNING: too large buffers here consume the memory required
+ * for mixer handling. Do not allocate more than 80 bytes for
+ * output.
+ */
+#define NUM_MSG 2
+static char msg[NUM_MSG][40];
+
+/*
+ * add a debug message to be printed on the console
+ */
+void
+isr_debug(uint8_t level, const char *fmt, ...)
+{
+	if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) {
+		return;
+	}
+	va_list ap;
+	va_start(ap, fmt);
+	vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
+	va_end(ap);
+	msg_next_in = (msg_next_in+1) % NUM_MSG;
+	msg_counter++;
+}
+
+/*
+ * show all pending debug messages
+ */
+static void
+show_debug_messages(void)
+{
+	if (msg_counter != last_msg_counter) {
+		uint32_t n = msg_counter - last_msg_counter;
+		if (n > NUM_MSG) n = NUM_MSG;
+		last_msg_counter = msg_counter;
+		while (n--) {
+			debug("%s", msg[msg_next_out]);
+			msg_next_out = (msg_next_out+1) % NUM_MSG;
+		}
+	}
+}
+
+int
+user_start(int argc, char *argv[])
+{
+	/* run C++ ctors before we go any further */
+	up_cxxinitialize();
+
+	/* reset all to zero */
+	memset(&system_state, 0, sizeof(system_state));
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+
+	/* print some startup info */
+	lowsyslog("\nPX4IO: starting\n");
+
+	/* default all the LEDs to off while we start */
+	LED_AMBER(false);
+	LED_BLUE(false);
+	LED_SAFETY(false);
+
+	/* turn on servo power */
+	POWER_SERVO(true);
+
+	/* start the safety switch handler */
+	safety_init();
+
+	/* configure the first 8 PWM outputs (i.e. all of them) */
+	up_pwm_servo_init(0xff);
+
+	/* initialise the control inputs */
+	controls_init();
+
+	/* start the i2c handler */
+	i2c_init();
+
+	/* add a performance counter for mixing */
+	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
+
+	/* add a performance counter for controls */
+	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
+
+	/* and one for measuring the loop rate */
+	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
+
+	struct mallinfo minfo = mallinfo();
+	lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
+
+#if 0
+	/* not enough memory, lock down */
+	if (minfo.mxordblk < 500) {
+		lowsyslog("ERR: not enough MEM");
+		bool phase = false;
+
+		if (phase) {
+			LED_AMBER(true);
+			LED_BLUE(false);
+		} else {
+			LED_AMBER(false);
+			LED_BLUE(true);
+		}
+
+		phase = !phase;
+		usleep(300000);
+	}
+#endif
+
+	/*
+	 * Run everything in a tight loop.
+	 */
+
+	uint64_t last_debug_time = 0;
+	for (;;) {
+
+		/* track the rate at which the loop is running */
+		perf_count(loop_perf);
+
+		/* kick the mixer */
+		perf_begin(mixer_perf);
+		mixer_tick();
+		perf_end(mixer_perf);
+
+		/* kick the control inputs */
+		perf_begin(controls_perf);
+		controls_tick();
+		perf_end(controls_perf);
+
+		/* check for debug activity */
+		show_debug_messages();
+
+		/* post debug state at ~1Hz */
+		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
+
+			struct mallinfo minfo = mallinfo();
+
+			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", 
+				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
+				  (unsigned)r_status_flags,
+				  (unsigned)r_setup_arming,
+				  (unsigned)r_setup_features,
+				  (unsigned)i2c_loop_resets,
+				  (unsigned)minfo.mxordblk);
+			last_debug_time = hrt_absolute_time();
+		}
+	}
+}
+
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
new file mode 100644
index 000000000..202e9d9d9
--- /dev/null
+++ b/src/modules/px4iofirmware/px4io.h
@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4io.h
+ *
+ * General defines and structures for the PX4IO module firmware.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <drivers/boards/px4io/px4io_internal.h>
+
+#include "protocol.h"
+
+/*
+ * Constants and limits.
+ */
+#define MAX_CONTROL_CHANNELS	12
+#define IO_SERVO_COUNT		8
+
+/*
+ * Debug logging
+ */
+
+#ifdef DEBUG
+# include <debug.h>
+# define debug(fmt, args...)	lowsyslog(fmt "\n", ##args)
+#else
+# define debug(fmt, args...)	do {} while(0)
+#endif
+
+/*
+ * Registers.
+ */
+extern uint16_t			r_page_status[];	/* PX4IO_PAGE_STATUS */
+extern uint16_t			r_page_actuators[];	/* PX4IO_PAGE_ACTUATORS */
+extern uint16_t			r_page_servos[];	/* PX4IO_PAGE_SERVOS */
+extern uint16_t			r_page_raw_rc_input[];	/* PX4IO_PAGE_RAW_RC_INPUT */
+extern uint16_t			r_page_rc_input[];	/* PX4IO_PAGE_RC_INPUT */
+extern uint16_t			r_page_adc[];		/* PX4IO_PAGE_RAW_ADC_INPUT */
+
+extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
+extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
+extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
+extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+
+/*
+ * Register aliases.
+ *
+ * Handy aliases for registers that are widely used.
+ */
+#define r_status_flags		r_page_status[PX4IO_P_STATUS_FLAGS]
+#define r_status_alarms		r_page_status[PX4IO_P_STATUS_ALARMS]
+
+#define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
+#define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
+#define r_rc_values		(&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+
+#define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
+#define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
+#define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
+#define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
+#define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
+#define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+
+#define r_control_values	(&r_page_controls[0])
+
+/*
+ * System state structure.
+ */
+struct sys_state_s {
+
+	volatile uint64_t	rc_channels_timestamp;
+
+	/**
+	 * Last FMU receive time, in microseconds since system boot
+	 */
+	volatile uint64_t	fmu_data_received_time;
+
+};
+
+extern struct sys_state_s system_state;
+
+/*
+ * GPIO handling.
+ */
+#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+#define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+#define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+#define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+#define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+#define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
+
+#define ADC_VBATT		4
+#define ADC_IN5			5
+#define ADC_CHANNEL_COUNT	2
+
+/*
+ * Mixer
+ */
+extern void	mixer_tick(void);
+extern void	mixer_handle_text(const void *buffer, size_t length);
+
+/**
+ * Safety switch/LED.
+ */
+extern void	safety_init(void);
+
+/**
+ * FMU communications
+ */
+extern void	i2c_init(void);
+
+/**
+ * Register space
+ */
+extern void	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int	registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
+
+/**
+ * Sensors/misc inputs
+ */
+extern int	adc_init(void);
+extern uint16_t	adc_measure(unsigned channel);
+
+/**
+ * R/C receiver handling.
+ *
+ * Input functions return true when they receive an update from the RC controller.
+ */
+extern void	controls_init(void);
+extern void	controls_tick(void);
+extern int	dsm_init(const char *device);
+extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern int	sbus_init(const char *device);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
+
+/** global debug level for isr_debug() */
+extern volatile uint8_t debug_level;
+
+/* send a debug message to the console */
+extern void isr_debug(uint8_t level, const char *fmt, ...);
+
+void i2c_dump(void);
+void i2c_reset(void);
+
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
new file mode 100644
index 000000000..6c09def9e
--- /dev/null
+++ b/src/modules/px4iofirmware/registers.c
@@ -0,0 +1,644 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 registers.c
+ *
+ * Implementation of the PX4IO register space.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+#include "protocol.h"
+
+static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
+static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
+
+/**
+ * PAGE 0
+ *
+ * Static configuration parameters.
+ */
+static const uint16_t	r_page_config[] = {
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 3,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
+};
+
+/**
+ * PAGE 1
+ *
+ * Status values.
+ */
+uint16_t		r_page_status[] = {
+	[PX4IO_P_STATUS_FREEMEM]		= 0,
+	[PX4IO_P_STATUS_CPULOAD]		= 0,
+	[PX4IO_P_STATUS_FLAGS]			= 0,
+	[PX4IO_P_STATUS_ALARMS]			= 0,
+	[PX4IO_P_STATUS_VBATT]			= 0,
+	[PX4IO_P_STATUS_IBATT]			= 0
+};
+
+/**
+ * PAGE 2
+ *
+ * Post-mixed actuator values.
+ */
+uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+
+/**
+ * PAGE 3
+ *
+ * Servo PWM values
+ */
+uint16_t		r_page_servos[IO_SERVO_COUNT];
+
+/**
+ * PAGE 4
+ *
+ * Raw RC input
+ */
+uint16_t		r_page_raw_rc_input[] =
+{
+	[PX4IO_P_RAW_RC_COUNT]			= 0,
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
+
+/**
+ * PAGE 5
+ *
+ * Scaled/routed RC input
+ */
+uint16_t		r_page_rc_input[] = {
+	[PX4IO_P_RC_VALID]			= 0,
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
+
+/**
+ * Scratch page; used for registers that are constructed as-read.
+ *
+ * PAGE 6 Raw ADC input.
+ * PAGE 7 PWM rate maps.
+ */
+uint16_t		r_page_scratch[32];
+
+/**
+ * PAGE 100
+ *
+ * Setup registers
+ */
+volatile uint16_t	r_page_setup[] =
+{
+	[PX4IO_P_SETUP_FEATURES]		= 0,
+	[PX4IO_P_SETUP_ARMING]			= 0,
+	[PX4IO_P_SETUP_PWM_RATES]		= 0,
+	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
+	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
+	[PX4IO_P_SETUP_RELAYS]			= 0,
+	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
+	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
+	[PX4IO_P_SETUP_IBATT_BIAS]		= 0,
+	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
+};
+
+#define PX4IO_P_SETUP_FEATURES_VALID	(0)
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
+					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+#define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
+
+/**
+ * PAGE 101
+ *
+ * Control values from the FMU.
+ */
+volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
+
+/*
+ * PAGE 102 does not have a buffer.
+ */
+
+/**
+ * PAGE 103
+ *
+ * R/C channel input configuration.
+ */
+uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+
+/* valid options */
+#define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
+
+/*
+ * PAGE 104 uses r_page_servos.
+ */
+
+/**
+ * PAGE 105
+ *
+ * Failsafe servo PWM values
+ */
+uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
+
+void
+registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+
+	switch (page) {
+
+		/* handle bulk controls input */
+	case PX4IO_PAGE_CONTROLS:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_controls[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+
+		system_state.fmu_data_received_time = hrt_absolute_time();
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+        	r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
+		
+		break;
+
+		/* handle raw PWM input */
+	case PX4IO_PAGE_DIRECT_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_servos[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+
+		system_state.fmu_data_received_time = hrt_absolute_time();
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
+
+		break;
+
+		/* handle setup for servo failsafe values */
+	case PX4IO_PAGE_FAILSAFE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_servo_failsafe[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+		/* handle text going to the mixer parser */
+	case PX4IO_PAGE_MIXERLOAD:
+		mixer_handle_text(values, num_values * sizeof(*values));
+		break;
+
+	default:
+		/* avoid offset wrap */
+		if ((offset + num_values) > 255)
+			num_values = 255 - offset;
+
+		/* iterate individual registers, set each in turn */
+		while (num_values--) {
+			if (registers_set_one(page, offset, *values))
+				break;
+			offset++;
+			values++;
+		}
+	}
+}
+
+static int
+registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
+{
+	switch (page) {
+
+	case PX4IO_PAGE_STATUS:
+		switch (offset) {
+		case PX4IO_P_STATUS_ALARMS:
+			/* clear bits being written */
+			r_status_alarms &= ~value;
+			break;
+
+		case PX4IO_P_STATUS_FLAGS:
+			/* 
+			 * Allow FMU override of arming state (to allow in-air restores),
+			 * but only if the arming state is not in sync on the IO side.
+			 */
+			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+				r_status_flags = value;
+			}
+			break;
+
+		default:
+			/* just ignore writes to other registers in this page */
+			break;
+		}
+		break;
+
+	case PX4IO_PAGE_SETUP:
+		switch (offset) {
+		case PX4IO_P_SETUP_FEATURES:
+
+			value &= PX4IO_P_SETUP_FEATURES_VALID;
+			r_setup_features = value;
+
+			/* no implemented feature selection at this point */
+
+			break;
+
+		case PX4IO_P_SETUP_ARMING:
+
+			value &= PX4IO_P_SETUP_ARMING_VALID;
+
+			/*
+			 * Update arming state - disarm if no longer OK.
+			 * This builds on the requirement that the FMU driver
+			 * asks about the FMU arming state on initialization,
+			 * so that an in-air reset of FMU can not lead to a
+			 * lockup of the IO arming state.
+			 */
+			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			}
+
+			r_setup_arming = value;
+
+			break;
+
+		case PX4IO_P_SETUP_PWM_RATES:
+			value &= PX4IO_P_SETUP_RATES_VALID;
+			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
+			break;
+
+		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
+			if (value < 50)
+				value = 50;
+			if (value > 400)
+				value = 400;
+			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
+			break;
+
+		case PX4IO_P_SETUP_PWM_ALTRATE:
+			if (value < 50)
+				value = 50;
+			if (value > 400)
+				value = 400;
+			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
+			break;
+
+		case PX4IO_P_SETUP_RELAYS:
+			value &= PX4IO_P_SETUP_RELAYS_VALID;
+			r_setup_relays = value;
+			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+			POWER_ACC1(value & (1 << 2) ? 1 : 0);
+			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			break;
+
+		case PX4IO_P_SETUP_SET_DEBUG:
+			r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
+			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
+			break;
+
+		default:
+			return -1;
+		}
+		break;
+
+	case PX4IO_PAGE_RC_CONFIG: {
+
+		/* do not allow a RC config change while fully armed */
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+			break;
+		}
+
+		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
+		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
+		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
+
+		if (channel >= MAX_CONTROL_CHANNELS)
+			return -1;
+
+		/* disable the channel until we have a chance to sanity-check it */
+		conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+
+		switch (index) {
+
+		case PX4IO_P_RC_CONFIG_MIN:
+		case PX4IO_P_RC_CONFIG_CENTER:
+		case PX4IO_P_RC_CONFIG_MAX:
+		case PX4IO_P_RC_CONFIG_DEADZONE:
+		case PX4IO_P_RC_CONFIG_ASSIGNMENT:
+			conf[index] = value;
+			break;
+
+		case PX4IO_P_RC_CONFIG_OPTIONS:
+			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+
+			/* set all options except the enabled option */
+			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+
+			/* should the channel be enabled? */
+			/* this option is normally set last */
+			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+				uint8_t count = 0;
+
+				/* assert min..center..max ordering */
+				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
+					count++;
+				}
+
+				/* assert deadzone is sane */
+				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
+					count++;
+				}
+				// The following check isn't needed as constraint checks in controls.c will catch this.
+				//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+				//	count++;
+				//}
+				//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+				//	count++;
+				//}
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+					count++;
+				}
+
+				/* sanity checks pass, enable channel */
+				if (count) {
+					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
+					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
+				} else {
+					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+				}
+			}
+			break;
+			/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
+
+		}
+		break;
+		/* case PX4IO_RC_PAGE_CONFIG */
+	}
+
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+uint8_t last_page;
+uint8_t last_offset;
+
+int
+registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
+{
+#define SELECT_PAGE(_page_name)							\
+	do {									\
+		*values = &_page_name[0];					\
+		*num_values = sizeof(_page_name) / sizeof(_page_name[0]);	\
+	} while(0)
+
+	switch (page) {
+
+	/*
+	 * Handle pages that are updated dynamically at read time.
+	 */
+	case PX4IO_PAGE_STATUS:
+		/* PX4IO_P_STATUS_FREEMEM */
+		{
+			struct mallinfo minfo = mallinfo();
+			r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
+		}
+
+		/* XXX PX4IO_P_STATUS_CPULOAD */
+
+		/* PX4IO_P_STATUS_FLAGS maintained externally */
+
+		/* PX4IO_P_STATUS_ALARMS maintained externally */
+
+		/* PX4IO_P_STATUS_VBATT */
+		{
+			/*
+			 * Coefficients here derived by measurement of the 5-16V
+			 * range on one unit:
+			 *
+			 * V   counts
+			 *  5  1001
+			 *  6  1219
+			 *  7  1436
+			 *  8  1653
+			 *  9  1870
+			 * 10  2086
+			 * 11  2303
+			 * 12  2522
+			 * 13  2738
+			 * 14  2956
+			 * 15  3172
+			 * 16  3389
+			 *
+			 * slope = 0.0046067
+			 * intercept = 0.3863
+			 *
+			 * Intercept corrected for best results @ 12V.
+			 */
+			unsigned counts = adc_measure(ADC_VBATT);
+			unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+			unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+
+			r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+		}
+
+		/* PX4IO_P_STATUS_IBATT */
+		{
+			unsigned counts = adc_measure(ADC_VBATT);
+			unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
+			int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
+			if (corrected < 0)
+				corrected = 0;
+			r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+		}
+
+		SELECT_PAGE(r_page_status);
+		break;
+
+	case PX4IO_PAGE_RAW_ADC_INPUT:
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		r_page_scratch[0] = adc_measure(ADC_VBATT);
+		r_page_scratch[1] = adc_measure(ADC_IN5);
+
+		SELECT_PAGE(r_page_scratch);
+		break;
+
+	case PX4IO_PAGE_PWM_INFO:
+		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
+
+		SELECT_PAGE(r_page_scratch);
+		break;
+
+	/*
+	 * Pages that are just a straight read of the register state.
+	 */
+
+	/* status pages */
+	case PX4IO_PAGE_CONFIG:
+		SELECT_PAGE(r_page_config);
+		break;
+	case PX4IO_PAGE_ACTUATORS:
+		SELECT_PAGE(r_page_actuators);
+		break;
+	case PX4IO_PAGE_SERVOS:
+		SELECT_PAGE(r_page_servos);
+		break;
+	case PX4IO_PAGE_RAW_RC_INPUT:
+		SELECT_PAGE(r_page_raw_rc_input);
+		break;
+	case PX4IO_PAGE_RC_INPUT:
+		SELECT_PAGE(r_page_rc_input);
+		break;
+
+	/* readback of input pages */
+	case PX4IO_PAGE_SETUP:
+		SELECT_PAGE(r_page_setup);
+		break;
+	case PX4IO_PAGE_CONTROLS:
+		SELECT_PAGE(r_page_controls);
+		break;
+	case PX4IO_PAGE_RC_CONFIG:
+		SELECT_PAGE(r_page_rc_input_config);
+		break;
+	case PX4IO_PAGE_DIRECT_PWM:
+		SELECT_PAGE(r_page_servos);
+		break;
+	case PX4IO_PAGE_FAILSAFE_PWM:
+		SELECT_PAGE(r_page_servo_failsafe);
+		break;
+
+	default:
+		return -1;
+	}
+
+#undef SELECT_PAGE
+#undef COPY_PAGE
+
+last_page = page;
+last_offset = offset;
+
+	/* if the offset is at or beyond the end of the page, we have no data */
+	if (offset >= *num_values)
+		return -1;
+
+	/* correct the data pointer and count for the offset */
+	*values += offset;
+	*num_values -= offset;
+
+	return 0;
+}
+
+/*
+ * Helper function to handle changes to the PWM rate control registers.
+ */
+static void
+pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
+{
+	for (unsigned pass = 0; pass < 2; pass++) {
+		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+
+			/* get the channel mask for this rate group */
+			uint32_t mask = up_pwm_servo_get_rate_group(group);
+			if (mask == 0)
+				continue;
+
+			/* all channels in the group must be either default or alt-rate */
+			uint32_t alt = map & mask;
+
+			if (pass == 0) {
+				/* preflight */
+				if ((alt != 0) && (alt != mask)) {
+					/* not a legal map, bail with an alarm */
+					r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+					return;
+				}
+			} else {
+				/* set it - errors here are unexpected */
+				if (alt != 0) {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				} else {
+					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK)
+						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+				}
+			}
+		}
+	}
+	r_setup_pwm_rates = map;
+	r_setup_pwm_defaultrate = defaultrate;
+	r_setup_pwm_altrate = altrate;
+}
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
new file mode 100644
index 000000000..5495d5ae1
--- /dev/null
+++ b/src/modules/px4iofirmware/safety.c
@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Safety button logic.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+
+static struct hrt_call arming_call;
+static struct hrt_call heartbeat_call;
+static struct hrt_call failsafe_call;
+
+/*
+ * Count the number of times in a row that we see the arming button
+ * held down.
+ */
+static unsigned counter = 0;
+
+/*
+ * Define the various LED flash sequences for each system state.
+ */
+#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
+#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0xFFFE		/**< always on with short break 	*/
+#define LED_PATTERN_FMU_ARMED 			0x4444		/**< slow blinking			*/
+#define LED_PATTERN_IO_ARMED 			0x5555		/**< fast blinking 			*/
+#define LED_PATTERN_IO_FMU_ARMED 		0x5050		/**< long off then double blink 	*/
+
+static unsigned blink_counter = 0;
+
+/*
+ * IMPORTANT: The arming state machine critically
+ * 	      depends on using the same threshold
+ *            for arming and disarming. Since disarming
+ *            is quite deadly for the system, a similar
+ *            length can be justified.
+ */
+#define ARM_COUNTER_THRESHOLD	10
+
+static bool safety_button_pressed;
+
+static void safety_check_button(void *arg);
+static void heartbeat_blink(void *arg);
+static void failsafe_blink(void *arg);
+
+void
+safety_init(void)
+{
+	/* arrange for the button handler to be called at 10Hz */
+	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+
+	/* arrange for the heartbeat handler to be called at 4Hz */
+	hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
+
+	/* arrange for the failsafe blinker to be called at 8Hz */
+	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
+}
+
+static void
+safety_check_button(void *arg)
+{
+	/*
+	 * Debounce the safety button, change state if it has been held for long enough.
+	 *
+	 */
+	safety_button_pressed = BUTTON_SAFETY;
+
+	/*
+	 * Keep pressed for a while to arm.
+	 *
+	 * Note that the counting sequence has to be same length
+	 * for arming / disarming in order to end up as proper
+	 * state machine, keep ARM_COUNTER_THRESHOLD the same
+	 * length in all cases of the if/else struct below.
+	 */
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+
+		if (counter < ARM_COUNTER_THRESHOLD) {
+			counter++;
+
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
+			/* switch to armed state */
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			counter++;
+		}
+
+		/* Disarm quickly */
+
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+
+		if (counter < ARM_COUNTER_THRESHOLD) {
+			counter++;
+
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
+			/* change to disarmed state and notify the FMU */
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			counter++;
+		}
+
+	} else {
+		counter = 0;
+	}
+
+	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
+	uint16_t pattern = LED_PATTERN_SAFE;
+
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+		if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+			pattern = LED_PATTERN_IO_FMU_ARMED;
+
+		} else {
+			pattern = LED_PATTERN_IO_ARMED;
+		}
+
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+		pattern = LED_PATTERN_FMU_ARMED;
+
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
+		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
+	}
+
+	/* Turn the LED on if we have a 1 at the current bit position */
+	LED_SAFETY(pattern & (1 << blink_counter++));
+
+	if (blink_counter > 15) {
+		blink_counter = 0;
+	}
+}
+
+static void
+heartbeat_blink(void *arg)
+{
+	static bool heartbeat = false;
+
+	/* XXX add flags here that need to be frobbed by various loops */
+
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+failsafe_blink(void *arg)
+{
+	/* indicate that a serious initialisation error occured */
+	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
+		LED_AMBER(true);
+	    return;
+    }
+
+	static bool failsafe = false;
+
+	/* blink the failsafe LED if we don't have FMU input */
+	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+		failsafe = !failsafe;
+	} else {
+		failsafe = false;
+	}
+
+	LED_AMBER(failsafe);
+}
\ No newline at end of file
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
new file mode 100644
index 000000000..073ddeaba
--- /dev/null
+++ b/src/modules/px4iofirmware/sbus.c
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 sbus.c
+ *
+ * Serial protocol decoder for the Futaba S.bus protocol.
+ */
+
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <systemlib/ppm_decode.h>
+
+#include <drivers/drv_hrt.h>
+
+#define DEBUG
+#include "px4io.h"
+#include "protocol.h"
+#include "debug.h"
+
+#define SBUS_FRAME_SIZE		25
+#define SBUS_INPUT_CHANNELS	16
+
+static int sbus_fd = -1;
+
+static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
+
+static uint8_t	frame[SBUS_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+
+unsigned sbus_frame_drops;
+
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
+
+int
+sbus_init(const char *device)
+{
+	if (sbus_fd < 0)
+		sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
+
+	if (sbus_fd >= 0) {
+		struct termios t;
+
+		/* 100000bps, even parity, two stop bits */
+		tcgetattr(sbus_fd, &t);
+		cfsetspeed(&t, 100000);
+		t.c_cflag |= (CSTOPB | PARENB);
+		tcsetattr(sbus_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		partial_frame_count = 0;
+		last_rx_time = hrt_absolute_time();
+
+		debug("S.Bus: ready");
+
+	} else {
+		debug("S.Bus: open failed");
+	}
+
+	return sbus_fd;
+}
+
+bool
+sbus_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 * The S.bus protocol doesn't provide reliable framing,
+	 * so we detect frame boundaries by the inter-frame delay.
+	 *
+	 * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
+	 * frame transmission time is ~2ms.
+	 *
+	 * We expect to only be called when bytes arrive for processing,
+	 * and if an interval of more than 3ms passes between calls,
+	 * the first byte we read will be the first byte of a frame.
+	 *
+	 * In the case where byte(s) are dropped from a frame, this also
+	 * provides a degree of protection. Of course, it would be better
+	 * if we didn't drop bytes...
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - last_rx_time) > 3000) {
+		if (partial_frame_count > 0) {
+			sbus_frame_drops++;
+			partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current frame.
+	 */
+	ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	last_rx_time = now;
+
+	/*
+	 * Add bytes to the current frame
+	 */
+	partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full frame, return
+	 */
+	if (partial_frame_count < SBUS_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	partial_frame_count = 0;
+	return sbus_decode(now, values, num_values);
+}
+
+/*
+ * S.bus decoder matrix.
+ *
+ * Each channel value can come from up to 3 input bytes. Each row in the
+ * matrix describes up to three bytes, and each entry gives:
+ *
+ * - byte offset in the data portion of the frame
+ * - right shift applied to the data byte
+ * - mask for the data byte
+ * - left shift applied to the result into the channel value
+ */
+struct sbus_bit_pick {
+	uint8_t byte;
+	uint8_t rshift;
+	uint8_t mask;
+	uint8_t lshift;
+};
+static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
+	/*  0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
+	/*  1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
+	/*  2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
+	/*  3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
+	/*  4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
+	/*  5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03,  9} },
+	/*  6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
+	/*  7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00,  0} },
+	/*  8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
+	/*  9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
+	/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
+	/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
+	/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
+	/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03,  9} },
+	/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
+	/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00,  0} }
+};
+
+static bool
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+{
+	/* check frame boundary markers to avoid out-of-sync cases */
+	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
+		sbus_frame_drops++;
+		return false;
+	}
+
+	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
+	if ((frame[23] & (1 << 2)) && /* signal lost */
+	    (frame[23] & (1 << 3))) { /* failsafe */
+
+		/* actively announce signal loss */
+		*values = 0;
+		return false;
+	}
+
+	/* we have received something we think is a frame */
+	last_frame_time = frame_time;
+
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
+			     SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+
+	/* use the decoder matrix to extract channel data */
+	for (unsigned channel = 0; channel < chancount; channel++) {
+		unsigned value = 0;
+
+		for (unsigned pick = 0; pick < 3; pick++) {
+			const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
+
+			if (decode->mask != 0) {
+				unsigned piece = frame[1 + decode->byte];
+				piece >>= decode->rshift;
+				piece &= decode->mask;
+				piece <<= decode->lshift;
+
+				value |= piece;
+			}
+		}
+
+		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
+		values[channel] = (value / 2) + 998;
+	}
+
+	/* decode switch channels if data fields are wide enough */
+	if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
+		chancount = 18;
+
+		/* channel 17 (index 16) */
+		values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		/* channel 18 (index 17) */
+		values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+	}
+
+	/* note the number of channels decoded */
+	*num_values = chancount;
+
+	return true;
+}
diff --git a/src/modules/px4iofirmware/up_cxxinitialize.c b/src/modules/px4iofirmware/up_cxxinitialize.c
new file mode 100644
index 000000000..c78f29570
--- /dev/null
+++ b/src/modules/px4iofirmware/up_cxxinitialize.c
@@ -0,0 +1,150 @@
+/************************************************************************************
+ * configs/stm32f4discovery/src/up_cxxinitialize.c
+ * arch/arm/src/board/up_cxxinitialize.c
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <debug.h>
+
+#include <nuttx/arch.h>
+
+//#include <arch/stm32/chip.h>
+//#include "chip.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+/* Debug ****************************************************************************/
+/* Non-standard debug that may be enabled just for testing the static constructors */
+
+#ifndef CONFIG_DEBUG
+#  undef CONFIG_DEBUG_CXX
+#endif
+
+#ifdef CONFIG_DEBUG_CXX
+#  define cxxdbg              dbg
+#  define cxxlldbg            lldbg
+#  ifdef CONFIG_DEBUG_VERBOSE
+#    define cxxvdbg           vdbg
+#    define cxxllvdbg         llvdbg
+#  else
+#    define cxxvdbg(x...)
+#    define cxxllvdbg(x...)
+#  endif
+#else
+#  define cxxdbg(x...)
+#  define cxxlldbg(x...)
+#  define cxxvdbg(x...)
+#  define cxxllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Types
+ ************************************************************************************/
+/* This type defines one entry in initialization array */
+
+typedef void (*initializer_t)(void);
+
+/************************************************************************************
+ * External references
+ ************************************************************************************/
+/* _sinit and _einit are symbols exported by the linker script that mark the
+ * beginning and the end of the C++ initialization section.
+ */
+
+extern initializer_t _sinit;
+extern initializer_t _einit;
+
+/* _stext and _etext are symbols exported by the linker script that mark the
+ * beginning and the end of text.
+ */
+
+extern uint32_t _stext;
+extern uint32_t _etext;
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/****************************************************************************
+ * Name: up_cxxinitialize
+ *
+ * Description:
+ *   If C++ and C++ static constructors are supported, then this function
+ *   must be provided by board-specific logic in order to perform
+ *   initialization of the static C++ class instances.
+ *
+ *   This function should then be called in the application-specific
+ *   user_start logic in order to perform the C++ initialization.  NOTE
+ *   that no component of the core NuttX RTOS logic is involved; This
+ *   function defintion only provides the 'contract' between application
+ *   specific C++ code and platform-specific toolchain support
+ *
+ ***************************************************************************/
+
+__EXPORT void up_cxxinitialize(void)
+{
+  initializer_t *initp;
+
+  cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n",
+         &_sinit, &_einit, &_stext, &_etext);
+
+  /* Visit each entry in the initialzation table */
+
+  for (initp = &_sinit; initp != &_einit; initp++)
+    {
+      initializer_t initializer = *initp;
+      cxxdbg("initp: %p initializer: %p\n", initp, initializer);
+
+      /* Make sure that the address is non-NULL and lies in the text region
+       * defined by the linker script.  Some toolchains may put NULL values
+       * or counts in the initialization table
+       */
+
+      if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext)
+        {
+          cxxdbg("Calling %p\n", initializer);
+          initializer();
+        }
+    }
+}