Added stack checking

3 files changed, 42 insertions, 2 deletions

diff --git a/nuttx/configs/px4fmu-v1/src/Makefile b/nuttx/configs/px4fmu-v1/src/Makefile
index 07ae4ecff..9b914e52a 100644
--- a/nuttx/configs/px4fmu-v1/src/Makefile
+++ b/nuttx/configs/px4fmu-v1/src/Makefile
@@ -40,7 +40,7 @@ CFLAGS += -I$(TOPDIR)/sched
 ASRCS = 
 AOBJS = $(ASRCS:.S=$(OBJEXT))
 
-CSRCS = up_boot.c up_spi.c
+CSRCS = up_boot.c up_spi.c up_stackcheck.c
 
 ifeq ($(CONFIG_HAVE_CXX),y)
 CSRCS += up_cxxinitialize.c
diff --git a/nuttx/configs/px4fmu-v1/src/up_spi.c b/nuttx/configs/px4fmu-v1/src/up_spi.c
index db4d1e9de..63d1b8520 100644
--- a/nuttx/configs/px4fmu-v1/src/up_spi.c
+++ b/nuttx/configs/px4fmu-v1/src/up_spi.c
@@ -93,7 +93,7 @@
  *
  ************************************************************************************/
 
-void weak_function stm32_spiinitialize(void)
+void stm32_spiinitialize(void)
 {
 #ifdef CONFIG_STM32_SPI1
   stm32_configgpio(GPIO_SPI_CS_GYRO);
diff --git a/nuttx/configs/px4fmu-v1/src/up_stackcheck.c b/nuttx/configs/px4fmu-v1/src/up_stackcheck.c
new file mode 100644
index 000000000..e8f02a863
--- /dev/null
+++ b/nuttx/configs/px4fmu-v1/src/up_stackcheck.c
@@ -0,0 +1,40 @@
+
+
+void    __cyg_profile_func_enter(void *func, void *caller) __attribute__((naked, no_instrument_function));
+void    __cyg_profile_func_exit(void *func, void *caller)  __attribute__((naked, no_instrument_function));
+void    __stack_overflow_trap(void) __attribute__((naked, no_instrument_function));
+
+void
+__stack_overflow_trap(void)
+{
+    /* if we get here, the stack has overflowed */
+    asm ( "b .");
+}
+
+void
+__cyg_profile_func_enter(void *func, void *caller)
+{
+    asm volatile (
+            "   mrs r2, ipsr        \n" /* Check whether we are in interrupt mode */
+            "   cmp r2, #0          \n" /* since we don't switch r10 on interrupt entry, we */
+            "   bne 2f              \n" /* can't detect overflow of the interrupt stack. */
+            "                       \n"
+            "   sub r2, sp, #68     \n" /* compute stack pointer as though we just stacked a full frame */
+            "   mrs r1, control     \n" /* Test CONTROL.FPCA to see whether we also need room for the FP */
+            "   tst r1, #4          \n" /* context. */
+            "   beq 1f              \n"
+            "   sub r2, r2, #136    \n" /* subtract FP context frame size */
+            "1:                     \n"
+            "   cmp r2, r10         \n" /* compare stack with limit */
+            "   bgt 2f              \n" /* stack is above limit and thus OK */
+            "   b __stack_overflow_trap\n"
+            "2:                     \n"
+            "   bx lr               \n"
+    );
+}
+
+void
+__cyg_profile_func_exit(void *func, void *caller)
+{
+    asm volatile("bx lr");
+}