Merge pull request #642 from iNavFlight/martinbudden-inav_stack_check

Stack watermarking

Makefile

@@ -396,6 +396,7 @@ COMMON_SRC = \
             drivers/serial.c \
             drivers/serial_uart.c \
             drivers/sound_beeper.c \
+            drivers/stack_check.c \
             drivers/system.c \
             drivers/timer.c \
 			drivers/io_pca9685.c \

src/main/drivers/stack_check.c

@@ -0,0 +1,79 @@
+/*
+ * stack_check.c
+ *
+ *  Created on: 23 Aug 2016
+ *      Author: martinbudden
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef STACK_CHECK
+
+#include "build/debug.h"
+
+#define STACK_FILL_CHAR 0xa5
+
+extern char _estack; // end of stack, declared in .LD file
+extern char _Min_Stack_Size; // declared in .LD file
+static uint32_t _Used_Stack_Size;
+
+/*
+ * The ARM processor uses a full descending stack. This means the stack pointer holds the address
+ * of the last stacked item in memory. When the processor pushes a new item onto the stack,
+ * it decrements the stack pointer and then writes the item to the new memory location.
+ *
+ *
+ * RAM layout is generally as below, although some targets vary
+ *
+ * F1 Boards
+ * RAM is origin 0x20000000 length 20K that is:
+ * 0x20000000 to 0x20005000
+ *
+ * F3 Boards
+ * RAM is origin 0x20000000 length 40K that is:
+ * 0x20000000 to 0x2000a000
+ *
+ * F4 Boards
+ * RAM is origin 0x20000000 length 128K that is:
+ * 0x20000000 to 0x20020000
+ *
+ */
+void taskStackCheck(void)
+{
+    char * const stackHighMem = &_estack;
+    const uint32_t stackSize = (uint32_t)&_Min_Stack_Size;
+    char * const stackLowMem = stackHighMem - stackSize;
+    const char * const stackCurrent = (char *)&stackLowMem;
+
+    char *p;
+	for (p = stackLowMem; p < stackCurrent; ++p) {
+		if (*p != STACK_FILL_CHAR) {
+			break;
+		}
+	}
+
+    _Used_Stack_Size = (uint32_t)stackHighMem - (uint32_t)p;
+
+#ifdef DEBUG_STACK
+	debug[0] = (uint32_t)stackHighMem & 0xffff;
+	debug[1] = (uint32_t)stackLowMem & 0xffff;
+	debug[2] = (uint32_t)stackCurrent & 0xffff;
+	debug[3] = (uint32_t)p & 0xffff;
+#endif
+}
+
+uint32_t getTotalStackSize(void)
+{
+    return (uint32_t)&_Min_Stack_Size;
+}
+
+uint32_t getUsedStackSize(void)
+{
+    return _Used_Stack_Size;
+}
+
+#endif

src/main/drivers/stack_check.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef STACK_CHECK
+uint32_t getTotalStackSize(void);
+uint32_t getUsedStackSize(void);
+#endif

src/main/io/serial_cli.c

@@ -52,6 +52,7 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/sdcard.h"
+#include "drivers/stack_check.h"
 
 #include "drivers/buf_writer.h"
 
@@ -2818,6 +2819,10 @@ static void cliStatus(char *cmdline)
     const uint16_t i2cErrorCounter = 0;
 #endif
 
+#ifdef STACK_CHECK
+    cliPrintf("Used stack: %d, Total stack: %d\r\n", getUsedStackSize(), getTotalStackSize());
+#endif
+
     cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
 }
 

src/main/main.c

@@ -652,6 +652,9 @@ int main(void)
 #ifdef LED_STRIP
     setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
 #endif
+#ifdef STACK_CHECK
+    setTaskEnabled(TASK_STACK_CHECK, true);
+#endif
 
 #ifdef USE_PMW_SERVO_DRIVER
     setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER));

src/main/scheduler/scheduler.h

@@ -71,6 +71,9 @@ typedef enum {
 #ifdef USE_PMW_SERVO_DRIVER
     TASK_PWMDRIVER,
 #endif
+#ifdef STACK_CHECK
+    TASK_STACK_CHECK,
+#endif
 
     /* Count of real tasks */
     TASK_COUNT,

src/main/scheduler/scheduler_tasks.c

@@ -139,4 +139,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef STACK_CHECK
+    [TASK_STACK_CHECK] = {
+        .taskName = "STACKCHECK",
+        .taskFunc = taskStackCheck,
+        .desiredPeriod = 1000000 / 10,          // 10 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
 };

src/main/scheduler/scheduler_tasks.h

@@ -36,3 +36,4 @@ void taskSystem(void);
 #ifdef USE_PMW_SERVO_DRIVER
 void taskSyncPwmDriver(void);
 #endif
+void taskStackCheck(void);

src/main/startup/startup_stm32f10x_hd_gcc.S

@@ -98,6 +98,19 @@ LoopFillZerobss:
 	cmp	r2, r3
 	bcc	FillZerobss
 
+/* Mark the heap and stack */
+    ldr	r2, =_heap_stack_begin
+    b	LoopMarkHeapStack
+
+MarkHeapStack:
+	movs	r3, 0xa5a5a5a5
+	str	r3, [r2], #4
+
+LoopMarkHeapStack:
+	ldr	r3, = _heap_stack_end
+	cmp	r2, r3
+	bcc	MarkHeapStack
+   
 /* Call the clock system intitialization function.*/
     bl  SystemInit
 /* Call the application's entry point.*/

src/main/startup/startup_stm32f10x_md_gcc.S

@@ -97,6 +97,19 @@ LoopFillZerobss:
 	cmp	r2, r3
 	bcc	FillZerobss
 
+/* Mark the heap and stack */
+    ldr	r2, =_heap_stack_begin
+    b	LoopMarkHeapStack
+
+MarkHeapStack:
+	movs	r3, 0xa5a5a5a5
+	str	r3, [r2], #4
+
+LoopMarkHeapStack:
+	ldr	r3, = _heap_stack_end
+	cmp	r2, r3
+	bcc	MarkHeapStack
+
 /* Call the clock system intitialization function.*/
     bl  SystemInit
 /* Call the application's entry point.*/

src/main/startup/startup_stm32f30x_md_gcc.S

@@ -105,6 +105,19 @@ LoopFillZerobss:
   cmp  r2, r3
   bcc  FillZerobss
 
+/* Mark the heap and stack */
+    ldr	r2, =_heap_stack_begin
+    b	LoopMarkHeapStack
+
+MarkHeapStack:
+	movs	r3, 0xa5a5a5a5
+	str	r3, [r2], #4
+
+LoopMarkHeapStack:
+	ldr	r3, = _heap_stack_end
+	cmp	r2, r3
+	bcc	MarkHeapStack
+
 /* Call the clock system intitialization function.*/
   bl  SystemInit
 /* Call the application's entry point.*/

src/main/startup/startup_stm32f3_debug_hardfault_handler.S

@@ -108,6 +108,19 @@ LoopFillZerobss:
   cmp  r2, r3
   bcc  FillZerobss
 
+/* Mark the heap and stack */
+    ldr	r2, =_heap_stack_begin
+    b	LoopMarkHeapStack
+
+MarkHeapStack:
+	movs	r3, 0xa5a5a5a5
+	str	r3, [r2], #4
+
+LoopMarkHeapStack:
+	ldr	r3, = _heap_stack_end
+	cmp	r2, r3
+	bcc	MarkHeapStack
+
 /* Call the clock system intitialization function.*/
   bl  SystemInit
 /* Call the application's entry point.*/

src/main/startup/startup_stm32f40xx.s

@@ -107,6 +107,19 @@ LoopFillZerobss:
   cmp  r2, r3
   bcc  FillZerobss
 
+/* Mark the heap and stack */
+    ldr	r2, =_heap_stack_begin
+    b	LoopMarkHeapStack
+
+MarkHeapStack:
+	movs	r3, 0xa5a5a5a5
+	str	r3, [r2], #4
+
+LoopMarkHeapStack:
+	ldr	r3, = _heap_stack_end
+	cmp	r2, r3
+	bcc	MarkHeapStack
+
 /*FPU settings*/
  ldr     r0, =0xE000ED88           /* Enable CP10,CP11 */
  ldr     r1,[r0]

src/main/startup/startup_stm32f411xe.s

@@ -107,6 +107,19 @@ LoopFillZerobss:
   cmp  r2, r3
   bcc  FillZerobss
 
+/* Mark the heap and stack */
+    ldr	r2, =_heap_stack_begin
+    b	LoopMarkHeapStack
+
+MarkHeapStack:
+	movs	r3, 0xa5a5a5a5
+	str	r3, [r2], #4
+
+LoopMarkHeapStack:
+	ldr	r3, = _heap_stack_end
+	cmp	r2, r3
+	bcc	MarkHeapStack
+
 /*FPU settings*/
  ldr     r0, =0xE000ED88           /* Enable CP10,CP11 */
  ldr     r1,[r0]

src/main/target/CJMCU/target.h

@@ -109,6 +109,9 @@
 #undef BLACKBOX
 #endif
 
+#define STACK_CHECK
+#define DEBUG_STACK
+
 // Number of available PWM outputs
 #define MAX_PWM_OUTPUT_PORTS    4
 

src/main/target/link/stm32_flash.ld

@@ -110,6 +110,9 @@ SECTIONS
   } >RAM
 
   /* User_heap_stack section, used to check that there is enough RAM left */
+  _heap_stack_end = ORIGIN(RAM)+LENGTH(RAM) - 8; /* 8 bytes to allow for alignment */
+  _heap_stack_begin = _heap_stack_end - _Min_Stack_Size  - _Min_Heap_Size;
+  . = _heap_stack_begin;
   ._user_heap_stack :
   {
     . = ALIGN(4);
@@ -118,7 +121,7 @@ SECTIONS
     . = . + _Min_Heap_Size;
     . = . + _Min_Stack_Size;
     . = ALIGN(4);
-  } >RAM
+  } >RAM = 0xa5
 
   /* MEMORY_bank1 section, code must be located here explicitly            */
   /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */