From 2b3cc549e9e2e0ff8e9d0829344c990a4b8b5940 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 1 Dec 2016 09:16:44 +0000 Subject: [PATCH] Enabled CCM RAM for stack. Added stack watermarking --- Makefile | 1 + src/main/build/debug.h | 1 + src/main/drivers/stack_check.c | 99 ++++++++++++++++++++++++++ src/main/drivers/stack_check.h | 25 +++++++ src/main/fc/fc_tasks.c | 13 ++++ src/main/io/serial_cli.c | 6 ++ src/main/scheduler/scheduler.h | 3 + src/main/startup/startup_stm32f40xx.s | 24 ++++++- src/main/startup/startup_stm32f411xe.s | 24 ++++++- src/main/target/link/stm32_flash.ld | 13 +++- 10 files changed, 204 insertions(+), 5 deletions(-) create mode 100644 src/main/drivers/stack_check.c create mode 100755 src/main/drivers/stack_check.h diff --git a/Makefile b/Makefile index 5708af8af..f2edc40d5 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 27377be76..6b2e7ca9f 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 000000000..d153d4c76 --- /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 000000000..ccd29c9bb --- /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 a94666b37..622a00528 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 d2a85c239..7fb8ba842 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 2e0d0efbb..d1bdb4a3e 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 b19a8ef54..db7c6635f 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 f3003a67e..8cdbbb6f3 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 40bc9965b..2cc81c9fd 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"))); */