diff --git a/Makefile b/Makefile
index 5708af8af9..f2edc40d58 100644
--- a/Makefile
+++ b/Makefile
@@ -506,6 +506,7 @@ COMMON_SRC = \
drivers/serial.c \
drivers/serial_uart.c \
drivers/sound_beeper.c \
+ drivers/stack_check.c \
drivers/system.c \
drivers/timer.c \
fc/config.c \
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 27377be765..6b2e7ca9f9 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -61,5 +61,6 @@ typedef enum {
DEBUG_ANGLERATE,
DEBUG_ESC_TELEMETRY,
DEBUG_SCHEDULER,
+ DEBUG_STACK,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/drivers/stack_check.c b/src/main/drivers/stack_check.c
new file mode 100644
index 0000000000..d153d4c763
--- /dev/null
+++ b/src/main/drivers/stack_check.c
@@ -0,0 +1,99 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/utils.h"
+
+#include "drivers/stack_check.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
+
+/*
+ * 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
+ *
+ */
+
+#ifdef STACK_CHECK
+
+static uint32_t usedStackSize;
+
+void taskStackCheck(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ 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;
+ }
+ }
+
+ usedStackSize = (uint32_t)stackHighMem - (uint32_t)p;
+
+ DEBUG_SET(DEBUG_STACK, 0, (uint32_t)stackHighMem & 0xffff);
+ DEBUG_SET(DEBUG_STACK, 1, (uint32_t)stackLowMem & 0xffff);
+ DEBUG_SET(DEBUG_STACK, 2, (uint32_t)stackCurrent & 0xffff);
+ DEBUG_SET(DEBUG_STACK, 3, (uint32_t)p & 0xffff);
+}
+
+uint32_t stackUsedSize(void)
+{
+ return usedStackSize;
+}
+#endif
+
+uint32_t stackTotalSize(void)
+{
+ return (uint32_t)&_Min_Stack_Size;
+}
+
+uint32_t stackHighMem(void)
+{
+ return (uint32_t)&_estack;
+}
diff --git a/src/main/drivers/stack_check.h b/src/main/drivers/stack_check.h
new file mode 100755
index 0000000000..ccd29c9bb0
--- /dev/null
+++ b/src/main/drivers/stack_check.h
@@ -0,0 +1,25 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+void taskStackCheck(timeUs_t currentTimeUs);
+uint32_t stackUsedSize(void);
+uint32_t stackTotalSize(void);
+uint32_t stackHighMem(void);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index fc93fb98f3..27ed999aef 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -31,6 +31,7 @@
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/serial.h"
+#include "drivers/stack_check.h"
#include "fc/config.h"
#include "fc/fc_msp.h"
@@ -286,6 +287,9 @@ void fcTasksInit(void)
setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
#endif
#endif
+#ifdef STACK_CHECK
+ setTaskEnabled(TASK_STACK_CHECK, true);
+#endif
}
cfTask_t cfTasks[TASK_COUNT] = {
@@ -463,4 +467,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
+
+#ifdef STACK_CHECK
+ [TASK_STACK_CHECK] = {
+ .taskName = "STACKCHECK",
+ .taskFunc = taskStackCheck,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz
+ .staticPriority = TASK_PRIORITY_IDLE,
+ },
+#endif
};
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 4a5b6cfe83..3cad9dd6d1 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -58,6 +58,7 @@ uint8_t cliMode = 0;
#include "drivers/sdcard.h"
#include "drivers/buf_writer.h"
#include "drivers/serial_escserial.h"
+#include "drivers/stack_check.h"
#include "drivers/vcd.h"
#include "fc/config.h"
@@ -3600,6 +3601,11 @@ static void cliStatus(char *cmdline)
uint16_t i2cErrorCounter = 0;
#endif
+#ifdef STACK_CHECK
+ cliPrintf("Stack used: %d, ", stackUsedSize());
+#endif
+ cliPrintf("Stack size: %d, Stack address: 0x%x\r\n", stackTotalSize(), stackHighMem());
+
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
#ifdef USE_SDCARD
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 2e0d0efbb7..d1bdb4a3ed 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -85,6 +85,9 @@ typedef enum {
#ifdef TRANSPONDER
TASK_TRANSPONDER,
#endif
+#ifdef STACK_CHECK
+ TASK_STACK_CHECK,
+#endif
#ifdef OSD
TASK_OSD,
#endif
diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s
index b19a8ef54f..db7c6635f9 100644
--- a/src/main/startup/startup_stm32f40xx.s
+++ b/src/main/startup/startup_stm32f40xx.s
@@ -70,7 +70,16 @@ defined in linker script */
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
-Reset_Handler:
+Reset_Handler:
+ // Enable CCM
+ // RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN;
+ ldr r0, =0x40023800 // RCC_BASE
+ ldr r1, [r0, #0x30] // AHB1ENR
+ orr r1, r1, 0x00100000 // RCC_AHB1ENR_CCMDATARAMEN
+ str r1, [r0, #0x30]
+ dsb
+
+ // Check for bootloader reboot
ldr r0, =0x2001FFFC // mj666
ldr r1, =0xDEADBEEF // mj666
ldr r2, [r0, #0] // mj666
@@ -106,6 +115,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]
diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s
index f3003a67eb..8cdbbb6f38 100644
--- a/src/main/startup/startup_stm32f411xe.s
+++ b/src/main/startup/startup_stm32f411xe.s
@@ -70,7 +70,16 @@ defined in linker script */
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
-Reset_Handler:
+Reset_Handler:
+ // Enable CCM
+ // RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN;
+ ldr r0, =0x40023800 // RCC_BASE
+ ldr r1, [r0, #0x30] // AHB1ENR
+ orr r1, r1, 0x00100000 // RCC_AHB1ENR_CCMDATARAMEN
+ str r1, [r0, #0x30]
+ dsb
+
+ // Check for bootloader reboot
ldr r0, =0x2001FFFC // mj666
ldr r1, =0xDEADBEEF // mj666
ldr r2, [r0, #0] // mj666
@@ -106,6 +115,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]
diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld
index 40bc9965ba..2cc81c9fd5 100644
--- a/src/main/target/link/stm32_flash.ld
+++ b/src/main/target/link/stm32_flash.ld
@@ -12,11 +12,15 @@
ENTRY(Reset_Handler)
/* Highest address of the user mode stack */
-_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */
+_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM); /* end of RAM */
+
+/* Base address where the config is stored. */
+__config_start = ORIGIN(FLASH_CONFIG);
+__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG);
/* Generate a link error if heap and stack don't fit into RAM */
_Min_Heap_Size = 0; /* required amount of heap */
-_Min_Stack_Size = 0x400; /* required amount of stack */
+_Min_Stack_Size = 0x800; /* required amount of stack */
/* Define output sections */
SECTIONS
@@ -110,6 +114,9 @@ SECTIONS
} >RAM
/* User_heap_stack section, used to check that there is enough RAM left */
+ _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 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 +125,7 @@ SECTIONS
. = . + _Min_Heap_Size;
. = . + _Min_Stack_Size;
. = ALIGN(4);
- } >RAM
+ } >STACKRAM = 0xa5
/* MEMORY_bank1 section, code must be located here explicitly */
/* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */