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

35 files changed, 1006 insertions, 121 deletions

diff --git a/apps/drivers/stm32/adc/Makefile b/apps/drivers/stm32/adc/Makefile
deleted file mode 100644
index 443bc0623..000000000
--- a/apps/drivers/stm32/adc/Makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# STM32 ADC driver
-#
-
-APPNAME		 = adc
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile
deleted file mode 100644
index 0eb3ffcf7..000000000
--- a/apps/px4io/Makefile
+++ /dev/null
@@ -1,58 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Build the px4io application.
-#
-
-#
-# Note that we pull a couple of specific files from the systemlib, since
-# we can't support it all.
-#
-CSRCS		= adc.c \
-		  controls.c \
-		  dsm.c \
-		  i2c.c \
-		  px4io.c \
-		  registers.c \
-		  safety.c \
-		  sbus.c \
-		  ../systemlib/hx_stream.c \
-		  ../systemlib/perf_counter.c \
-		  ../systemlib/up_cxxinitialize.c
-
-CXXSRCS		= mixer.cpp
-
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-
-include $(APPDIR)/mk/app.mk
diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk
index fea9a814d..47e15b344 100644
--- a/makefiles/config_px4fmu_default.mk
+++ b/makefiles/config_px4fmu_default.mk
@@ -10,6 +10,9 @@ ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
 #
 # Board support modules
 #
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/px4io
 MODULES		+= drivers/px4fmu
 MODULES		+= drivers/boards/px4fmu
@@ -85,9 +88,7 @@ endef
 
 #                  command                 priority                   stack  entrypoint
 BUILTIN_COMMANDS := \
-	$(call _B, adc,                    ,                          2048,  adc_main                   ) \
 	$(call _B, math_demo,              ,                          8192,  math_demo_main             ) \
 	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
 	$(call _B, serdis,                 ,                          2048,  serdis_main                ) \
-	$(call _B, tone_alarm,             ,                          2048,  tone_alarm_main            ) \
 	$(call _B, uorb,                   ,                          4096,  uorb_main                  )
diff --git a/makefiles/config_px4io_default.mk b/makefiles/config_px4io_default.mk
index dc774fe63..cf70391bc 100644
--- a/makefiles/config_px4io_default.mk
+++ b/makefiles/config_px4io_default.mk
@@ -2,4 +2,9 @@
 # Makefile for the px4io_default configuration
 #
 
-# ... nothing here yet
+#
+# Board support modules
+#
+MODULES		+= drivers/stm32
+MODULES		+= drivers/boards/px4io
+MODULES		+= modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/module.mk b/makefiles/module.mk
index 538f6d318..253b3cd52 100644
--- a/makefiles/module.mk
+++ b/makefiles/module.mk
@@ -1,5 +1,5 @@
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
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/apps/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c
index 14e8dc13a..14e8dc13a 100644
--- a/apps/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io/px4io_init.c
diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h
index 44bb98513..44bb98513 100644
--- a/apps/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io/px4io_internal.h
diff --git a/apps/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c
index a2f73c429..a2f73c429 100644
--- a/apps/drivers/boards/px4io/px4io_pwm_servo.c
+++ b/src/drivers/boards/px4io/px4io_pwm_servo.c
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/apps/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 911def943..911def943 100644
--- a/apps/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
diff --git a/apps/drivers/boards/px4io/Makefile b/src/drivers/stm32/adc/module.mk
index 85806fe6f..48620feea 100644
--- a/apps/drivers/boards/px4io/Makefile
+++ b/src/drivers/stm32/adc/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -32,10 +32,11 @@
 ############################################################################
 
 #
-# Board-specific startup code for the PX4IO
+# STM32 ADC driver
 #
 
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-LIBNAME		 = brd_px4io
+MODULE_COMMAND	= adc
 
-include $(APPDIR)/mk/app.mk
+SRCS		= adc.cpp
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/apps/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index cec0c49ff..cec0c49ff 100644
--- a/apps/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index d7316e1f7..c1efb8515 100644
--- a/apps/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -290,6 +290,8 @@ 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
diff --git a/apps/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h
index 5dd4cf70c..5dd4cf70c 100644
--- a/apps/drivers/stm32/drv_pwm_servo.h
+++ b/src/drivers/stm32/drv_pwm_servo.h
diff --git a/apps/drivers/stm32/Makefile b/src/drivers/stm32/module.mk
index 4ad57f413..bb751c7f6 100644
--- a/apps/drivers/stm32/Makefile
+++ b/src/drivers/stm32/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -37,6 +37,7 @@
 # Modules in this directory are compiled for all STM32 targets.
 #
 
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+SRCS		= drv_hrt.c \
+		  drv_pwm_servo.c
 
-include $(APPDIR)/mk/app.mk
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/apps/drivers/stm32/tone_alarm/Makefile b/src/drivers/stm32/tone_alarm/module.mk
index d2ddf9534..827cf30b2 100644
--- a/apps/drivers/stm32/tone_alarm/Makefile
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -35,9 +35,8 @@
 # Tone alarm driver
 #
 
-APPNAME		 = tone_alarm
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+MODULE_COMMAND	= tone_alarm
 
-include $(APPDIR)/mk/app.mk
+SRCS		= tone_alarm.cpp
+
+INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index ac5511e60..ac5511e60 100644
--- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
diff --git a/apps/px4io/adc.c b/src/modules/px4iofirmware/adc.c
index 670b8d635..670b8d635 100644
--- a/apps/px4io/adc.c
+++ b/src/modules/px4iofirmware/adc.c
diff --git a/apps/px4io/controls.c b/src/modules/px4iofirmware/controls.c
index dc36f6c93..dc36f6c93 100644
--- a/apps/px4io/controls.c
+++ b/src/modules/px4iofirmware/controls.c
diff --git a/apps/px4io/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..ea35e5513 100644
--- a/apps/px4io/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
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/apps/px4io/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..4485daa5b 100644
--- a/apps/px4io/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
diff --git a/apps/px4io/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f38593d2a..f38593d2a 100644
--- a/apps/px4io/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
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/apps/px4io/protocol.h b/src/modules/px4iofirmware/protocol.h
index 8d8b7e941..8d8b7e941 100644
--- a/apps/px4io/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
diff --git a/apps/px4io/px4io.c b/src/modules/px4iofirmware/px4io.c
index 9de37e118..9de37e118 100644
--- a/apps/px4io/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
diff --git a/apps/px4io/px4io.h b/src/modules/px4iofirmware/px4io.h
index 202e9d9d9..202e9d9d9 100644
--- a/apps/px4io/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
diff --git a/apps/px4io/registers.c b/src/modules/px4iofirmware/registers.c
index 6c09def9e..6c09def9e 100644
--- a/apps/px4io/registers.c
+++ b/src/modules/px4iofirmware/registers.c
diff --git a/apps/px4io/safety.c b/src/modules/px4iofirmware/safety.c
index 5495d5ae1..5495d5ae1 100644
--- a/apps/px4io/safety.c
+++ b/src/modules/px4iofirmware/safety.c
diff --git a/apps/px4io/sbus.c b/src/modules/px4iofirmware/sbus.c
index 073ddeaba..073ddeaba 100644
--- a/apps/px4io/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
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();
+        }
+    }
+}