implement systemlib (still using dummy serial and hrt)

10 files changed, 302 insertions, 260 deletions

diff --git a/NuttX b/NuttX
-Subproject 9c9892196b2498615dc1c537f047cc6e56653d4
+Subproject 80cfb88472806a3171a1457ce4946a5172efb0f
diff --git a/makefiles/config_trifle_default.mk b/makefiles/config_trifle_default.mk
index 339a7bdec..aa5e996cd 100644
--- a/makefiles/config_trifle_default.mk
+++ b/makefiles/config_trifle_default.mk
@@ -11,9 +11,13 @@ ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_trifle
 # Board support modules
 #
 MODULES		+= drivers/boards/trifle
-MODULES		+= examples/pwm
 MODULES		+= drivers/device
 MODULES		+= drivers/kinetis
+MODULES		+= modules/systemlib
+MODULES		+= modules/uORB
+
+MODULES		+= examples/pwm
+
 #MODULES		+= drivers/stm32
 #MODULES		+= drivers/stm32/adc
 #MODULES		+= drivers/stm32/tone_alarm
diff --git a/nuttx-configs/trifle/scripts/ld.script b/nuttx-configs/trifle/scripts/ld.script
index d89b65314..23e6a5174 100755
--- a/nuttx-configs/trifle/scripts/ld.script
+++ b/nuttx-configs/trifle/scripts/ld.script
@@ -82,6 +82,12 @@ SECTIONS
 		*(.gcc_except_table)
 		*(.gnu.linkonce.r.*)
 		_etext = ABSOLUTE(.);
+
+		/*
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
 	} > progflash
 
 	.init_section : {
@@ -90,6 +96,15 @@ SECTIONS
 		_einit = ABSOLUTE(.);
 	} > progflash
 
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > progflash
+
 	.ARM.extab : {
 		*(.ARM.extab*)
 	} > progflash
diff --git a/src/drivers/boards/trifle/board_config.h b/src/drivers/boards/trifle/board_config.h
new file mode 100644
index 000000000..9004142aa
--- /dev/null
+++ b/src/drivers/boards/trifle/board_config.h
@@ -0,0 +1,9 @@
+#ifndef BOARD_CONFIG_H
+#define BOARD_CONFIG_H
+
+#include <nuttx/config.h>
+
+//totally random
+#define UDID_START 0x2000000
+
+#endif
diff --git a/src/drivers/kinetis/drv_hrt.c b/src/drivers/kinetis/drv_hrt.c
new file mode 100644
index 000000000..90714b7ba
--- /dev/null
+++ b/src/drivers/kinetis/drv_hrt.c
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 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.
+ *
+ ****************************************************************************/
+
+
+#include <drivers/drv_hrt.h>
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT 8
+__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+__EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t		latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+//FIXME: global dummy var
+static hrt_abstime result = 0x0000000000000000l;
+
+/*
+ * Get absolute time.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	return result;
+}
+
+/*
+ * Convert a timespec to absolute time.
+ */
+hrt_abstime
+ts_to_abstime(struct timespec *ts)
+{
+	return result;
+}
+
+/*
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+
+}
+
+/*
+ * Compute the delta between a timestamp taken in the past
+ * and now.
+ *
+ * This function is safe to use even if the timestamp is updated
+ * by an interrupt during execution.
+ */
+hrt_abstime
+hrt_elapsed_time(const volatile hrt_abstime *then)
+{
+	return result;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+hrt_abstime
+hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	return result;
+}
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	
+}
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	
+}
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval,
+				       hrt_callout callout, void *arg)
+{
+	
+}
+
+/*
+ * If this returns true, the entry has been invoked and removed from the callout list,
+ * or it has never been entered.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return true;
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	
+}
+
+/*
+ * initialise a hrt_call structure
+ */
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	
+}
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+void
+hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	
+}
+
+/*
+ * Initialise the HRT.
+ */
+void hrt_init(void)
+{
+	
+}
+
+__END_DECLS
diff --git a/src/drivers/kinetis/drv_pwm_servo.c b/src/drivers/kinetis/drv_pwm_servo.c
index e64a82764..a2bc0a27a 100644
--- a/src/drivers/kinetis/drv_pwm_servo.c
+++ b/src/drivers/kinetis/drv_pwm_servo.c
@@ -57,35 +57,35 @@
  *			as GPIOs or as another function.
  * @return		OK on success.
  */
- int
- up_pwm_servo_init(uint32_t channel_mask)
- {
+int
+up_pwm_servo_init(uint32_t channel_mask)
+{
    
-   for (size_t i = 0; i < PWM_SERVO_MAX_TIMERS; ++i) {
+	for (size_t i = 0; i < PWM_SERVO_MAX_TIMERS; ++i) {
      
-     const struct pwm_servo_timer* timer = &pwm_servo_timers[i];
-     uint32_t regval;
+		const struct pwm_servo_timer* timer = &pwm_servo_timers[i];
+		uint32_t regval;
      
-     regval = getreg32(timer->scgc);
-     putreg32(regval | timer->scgc_enable, timer->scgc); // enable clock signal
-     up_pwm_servo_set_rate_group_update(i, 50); // 50Hz default
+		regval = getreg32(timer->scgc);
+		putreg32(regval | timer->scgc_enable, timer->scgc); // enable clock signal
+		up_pwm_servo_set_rate_group_update(i, 50); // 50Hz default
      
-   }
+	}
 
-   for (size_t i = 0; i < PWM_SERVO_MAX_CHANNELS; ++i) {
-     const struct pwm_servo_channel* channel = &pwm_servo_channels[i];
+	for (size_t i = 0; i < PWM_SERVO_MAX_CHANNELS; ++i) {
+		const struct pwm_servo_channel* channel = &pwm_servo_channels[i];
 
-     if ((1 << i) & channel_mask) {
-       // enable edge aligned pwm
-       putreg32(0x28, channel->timer->ftm_base + KINETIS_FTM_CSC_OFFSET(channel->ftm_channel));
-       // configure pin
-       kinetis_pinconfig(channel->pinconfig); 
-     }
+		if ((1 << i) & channel_mask) {
+			// enable edge aligned pwm
+			putreg32(0x28, channel->timer->ftm_base + KINETIS_FTM_CSC_OFFSET(channel->ftm_channel));
+			// configure pin
+			kinetis_pinconfig(channel->pinconfig); 
+		}
       
-   }
+	}
 
-   return 0;
- }
+	return 0;
+}
 
 /**
  * De-initialise the PWM servo outputs.
@@ -109,21 +109,21 @@ up_pwm_servo_deinit(void)
  */
 void up_pwm_servo_arm(bool armed)
 {
-  for (size_t i = 0; i < PWM_SERVO_MAX_TIMERS; ++i) {
-    const struct pwm_servo_timer* timer = &pwm_servo_timers[i];
+	for (size_t i = 0; i < PWM_SERVO_MAX_TIMERS; ++i) {
+		const struct pwm_servo_timer* timer = &pwm_servo_timers[i];
 
-    uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_SC_OFFSET);
-    regval = regval & ~FTM_SC_CLKS_MASK;
+		uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_SC_OFFSET);
+		regval = regval & ~FTM_SC_CLKS_MASK;
     
-    if (armed) {
-      regval = regval | FTM_SC_CLKS_SYSCLK;
-      putreg32(regval, timer->ftm_base + KINETIS_FTM_SC_OFFSET);
-    } else {
-      regval = regval | FTM_SC_CLKS_NONE;
-      putreg32(regval, timer->ftm_base + KINETIS_FTM_SC_OFFSET);
-    }
+		if (armed) {
+			regval = regval | FTM_SC_CLKS_SYSCLK;
+			putreg32(regval, timer->ftm_base + KINETIS_FTM_SC_OFFSET);
+		} else {
+			regval = regval | FTM_SC_CLKS_NONE;
+			putreg32(regval, timer->ftm_base + KINETIS_FTM_SC_OFFSET);
+		}
     
-  }
+	}
 }
 
 
@@ -136,10 +136,10 @@ void up_pwm_servo_arm(bool armed)
 int
 up_pwm_servo_set_rate(unsigned rate)
 {
-  for (size_t i = 0; i < PWM_SERVO_MAX_TIMERS; ++i) {
-    up_pwm_servo_set_rate_group_update(i, rate);
-  }
-  return 0;
+	for (size_t i = 0; i < PWM_SERVO_MAX_TIMERS; ++i) {
+		up_pwm_servo_set_rate_group_update(i, rate);
+	}
+	return 0;
 }
 
 /**
@@ -153,7 +153,7 @@ up_pwm_servo_set_rate(unsigned rate)
 uint32_t
 up_pwm_servo_get_rate_group(unsigned group)
 {
-  return 0;
+	return 0;
 }
 
 /**
@@ -167,38 +167,38 @@ int
 up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 {
 
-  if (group > PWM_SERVO_MAX_TIMERS) {
-    return -ERANGE;
-  }
+	if (group > PWM_SERVO_MAX_TIMERS) {
+		return -ERANGE;
+	}
   
-  /*
-   * ftm rate = bus frequency / prescaler
-   * rate = ftm rate / modulo
-   *
-   * - bus 48MHz
-   * - prescaler 32
-   * - modulo 16-bit
-   * => minimum rate = 48E6/32/0xffff = 22.8 Hz
-   * => maximum rate = 48E6/32/1 => 1500000 Hz
-   * (arbitrarily limit to allow several duty cycle levels)
-   */
-  if (rate < 23 || 10000 < rate) {
-    return -ERANGE;
-  }
+	/*
+	 * ftm rate = bus frequency / prescaler
+	 * rate = ftm rate / modulo
+	 *
+	 * - bus 48MHz
+	 * - prescaler 32
+	 * - modulo 16-bit
+	 * => minimum rate = 48E6/32/0xffff = 22.8 Hz
+	 * => maximum rate = 48E6/32/1 => 1500000 Hz
+	 * (arbitrarily limit to allow several duty cycle levels)
+	 */
+	if (rate < 23 || 10000 < rate) {
+		return -ERANGE;
+	}
     
-  const struct pwm_servo_timer* timer = &pwm_servo_timers[group];
+	const struct pwm_servo_timer* timer = &pwm_servo_timers[group];
   
-  uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_SC_OFFSET);
-  regval = regval & ~FTM_SC_PS_MASK;
-  regval = regval | FTM_SC_PS_32;
-  putreg32(regval, timer->ftm_base + KINETIS_FTM_SC_OFFSET);
-  putreg32(0, timer->ftm_base + KINETIS_FTM_CNT_OFFSET);
-  putreg32(0, timer->ftm_base + KINETIS_FTM_CNTIN_OFFSET);
-  putreg32(
-	   BOARD_BUS_FREQ / 32 / rate,
-	   timer->ftm_base + KINETIS_FTM_MOD_OFFSET);
+	uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_SC_OFFSET);
+	regval = regval & ~FTM_SC_PS_MASK;
+	regval = regval | FTM_SC_PS_32;
+	putreg32(regval, timer->ftm_base + KINETIS_FTM_SC_OFFSET);
+	putreg32(0, timer->ftm_base + KINETIS_FTM_CNT_OFFSET);
+	putreg32(0, timer->ftm_base + KINETIS_FTM_CNTIN_OFFSET);
+	putreg32(
+		BOARD_BUS_FREQ / 32 / rate,
+		timer->ftm_base + KINETIS_FTM_MOD_OFFSET);
   
-  return 0;
+	return 0;
 }
 
 /**
@@ -210,16 +210,16 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 int
 up_pwm_servo_set(unsigned channel, servo_position_t value)
 {
-  if (channel > PWM_SERVO_MAX_CHANNELS) {
-    return -ERANGE;
-  }
+	if (channel > PWM_SERVO_MAX_CHANNELS) {
+		return -ERANGE;
+	}
   
-  const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
+	const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
 
-  putreg32(
-	   BOARD_BUS_FREQ / 1000000 * ((uint32_t) value) / 32,
-	   ch->timer->ftm_base + KINETIS_FTM_CV_OFFSET(ch->ftm_channel));
-  return 0;
+	putreg32(
+		BOARD_BUS_FREQ / 1000000 * ((uint32_t) value) / 32,
+		ch->timer->ftm_base + KINETIS_FTM_CV_OFFSET(ch->ftm_channel));
+	return 0;
 }
 
 /**
@@ -232,40 +232,12 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
 servo_position_t
 up_pwm_servo_get(unsigned channel)
 {
-  if (channel > PWM_SERVO_MAX_CHANNELS) {
-    return -ERANGE;
-  }
-  
-  const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
-  uint32_t reg = ch->timer->ftm_base + KINETIS_FTM_CV_OFFSET(ch->ftm_channel);
-  uint32_t mod = getreg32(reg);
-  return mod * 32 / (BOARD_BUS_FREQ/1000000);
-}
-
-
-/*
-int helloworld_main(int args, char *argv[]) {
-  printf("Setting timer\n");
-
-  uint8_t ftm_channel = 5;
-  struct pwm_timer* timer = &pwm0;
-  
-
-  uint32_t regval;
-  regval = getreg32(timer->scgc);
-  putreg32(regval | timer->scgc_enable, timer->scgc); // enable clock signal to FTM0
-
-  putreg32(0, timer->ftm_base + KINETIS_FTM_CNT_OFFSET);
-  putreg32(36864, timer->ftm_base + KINETIS_FTM_MOD_OFFSET);
-  
-  putreg32(0x28, timer->ftm_base + KINETIS_FTM_CSC_OFFSET(ftm_channel)); // channle, enable edge aligned pwm
-
-  putreg32(FTM_SC_CLKS_SYSCLK | FTM_SC_PS_1, timer->ftm_base + KINETIS_FTM_SC_OFFSET); // set clock source of FTM module
-  putreg32(atoi(argv[1]), timer->ftm_base + KINETIS_FTM_CV_OFFSET(ftm_channel));
-
-  kinetis_pinconfig(PIN_PORTD | PIN5 |  PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE ) ;
+	if (channel > PWM_SERVO_MAX_CHANNELS) {
+		return -ERANGE;
+	}
   
-  printf("Done\n");
-  return 0;
+	const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
+	uint32_t reg = ch->timer->ftm_base + KINETIS_FTM_CV_OFFSET(ch->ftm_channel);
+	uint32_t mod = getreg32(reg);
+	return mod * 32 / (BOARD_BUS_FREQ/1000000);
 }
-*/
diff --git a/src/drivers/kinetis/module.mk b/src/drivers/kinetis/module.mk
index 2dbf51011..8f3336338 100644
--- a/src/drivers/kinetis/module.mk
+++ b/src/drivers/kinetis/module.mk
@@ -1,3 +1,4 @@
-SRCS = up_cxxinitialize.c drv_pwm_servo.c
+SRCS =	drv_hrt.c \
+	drv_pwm_servo.c
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/kinetis/up_cxxinitialize.c b/src/drivers/kinetis/up_cxxinitialize.c
deleted file mode 100644
index 6c55db6ff..000000000
--- a/src/drivers/kinetis/up_cxxinitialize.c
+++ /dev/null
@@ -1,147 +0,0 @@
-/************************************************************************************
- * 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>
-
-/************************************************************************************
- * 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();
-        }
-    }
-}
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
index 24f4e4207..dfe38864a 100644
--- a/src/modules/systemlib/mcu_version.c
+++ b/src/modules/systemlib/mcu_version.c
@@ -61,9 +61,11 @@
 /** Copy the 96bit MCU Unique ID into the provided pointer */
 void mcu_unique_id(uint32_t *uid_96_bit)
 {
+  #ifdef CONFIG_ARCH_CHIP_STM32
 	uid_96_bit[0] = getreg32(UNIQUE_ID);
 	uid_96_bit[1] = getreg32(UNIQUE_ID+4);
 	uid_96_bit[2] = getreg32(UNIQUE_ID+8);
+  #endif
 }
 
 int mcu_version(char* rev, char** revstr)
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 280c30023..d73e99e4f 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -50,7 +50,9 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 
-#include <stm32_pwr.h>
+#ifdef CONFIG_ARCH_CHIP_STM32
+  #include <stm32_pwr.h>
+#endif
 
 #include "systemlib.h"
 
@@ -60,13 +62,15 @@ extern void up_systemreset(void) noreturn_function;
 void
 systemreset(bool to_bootloader)
 {
+#ifdef CONFIG_ARCH_CHIP_STM32
 	if (to_bootloader) {
 		stm32_pwr_enablebkp();
 
 		/* XXX wow, this is evil - write a magic number into backup register zero */
 		*(uint32_t *)0x40002850 = 0xb007b007;
 	}
-
+#endif
+    
 	up_systemreset();
 
 	/* lock up here */