From 3780de8d3e28b050cd353487f57287de2c8fc01d Mon Sep 17 00:00:00 2001 From: Mathieu Rondonneau Date: Tue, 14 Jun 2011 20:10:53 -0700 Subject: [PATCH] OP-423 port to OP (heap2) the previous changes done in CC (heap1) (see c95b1991665944bed634d9cfe7fccb21dbedb167) I managed to test CC with heap2 changes and the init stack claimed back to heap once scheduler starts. the changes of this commit are OP related (just cleanup on CC side): Arch specific stuff (in reset vector) to hide this from portable code: - switch back to MSP stack before starting the scheduler so that the sheduler can use the IRQ stack (when/if needed). - call the C portable function in heap2 to claim some stack back (the number to claim is taken from linker file). - start the scheduler from reset vector (I move this here from main because it make sense to not go back to C (so that I don't need to copy the rolled stack in case the sheduler returns). This make it more clean. - Also I have added the call to the mem manager if sheduler return. that way, we don't reset indefinitely if memory runs out. We will go to this handler and figure things out (right now, it's just looping but at least not rebooting. Probably trap NMI would be better (later improvement). --- flight/OpenPilot/System/openpilot.c | 11 ++++++-- .../FreeRTOS/Source/portable/MemMang/heap_2.c | 17 ++++++++++--- .../STM32F10x/link_STM3210E_OP_sections.ld | 10 +++++++- .../PiOS/STM32F10x/startup_stm32f10x_HD_OP.S | 25 ++++++++++++++----- .../PiOS/STM32F10x/startup_stm32f10x_MD_CC.S | 2 +- 5 files changed, 52 insertions(+), 13 deletions(-) diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c index be50653f7..3f89f2df6 100644 --- a/flight/OpenPilot/System/openpilot.c +++ b/flight/OpenPilot/System/openpilot.c @@ -88,7 +88,14 @@ int main() /* Initialize the system thread */ SystemModInitialize(); - /* Start the FreeRTOS scheduler */ + /* only do this for posix and win32 since the caller will take care + * of starting the scheduler and increase the heap and swith back to + * MSP stack. (all arch specific is hidden from here and take care by reset handler) + * LED blinking in case of scheduler returning back should be handled in NMI or other + * appropriate handlers like mem manager. + */ +#if defined(ARCH_POSIX) || defined(ARCH_WIN32) + /* Start the FreeRTOS scheduler which never returns.*/ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ @@ -100,7 +107,7 @@ int main() PIOS_LED_Toggle(LED2); PIOS_DELAY_WaitmS(100); } - +#endif return 0; } diff --git a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c index 0f8097561..aec6b41c6 100644 --- a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c +++ b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c @@ -101,6 +101,7 @@ static xBlockLink xStart, xEnd; /* Keeps track of the number of free bytes remaining, but says nothing about fragmentation. */ static size_t xFreeBytesRemaining = configTOTAL_HEAP_SIZE; +static size_t currentTOTAL_HEAP_SISE = configTOTAL_HEAP_SIZE; /* STATIC FUNCTIONS ARE DEFINED AS MACROS TO MINIMIZE THE FUNCTION CALL DEPTH. */ @@ -140,13 +141,13 @@ xBlockLink *pxFirstFreeBlock; \ xStart.xBlockSize = ( size_t ) 0; \ \ /* xEnd is used to mark the end of the list of free blocks. */ \ - xEnd.xBlockSize = configTOTAL_HEAP_SIZE; \ + xEnd.xBlockSize = currentTOTAL_HEAP_SISE; \ xEnd.pxNextFreeBlock = NULL; \ \ /* To start with there is a single free block that is sized to take up the \ entire heap space. */ \ pxFirstFreeBlock = ( void * ) xHeap.ucHeap; \ - pxFirstFreeBlock->xBlockSize = configTOTAL_HEAP_SIZE; \ + pxFirstFreeBlock->xBlockSize = currentTOTAL_HEAP_SISE; \ pxFirstFreeBlock->pxNextFreeBlock = &xEnd; \ } /*-----------------------------------------------------------*/ @@ -181,7 +182,7 @@ void *pvReturn = NULL; } } - if( ( xWantedSize > 0 ) && ( xWantedSize < configTOTAL_HEAP_SIZE ) ) + if( ( xWantedSize > 0 ) && ( xWantedSize < currentTOTAL_HEAP_SISE ) ) { /* Blocks are stored in byte order - traverse the list from the start (smallest) block until one of adequate size is found. */ @@ -276,3 +277,13 @@ void vPortInitialiseBlocks( void ) { /* This just exists to keep the linker quiet. */ } + +void xPortIncreaseHeapSize( size_t bytes ) +{ + vTaskSuspendAll(); + currentTOTAL_HEAP_SISE = configTOTAL_HEAP_SIZE + bytes; + xEnd.xBlockSize = currentTOTAL_HEAP_SISE; + xFreeBytesRemaining += bytes; + xTaskResumeAll(); +} +/*-----------------------------------------------------------*/ diff --git a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld index 12059ac1e..e3919e295 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld @@ -1,5 +1,7 @@ /* This is the size of the stack for early init and for all FreeRTOS IRQs */ _irq_stack_size = 0x400; +/* This is the size of the stack for early init: life span is until scheduler starts */ +_init_stack_size = 0x400; /* Check valid alignment for VTOR */ ASSERT(ORIGIN(FLASH) == ALIGN(ORIGIN(FLASH), 0x80), "Start of memory region flash not aligned for startup vector table"); @@ -264,11 +266,17 @@ SECTIONS .heap (NOLOAD) : { _sheap = . ; + _sheap_pre_rtos = . ; *(.heap) . = ALIGN(4); _eheap = . ; + _eheap_pre_rtos = . ; + _init_stack_end = . ; + _sheap_post_rtos = . ; + . = . + _init_stack_size ; + . = ALIGN(4); + _eheap_post_rtos = . ; _init_stack_top = . - 4 ; - . = ALIGN(4); } > RAM PROVIDE ( end = _ebss ); diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S b/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S index 7563e4c0d..a734143d8 100644 --- a/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S +++ b/flight/PiOS/STM32F10x/startup_stm32f10x_HD_OP.S @@ -36,6 +36,8 @@ .global g_pfnVectors .global SystemInit_ExtMemCtl_Dummy .global Default_Handler +.global vTaskStartScheduler +.global xPortIncreaseHeapSize /* start address for the initialization values of the .data section. defined in linker script */ @@ -71,7 +73,6 @@ Reset_Handler: /* restore original stack pointer */ LDR r0, =_irq_stack_top MSR msp, r0 - LDR r0, =_init_stack_top MSR psp, r0 /* DO @@ -94,7 +95,6 @@ LoopFillIRQStack: ldr r3, = _irq_stack_top cmp r2, r3 bcc FillIRQStack - /* Copy the data segment initializers from flash to SRAM */ movs r1, #0 b LoopCopyDataInit @@ -124,7 +124,20 @@ LoopFillZerobss: bcc FillZerobss /* Call the application's entry point.*/ bl main - bx lr +/* Switches stack back momentarily to MSP */ + add r0, #0 + msr control, r0 +/* add heap_post_rtos to the heap (if the capability/function exist) */ + ldr r0, = _init_stack_size + 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 /** @@ -296,13 +309,13 @@ g_pfnVectors: .weak NMI_Handler .thumb_set NMI_Handler,Default_Handler - + .weak HardFault_Handler .thumb_set HardFault_Handler,Default_Handler - + .weak MemManage_Handler .thumb_set MemManage_Handler,Default_Handler - + .weak BusFault_Handler .thumb_set BusFault_Handler,Default_Handler diff --git a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S index f35ca0699..a935c64fc 100644 --- a/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S +++ b/flight/PiOS/STM32F10x/startup_stm32f10x_MD_CC.S @@ -168,7 +168,7 @@ Infinite_Loop: g_pfnVectors: - .word _irq_stack_top /*_irq_stack_top */ + .word _irq_stack_top .word Reset_Handler .word NMI_Handler .word HardFault_Handler