From b3740ec025a7d06f687d5521d2313fe81fe74d3b Mon Sep 17 00:00:00 2001 From: Mathieu Rondonneau Date: Sun, 3 Jul 2011 12:43:38 -0700 Subject: [PATCH] OP-423: Clean startup and add weak function call for stack swap. --- flight/CopterControl/System/coptercontrol.c | 21 +++++++--- flight/OpenPilot/System/openpilot.c | 17 +++++++++ flight/PiOS.posix/inc/pios_initcall.h | 13 +------ flight/PiOS.win32/inc/pios_initcall.h | 12 +----- .../PiOS/STM32F10x/startup_stm32f10x_HD_OP.S | 29 +++++++++----- .../PiOS/STM32F10x/startup_stm32f10x_MD_CC.S | 38 ++++++++++++------- 6 files changed, 80 insertions(+), 50 deletions(-) diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index 3a5f31d45..f66cf1cf5 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -44,6 +44,7 @@ /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); +extern void Stack_Change(void); /** * OpenPilot Main function: @@ -68,14 +69,24 @@ int main() * */ PIOS_Board_Init(); -#ifdef ERASE_FLASH - PIOS_Flash_W25X_EraseChip(); - while(TRUE){}; -#endif - /* Initialize modules */ MODULE_INITIALISE_ALL + /* swap the stack to use the IRQ stack */ + Stack_Change(); + + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + PIOS_LED_Off(LED1); \ + for(;;) { \ + PIOS_LED_Toggle(LED1); \ + PIOS_DELAY_WaitmS(100); \ + }; + return 0; } diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c index 29e4ca797..81a8f17b1 100644 --- a/flight/OpenPilot/System/openpilot.c +++ b/flight/OpenPilot/System/openpilot.c @@ -64,6 +64,8 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); +extern void Stack_Change(void); +static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); /** * OpenPilot Main function: @@ -99,6 +101,21 @@ int main() xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL); #endif + /* swap the stack to use the IRQ stack (does nothing in sim mode) */ + Stack_Change_Weak(); + + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + PIOS_LED_Off(LED1); \ + for(;;) { \ + PIOS_LED_Toggle(LED1); \ + PIOS_DELAY_WaitmS(100); \ + }; + return 0; } diff --git a/flight/PiOS.posix/inc/pios_initcall.h b/flight/PiOS.posix/inc/pios_initcall.h index 2860d4bf6..32b3f7475 100644 --- a/flight/PiOS.posix/inc/pios_initcall.h +++ b/flight/PiOS.posix/inc/pios_initcall.h @@ -53,18 +53,7 @@ extern void StartModules(); /* Initialize modules */ \ InitModules(); \ /* Initialize the system thread */ \ - SystemModInitialize(); \ - /* Start the FreeRTOS scheduler which never returns.*/ \ - vTaskStartScheduler(); \ - /* If all is well we will never reach here as the scheduler will now be running. */ \ - /* If we do get here, it will most likely be because we ran out of heap space. */ \ - PIOS_LED_Off(LED1); \ - PIOS_LED_Off(LED2); \ - for(;;) { \ - PIOS_LED_Toggle(LED1); \ - PIOS_LED_Toggle(LED2); \ - PIOS_DELAY_WaitmS(100); \ - }; } + SystemModInitialize();} #endif /* PIOS_INITCALL_H */ diff --git a/flight/PiOS.win32/inc/pios_initcall.h b/flight/PiOS.win32/inc/pios_initcall.h index a71f64e03..f370f27ee 100644 --- a/flight/PiOS.win32/inc/pios_initcall.h +++ b/flight/PiOS.win32/inc/pios_initcall.h @@ -47,16 +47,8 @@ /* Initialize modules */ \ InitModules(); \ /* Start the FreeRTOS scheduler which never returns.*/ \ - vTaskStartScheduler(); \ - /* If all is well we will never reach here as the scheduler will now be running. */ \ - /* If we do get here, it will most likely be because we ran out of heap space. */ \ - PIOS_LED_Off(LED1); \ - PIOS_LED_Off(LED2); \ - for(;;) { \ - PIOS_LED_Toggle(LED1); \ - PIOS_LED_Toggle(LED2); \ - PIOS_DELAY_WaitmS(100); \ - }; } + /* Initialize the system thread */ \ + SystemModInitialize();} #endif /* PIOS_INITCALL_H */ diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S b/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S index 7e9964cb9..f6b6ba7a8 100644 --- a/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S +++ b/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S @@ -124,9 +124,25 @@ LoopFillZerobss: bcc FillZerobss /* Call the application's entry point.*/ bl main +/* will never return here */ + bx lr +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that swaps stack (from end of heap to irq_stack). + * Also reclaim the heap that was used as a stack. + * @param None + * @retval : None +*/ + .section .text.Stack_Change + .weak Stack_Change + .type Stack_Change, %function +Stack_Change: + mov r4, lr /* Switches stack back momentarily to MSP */ - add r0, #0 + movs r0, #0 msr control, r0 +Heap_Reclaim: /* add heap_post_rtos to the heap (if the capability/function exist) */ /* Also claim the unused memory (between end of heap to end of memory */ /* CAREFULL: the heap section must be the last section in RAM in order this to work */ @@ -136,15 +152,8 @@ LoopFillZerobss: subs r2, r2, r1 adds r0, r0, r2 bl xPortIncreaseHeapSize -/* Start the FreeRTOS scheduler which never returns.*/ - bl vTaskStartScheduler -/* If all is well we will never reach here as the scheduler will now be running. */ -/* If we do get here, it will most likely be because we ran out of heap space. */ -/* let the Memory manager handler figure it out (at least for now, it will make it more obvious */ - bl MemManage_Handler -/* will never return here for now until mem manager give up */ - bx lr -.size Reset_Handler, .-Reset_Handler + bx r4 + .size Stack_Change, .-Stack_Change /** * @brief Dummy SystemInit_ExtMemCtl function diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S index 72cf5280f..9c511b5c3 100644 --- a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S +++ b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S @@ -35,6 +35,7 @@ .global Default_Handler .global vTaskStartScheduler .global xPortIncreaseHeapSize +.global Stack_Change /* start address for the initialization values of the .data section. defined in linker script */ @@ -49,6 +50,7 @@ defined in linker script */ .word _ebss .equ BootRAM, 0xF108F85F +.equ WWDG, 0x40002C00 /** * @brief This is the code that gets called when the processor first @@ -127,9 +129,25 @@ LoopFillZerobss: bcc FillZerobss /* Call the application's entry point.*/ bl main +/* will never return here */ + bx lr +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that swaps stack (from end of heap to irq_stack). + * Also reclaim the heap that was used as a stack. + * @param None + * @retval : None +*/ + .section .text.Stack_Change + .weak Stack_Change + .type Stack_Change, %function +Stack_Change: + mov r4, lr /* Switches stack back momentarily to MSP */ - add r0, #0 + movs r0, #0 msr control, r0 +Heap_Reclaim: /* add heap_post_rtos to the heap (if the capability/function exist) */ /* Also claim the unused memory (between end of heap to end of memory */ /* CAREFULL: the heap section must be the last section in RAM in order this to work */ @@ -139,23 +157,17 @@ LoopFillZerobss: subs r2, r2, r1 adds r0, r0, r2 bl xPortIncreaseHeapSize -/* Start the FreeRTOS scheduler which never returns.*/ - bl vTaskStartScheduler -/* If all is well we will never reach here as the scheduler will now be running. */ -/* If we do get here, it will most likely be because we ran out of heap space. */ -/* let the Memory manager handler figure it out (at least for now, it will make it more obvious */ - bl MemManage_Handler -/* will never return here for now until mem manager give up */ - bx lr -.size Reset_Handler, .-Reset_Handler + bx r4 + .size Stack_Change, .-Stack_Change + /** - * @brief This is the code that gets called when the processor receives an + * @brief This is the code that gets called when the processor receives an * unexpected interrupt. This simply enters an infinite loop, preserving * the system state for examination by a debugger. * - * @param None - * @retval : None + * @param None + * @retval : None */ .section .text.Default_Handler,"ax",%progbits Default_Handler: