diff --git a/FreeRTOS/Source/portable/GCC/ARM_CM33/port.c b/FreeRTOS/Source/portable/GCC/ARM_CM33/port.c new file mode 100644 index 0000000000..a53facef56 --- /dev/null +++ b/FreeRTOS/Source/portable/GCC/ARM_CM33/port.c @@ -0,0 +1,634 @@ +/* + FreeRTOS V8.2.1 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation >>!AND MODIFIED BY!<< the FreeRTOS exception. + + *************************************************************************** + >>! NOTE: The modification to the GPL is included to allow you to !<< + >>! distribute a combined work that includes FreeRTOS without being !<< + >>! obliged to provide the source code for proprietary components !<< + >>! outside of the FreeRTOS kernel. !<< + *************************************************************************** + + FreeRTOS 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. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + +/*----------------------------------------------------------- + * Implementation of functions defined in portable.h for the ARM CM33F port. + * TODO: some day it might be nice to use PSPLIM/MSPLIM + *----------------------------------------------------------*/ + +/* Scheduler includes. */ +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" +#include + +#ifndef __VFP_FP__ + #error This port can only be used when the project options are configured to enable hardware floating point support. +#endif + +/* Constants required to access and manipulate the NVIC. */ +#define portNVIC_SYSTICK_CTRL ( ( volatile uint32_t * ) 0xe000e010 ) +#define portNVIC_SYSTICK_LOAD ( ( volatile uint32_t * ) 0xe000e014 ) +#define portNVIC_SYSPRI2 ( ( volatile uint32_t * ) 0xe000ed20 ) +#define portNVIC_SYSPRI1 ( ( volatile uint32_t * ) 0xe000ed1c ) +#define portNVIC_SYS_CTRL_STATE ( ( volatile uint32_t * ) 0xe000ed24 ) +#define portNVIC_MEM_FAULT_ENABLE ( 1UL << 16UL ) + +/* Constants required to access and manipulate the MPU. */ +#define portMPU_TYPE ( ( volatile uint32_t * ) 0xe000ed90 ) +#define portMPU_REGION_BASE_ADDRESS ( ( volatile uint32_t * ) 0xe000ed9C ) +#define portMPU_REGION_ATTRIBUTE ( ( volatile uint32_t * ) 0xe000edA0 ) +#define portMPU_CTRL ( ( volatile uint32_t * ) 0xe000ed94 ) +#define portMPU_REGION_VALID ( 0x10UL ) +#define portMPU_REGION_ENABLE ( 0x01UL ) +#define portPERIPHERALS_START_ADDRESS 0x40000000UL +#define portPERIPHERALS_END_ADDRESS 0x5FFFFFFFUL + +/* Constants required to access and manipulate the SysTick. */ +#define portNVIC_SYSTICK_CLK ( 0x00000004UL ) +#define portNVIC_SYSTICK_INT ( 0x00000002UL ) +#define portNVIC_SYSTICK_ENABLE ( 0x00000001UL ) +#define portNVIC_PENDSV_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL ) +#define portNVIC_SYSTICK_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) +#define portNVIC_SVC_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) + +/* Constants required to manipulate the VFP. */ +#define portFPCCR ( ( volatile uint32_t * ) 0xe000ef34 ) /* Floating point context control register. */ +#define portASPEN_AND_LSPEN_BITS ( 0x3UL << 30UL ) + +/* Constants required to set up the initial stack. */ +#define portINITIAL_XPSR ( 0x01000000 ) +#define portINITIAL_EXEC_RETURN ( 0xfffffffd ) +#define portINITIAL_CONTROL_IF_UNPRIVILEGED ( 0x03 ) +#define portINITIAL_CONTROL_IF_PRIVILEGED ( 0x02 ) + +/* Offsets in the stack to the parameters when inside the SVC handler. */ +#define portOFFSET_TO_PC ( 6 ) +#define portOFFSET_TO_LR ( 5 ) +#define portOFFSET_TO_PSR ( 7 ) +#define portPSR_STACK_PADDING_MASK (1UL << 9UL) + +/* Each task maintains its own interrupt status in the critical nesting +variable. Note this is not saved as part of the task context as context +switches can only occur when uxCriticalNesting is zero. */ +#define portCRITICAL_NESTING_INIT_VALUE 0xaaaaaaaa +static UBaseType_t uxCriticalNesting = portCRITICAL_NESTING_INIT_VALUE; + +/* + * Setup the timer to generate the tick interrupts. + */ +static void prvSetupTimerInterrupt( void ) PRIVILEGED_FUNCTION; + +/* + * Standard FreeRTOS exception handlers. + */ +void xPortPendSVHandler( void ) __attribute__ (( naked )) PRIVILEGED_FUNCTION; +void xPortSysTickHandler( void ) __attribute__ ((optimize("3"))) PRIVILEGED_FUNCTION; +void vPortSVCHandler( void ) __attribute__ (( naked )) PRIVILEGED_FUNCTION; + +/* + * Starts the scheduler by restoring the context of the first task to run. + */ +static void prvRestoreContextOfFirstTask( void ) __attribute__(( naked )) PRIVILEGED_FUNCTION; + +/* + * C portion of the SVC handler. The SVC handler is split between an asm entry + * and a C wrapper for simplicity of coding and maintenance. + */ +static void prvSVCHandler( uint32_t *pulRegisters ) __attribute__(( noinline )) PRIVILEGED_FUNCTION; + +/* + * Function to enable the VFP. + */ +static void vPortEnableVFP( void ) __attribute__ (( naked )); + +/* Used by gdb client (openocd) to determine how registers are stacked */ +uint8_t uxFreeRTOSRegisterStackingVersion = 2; + +/*-----------------------------------------------------------*/ + +/* + * See header file for description. + */ +StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) +{ + /* Simulate the stack frame as it would be created by a context switch + interrupt. */ + pxTopOfStack--; /* Offset added to account for the way the MCU uses the stack on entry/exit of interrupts. */ + *pxTopOfStack = portINITIAL_XPSR; /* xPSR */ + pxTopOfStack--; + *pxTopOfStack = ( StackType_t ) pxCode; /* PC */ + pxTopOfStack--; + *pxTopOfStack = 0; /* LR */ + pxTopOfStack -= 5; /* R12, R3, R2 and R1. */ + *pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */ + + /* A save method is being used that requires each task to maintain its own exec return value. */ + pxTopOfStack--; + *pxTopOfStack = portINITIAL_EXEC_RETURN; + + pxTopOfStack -= 9; /* R11, R10, R9, R8, R7, R6, R5 and R4. */ + + *pxTopOfStack = portINITIAL_CONTROL_IF_PRIVILEGED; + + return pxTopOfStack; +} +/*-----------------------------------------------------------*/ + +void vPortSVCHandler( void ) +{ + /* Assumes psp was in use. */ + __asm volatile + ( + #ifndef USE_PROCESS_STACK /* Code should not be required if a main() is using the process stack. */ + " tst lr, #4 \n" + " ite eq \n" + " mrseq r0, msp \n" + " mrsne r0, psp \n" + #else + " mrs r0, psp \n" + #endif + " b %0 \n" + ::"i"(prvSVCHandler):"r0" + ); +} +/*-----------------------------------------------------------*/ + +extern bool xApplicationIsAllowedToRaisePrivilege( uint32_t caller_pc ); +extern void vSetupSyscallRegisters( uint32_t orig_sp, uint32_t *lr_ptr ); + +static uintptr_t prvCalculateOriginalSP( uint32_t *exception_sp ) +{ + /* This calculation assumes floating point stacking is disabled + * on exception entry */ + + /* The exception frame is laid out as follows: + * {aligner}, xPSR, PC, LR, R12, r3, r2, r1, r0: 0x20 or 0x24 bytes */ + uintptr_t original_sp = (uintptr_t)exception_sp + 0x20; + + /* Determine if the aligner exists: */ + if (exception_sp[portOFFSET_TO_PSR] & portPSR_STACK_PADDING_MASK) { + original_sp += 4; + } + + return original_sp; +} + +static void prvSVCHandler( uint32_t *pulParam ) +{ +uint8_t ucSVCNumber; + + /* The stack contains: r0, r1, r2, r3, r12, r14, the return address and + xPSR. The first argument (r0) is pulParam[ 0 ]. */ + ucSVCNumber = ( ( uint8_t * ) pulParam[ portOFFSET_TO_PC ] )[ -2 ]; + switch( ucSVCNumber ) + { + case portSVC_START_SCHEDULER : *(portNVIC_SYSPRI1) |= portNVIC_SVC_PRI; + prvRestoreContextOfFirstTask(); + break; + + case portSVC_YIELD : *(portNVIC_INT_CTRL) = portNVIC_PENDSVSET; + /* Barriers are normally not required + but do ensure the code is completely + within the specified behaviour for the + architecture. */ + __asm volatile( "dsb" ); + __asm volatile( "isb" ); + + break; + + case portSVC_RAISE_PRIVILEGE : { + uint32_t caller_pc = pulParam[portOFFSET_TO_PC]; + if (xApplicationIsAllowedToRaisePrivilege(caller_pc)) + { + /* Setup necessary information for syscall protection */ + vSetupSyscallRegisters(prvCalculateOriginalSP(pulParam), pulParam + portOFFSET_TO_LR); + + /* Modify the control register to raise the thread mode privilege level */ + __asm volatile + ( + " mrs r1, control \n" /* Obtain current control value. */ + " bic r1, #1 \n" /* Set privilege bit. */ + " msr control, r1 \n" /* Write back new control value. */ + " isb \n" + :::"r1" + ); + } + } + break; + + default : /* Unknown SVC call. */ + break; + } +} +/*-----------------------------------------------------------*/ + +static void prvRestoreContextOfFirstTask( void ) +{ + __asm volatile + ( +#if MICRO_FAMILY_NRF52840 + " ldr r0, =__ISR_VECTOR_TABLE__ \n" /* VTOR does not point where we expect on Nordic-MBR machines! */ +#else + " ldr r0, =0xE000ED08 \n" /* Use the NVIC offset register to locate the stack. */ + " ldr r0, [r0] \n" +#endif + " ldr r0, [r0] \n" + " msr msp, r0 \n" /* Set the msp back to the start of the stack. */ + " ldr r3, pxCurrentTCBConst2 \n" /* Restore the context. */ + " ldr r1, [r3] \n" + " ldr r0, [r1] \n" /* The first item in the TCB is the task top of stack. */ + " add r1, r1, #4 \n" /* Move onto the second item in the TCB... */ + " ldr r2, =0xe000ed98 \n" /* MPU_RNR */ + " mov r4, #4\n" + " str r4, [r2]\n" /* MPU_RNR = 4 */ + " ldr r2, =0xe000ed9c \n" /* Region Base Address register. */ + " ldmia r1!, {r4-r11} \n" /* Read 4 sets of MPU registers. */ + " stmia r2!, {r4-r11} \n" /* Write 4 sets of MPU registers. */ + " ldmia r0!, {r3, r4-r11, r14} \n" /* Pop the registers that are not automatically saved on exception entry. */ + " msr control, r3 \n" + " msr psp, r0 \n" /* Restore the task stack pointer. */ + " mov r0, #0 \n" + " msr basepri, r0 \n" + " isb \n" + " bx r14 \n" + " \n" + " .align 2 \n" + "pxCurrentTCBConst2: .word pxCurrentTCB \n" + " .ltorg \n" + ); +} +/*-----------------------------------------------------------*/ + +/* + * See header file for description. + */ +BaseType_t xPortStartScheduler( void ) +{ + /* configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to 0. See + http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html */ + configASSERT( ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) ); + + /* Make PendSV and SysTick the same priority as the kernel. */ + *(portNVIC_SYSPRI2) |= portNVIC_PENDSV_PRI; + *(portNVIC_SYSPRI2) |= portNVIC_SYSTICK_PRI; + + /* Start the timer that generates the tick ISR. Interrupts are disabled + here already. */ + prvSetupTimerInterrupt(); + + /* Initialise the critical nesting count ready for the first task. */ + uxCriticalNesting = 0; + + /* Ensure the VFP is enabled - it should be anyway. */ + vPortEnableVFP(); + + /* Lazy save always. */ + *( portFPCCR ) |= portASPEN_AND_LSPEN_BITS; + + /* Start the first task. */ + __asm volatile( " dsb \n" + " svc %0 \n" + " isb \n" + :: "i" (portSVC_START_SCHEDULER) ); + + /* Should not get here! */ + return 0; +} +/*-----------------------------------------------------------*/ + +void vPortEndScheduler( void ) +{ + /* Not implemented in ports where there is nothing to return to. + Artificially force an assert. */ + configASSERT( uxCriticalNesting == 1000UL ); +} +/*-----------------------------------------------------------*/ + +void vPortEnterCritical( void ) +{ + portDISABLE_INTERRUPTS(); + uxCriticalNesting++; +} +/*-----------------------------------------------------------*/ + +void vPortExitCritical( void ) +{ + configASSERT( uxCriticalNesting ); + uxCriticalNesting--; + if( uxCriticalNesting == 0 ) + { + portENABLE_INTERRUPTS(); + } +} +/*-----------------------------------------------------------*/ + +bool vPortInCritical( void ) +{ + return (uxCriticalNesting > 0 && uxCriticalNesting != portCRITICAL_NESTING_INIT_VALUE); +} +/*-----------------------------------------------------------*/ + +void xPortPendSVHandler( void ) +{ + /* This is a naked function. */ + + __asm volatile + ( + " mrs r0, psp \n" + " isb \n" + " \n" + " ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */ + " ldr r2, [r3] \n" + " \n" +// Using real FPU +#if defined(__VFP_FP__) && !defined(__SOFTFP__) + " tst r14, #0x10 \n" /* Is the task using the FPU context? If so, push high vfp registers. */ + " it eq \n" + " vstmdbeq r0!, {s16-s31} \n" +#endif + " \n" + " mrs r1, control \n" + " stmdb r0!, {r1, r4-r11, r14} \n" /* Save the remaining registers. */ + " str r0, [r2] \n" /* Save the new top of stack into the first member of the TCB. */ + " \n" + " stmdb sp!, {r3, r14} \n" + " mov r0, %0 \n" + " msr basepri, r0 \n" + " dsb \n" + " isb \n" + " bl vTaskSwitchContext \n" + " mov r0, #0 \n" + " msr basepri, r0 \n" + " ldmia sp!, {r3, r14} \n" + " \n" /* Restore the context. */ + " ldr r1, [r3] \n" /* r1 is a pointer to the TCB struct. */ + " ldr r0, [r1] \n" /* The first item in the TCB is the task top of stack. */ + " add r1, r1, #4 \n" /* Move onto the second item in the TCB... */ + " \n" + " ldr r2, =0xe000ed90 \n" + " ldr r14, [r2, #0x4]\n" /* save MPU_CTRL */ + " mov r4, #0\n" + " str r4, [r2, #0x4]\n" /* turn off the MPU while loading new registers, since this is necessarily non-atomic */ + " mov r4, #4\n" + " str r4, [r2, #0x8]\n" /* MPU_RNR = 4 */ + " ldr r2, =0xe000ed9c \n" /* Region Base Address register. */ + " ldmia r1!, {r4-r11} \n" /* Read 4 sets of MPU registers. */ + " stmia r2!, {r4-r11} \n" /* Write 4 sets of MPU registers. */ + " dsb sy\n" + " ldr r2, =0xe000ed90 \n" + " str r14, [r2, #0x4]\n" /* put back MPU_CTRL */ + " \n" + " ldmia r0!, {r3, r4-r11, r14} \n" /* Pop the registers that are not automatically saved on exception entry. */ + " \n" + " msr control, r3 \n" + " \n" +// Using real FPU +#if defined(__VFP_FP__) && !defined(__SOFTFP__) + " tst r14, #0x10 \n" /* Is the task using the FPU context? If so, pop the high vfp registers too. */ + " it eq \n" + " vldmiaeq r0!, {s16-s31} \n" +#endif + " \n" + " msr psp, r0 \n" + " isb \n" + " bx r14 \n" + " \n" + " .align 2 \n" + "pxCurrentTCBConst: .word pxCurrentTCB \n" + " .ltorg \n" + ::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY) + ); +} +/*-----------------------------------------------------------*/ + +void xPortSysTickHandler( void ) +{ +uint32_t ulDummy; + + ulDummy = portSET_INTERRUPT_MASK_FROM_ISR(); + { + /* Increment the RTOS tick. */ + if( xTaskIncrementTick() != pdFALSE ) + { + /* Pend a context switch. */ + portYIELD_WITHIN_API(); + } +#ifdef TARGET_QEMU + /* When running under emulation, it might be necessary to correct ticks in case we + fell behind and missed some tick interrupts */ + if( vPortCorrectTicks() ) + { + /* Pend a context switch. */ + portYIELD_WITHIN_API(); + } +#endif + } + portCLEAR_INTERRUPT_MASK_FROM_ISR( ulDummy ); +} +/*-----------------------------------------------------------*/ + +/* + * Setup the systick timer to generate the tick interrupts at the required + * frequency. + */ +static void prvSetupTimerInterrupt( void ) +{ + /* Configure SysTick to interrupt at the requested rate. */ + *(portNVIC_SYSTICK_LOAD) = ( configCPU_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL; + *(portNVIC_SYSTICK_CTRL) = portNVIC_SYSTICK_CLK | portNVIC_SYSTICK_INT | portNVIC_SYSTICK_ENABLE; +} +/*-----------------------------------------------------------*/ + +/* This is a naked function. */ +static void vPortEnableVFP( void ) +{ + __asm volatile + ( + " ldr.w r0, =0xE000ED88 \n" /* The FPU enable bits are in the CPACR. */ + " ldr r1, [r0] \n" + " \n" + " orr r1, r1, #( 0xf << 20 ) \n" /* Enable CP10 and CP11 coprocessors, then save back. */ + " str r1, [r0] \n" + " bx r14 \n" + " .ltorg \n" + ); +} +/*-----------------------------------------------------------*/ + +void vPortStoreTaskMPUSettings( xMPU_SETTINGS *xMPUSettings, const struct xMEMORY_REGION * const xRegions, StackType_t *pxBottomOfStack, uint16_t usStackDepth ) +{ +unsigned long ul; +void *pvBaseAddress; +uint32_t ulParameters; + + for( ul = 0; ul < portNUM_CONFIGURABLE_REGIONS; ul++ ) + { + if (xRegions != NULL) + { + pvBaseAddress = xRegions[ ul ].pvBaseAddress; + ulParameters = xRegions[ ul ].ulParameters; + } + else + { + pvBaseAddress = 0; + ulParameters = 0; + } + + /* Set the configuration MPU regions. We're assuming that when the caller created they task, they setup the + * xRegions memory of the TaskParameters_t such that the ulParameters of each region contains what should be + * written into the MPU_RLAR register, and the base address is the RBAR for the region */ + xMPUSettings->xRegion[ ul ].ulRegionBaseAddress = (uintptr_t)pvBaseAddress; + xMPUSettings->xRegion[ ul ].ulRegionAttribute = ulParameters; + } +} + +/*-----------------------------------------------------------*/ +void portSetupTCB(void) { +#if __DCACHE_PRESENT + SCB_CleanDCache(); +#endif +#if __ICACHE_PRESENT + SCB_InvalidateICache(); +#endif +} + +/*-----------------------------------------------------------*/ +// s16-s31 are stacked by the xPortPendSVHandler() (not the CPU) +#define portNUM_EXTRA_STACKED_FLOATING_POINT_REGS (16) + +// s0-s15, fpscr, reserved are stacked by the CPU on exception entry +#define portNUM_BASIC_STACKED_FLOATING_POINT_REGS (18) + +// if bit 4 is 0 it indicates floating point is in use +#define FLOATING_POINT_ACTIVE(exc_return) ((exc_return & 0x10) == 0) + +uintptr_t ulPortGetStackedPC( StackType_t* pxTopOfStack ) +{ + int offset = portTASK_REG_INDEX_PC; + + if (FLOATING_POINT_ACTIVE(pxTopOfStack[portTASK_REG_EXC_RETURN])) { + offset += portNUM_EXTRA_STACKED_FLOATING_POINT_REGS; + } + return pxTopOfStack[offset]; +} +/*-----------------------------------------------------------*/ + +uintptr_t ulPortGetStackedLR( StackType_t* pxTopOfStack ) +{ + int offset = portTASK_REG_INDEX_LR; + + if (FLOATING_POINT_ACTIVE(pxTopOfStack[portTASK_REG_EXC_RETURN])) { + offset += portNUM_EXTRA_STACKED_FLOATING_POINT_REGS; + } + return pxTopOfStack[offset]; +} +/*-----------------------------------------------------------*/ + +void vPortGetTaskInfo( void *taskHandle, char const *pcTaskName, StackType_t *pxTopOfStack, + xPORT_TASK_INFO *pxTaskInfo) +{ + pxTaskInfo->taskHandle = taskHandle; + pxTaskInfo->pcName = pcTaskName; + + // The contents at the current sp _only_ match the registers the thread was + // using if the thread is _not_ currently running + if (taskHandle == xTaskGetCurrentTaskHandle()) + { + return; + } + + // Get the registers off the saved stack. See xPortPendSVHandler() for how the registers are stacked. + // Registers are stored in task_info in canonical order defined in xCANONICAL_REG: [r0-r12, sp, lr, pc, sr] + pxTopOfStack++; // Skip control register + for (int dstIdx = portCANONICAL_REG_INDEX_R4; dstIdx <=portCANONICAL_REG_INDEX_R11; dstIdx++) + { + pxTaskInfo->registers[dstIdx] = *pxTopOfStack++; + } + uint32_t exc_return = *pxTopOfStack++; + + // The xPortPendSVHandler() method saves these extra FP registers (s16-s31) + if (FLOATING_POINT_ACTIVE(exc_return)) { + pxTopOfStack += portNUM_EXTRA_STACKED_FLOATING_POINT_REGS; + } + + // The basic registers + for (int dstIdx = portCANONICAL_REG_INDEX_R0; dstIdx <=portCANONICAL_REG_INDEX_R3; dstIdx++) + { + pxTaskInfo->registers[dstIdx] = *pxTopOfStack++; + } + pxTaskInfo->registers[portCANONICAL_REG_INDEX_R12] = *pxTopOfStack++; + pxTaskInfo->registers[portCANONICAL_REG_INDEX_LR] = *pxTopOfStack++; + pxTaskInfo->registers[portCANONICAL_REG_INDEX_PC] = *pxTopOfStack++; + pxTaskInfo->registers[portCANONICAL_REG_INDEX_XPSR] = *pxTopOfStack++; + + // When FP is active, the basic FP registers are saved by the CPU before it saves the + // basic registers (r0-r3, r12, lr, pc, xpsr) + if (FLOATING_POINT_ACTIVE(exc_return)) { + pxTopOfStack += portNUM_BASIC_STACKED_FLOATING_POINT_REGS; + } + + pxTaskInfo->registers[portCANONICAL_REG_INDEX_SP] = (unsigned portLONG)pxTopOfStack; +} +/*-----------------------------------------------------------*/ + +extern uint32_t ulPortGetStackedControl( StackType_t* pxTopOfStack ) +{ + return pxTopOfStack[portTASK_REG_INDEX_CONTROL]; +} diff --git a/FreeRTOS/Source/portable/GCC/ARM_CM33/portmacro.h b/FreeRTOS/Source/portable/GCC/ARM_CM33/portmacro.h new file mode 100644 index 0000000000..26ce1a00f2 --- /dev/null +++ b/FreeRTOS/Source/portable/GCC/ARM_CM33/portmacro.h @@ -0,0 +1,297 @@ +/* + FreeRTOS V8.2.1 - Copyright (C) 2015 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation >>!AND MODIFIED BY!<< the FreeRTOS exception. + + *************************************************************************** + >>! NOTE: The modification to the GPL is included to allow you to !<< + >>! distribute a combined work that includes FreeRTOS without being !<< + >>! obliged to provide the source code for proprietary components !<< + >>! outside of the FreeRTOS kernel. !<< + *************************************************************************** + + FreeRTOS 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. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + + +#ifndef PORTMACRO_H +#define PORTMACRO_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/*----------------------------------------------------------- + * Port specific definitions. + * + * The settings in this file configure FreeRTOS correctly for the + * given hardware and compiler. + * + * These settings should not be altered. + *----------------------------------------------------------- + */ + +/* Type definitions. */ +#define portCHAR char +#define portFLOAT float +#define portDOUBLE double +#define portLONG long +#define portSHORT short +#define portSTACK_TYPE uint32_t +#define portBASE_TYPE long + +typedef portSTACK_TYPE StackType_t; +typedef long BaseType_t; +typedef unsigned long UBaseType_t; + +#if( configUSE_16_BIT_TICKS == 1 ) + typedef uint16_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffff +#else + typedef uint32_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffffffffUL + + /* 32-bit tick type on a 32-bit architecture, so reads of the tick count do + not need to be guarded with a critical section. */ + #define portTICK_TYPE_IS_ATOMIC 1 +#endif +/*-----------------------------------------------------------*/ + +#define portMPU_REGION_CACHEABLE_BUFFERABLE ( 0x07UL << 16UL ) + +#define portFIRST_CONFIGURABLE_REGION ( 4UL ) +#define portLAST_CONFIGURABLE_REGION ( 7UL ) +#define portNUM_CONFIGURABLE_REGIONS ( ( portLAST_CONFIGURABLE_REGION - portFIRST_CONFIGURABLE_REGION ) + 1 ) + +#define portSWITCH_TO_USER_MODE() __asm volatile ( " mrs r0, control \n orr r0, #1 \n msr control, r0 \n isb " :::"r0" ) + +#define portUSING_MPU 1 + +typedef struct MPU_REGION_REGISTERS +{ + unsigned portLONG ulRegionBaseAddress; + unsigned portLONG ulRegionAttribute; +} xMPU_REGION_REGISTERS; + +/* Plus 1 to create space for the stack region. */ +typedef struct MPU_SETTINGS +{ + xMPU_REGION_REGISTERS xRegion[ portNUM_CONFIGURABLE_REGIONS ]; +} xMPU_SETTINGS; + +/* Architecture specifics. */ +#define portSTACK_GROWTH ( -1 ) +#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ ) +#define portBYTE_ALIGNMENT 8 +/*-----------------------------------------------------------*/ + +/* SVC numbers for various services. */ +#define portSVC_START_SCHEDULER 0 +#define portSVC_YIELD 1 +#define portSVC_RAISE_PRIVILEGE 2 + +/* Scheduler utilities. */ + +#define portYIELD() __asm volatile ( " SVC %0 \n" :: "i" (portSVC_YIELD) ) +#define portYIELD_WITHIN_API() do { \ + *(portNVIC_INT_CTRL) = portNVIC_PENDSVSET; \ + __asm volatile ( "dsb \n"); \ +} while (0) + +#define portNVIC_INT_CTRL ( ( volatile uint32_t *) 0xe000ed04 ) +#define portNVIC_PENDSVSET 0x10000000 +#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired ) portYIELD_WITHIN_API() +#define portYIELD_FROM_ISR( x ) portEND_SWITCHING_ISR( x ) +/*-----------------------------------------------------------*/ + + +/* Critical section management. */ + +/* + * Set basepri to portMAX_SYSCALL_INTERRUPT_PRIORITY without effecting other + * registers. r0 is clobbered. + */ +#define portSET_INTERRUPT_MASK() \ + __asm volatile \ + ( \ + " mov r0, %0 \n" \ + " msr basepri, r0 \n" \ + " isb\n" \ + ::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY):"r0" \ + ) + +/* + * Set basepri back to 0 without effective other registers. + * r0 is clobbered. FAQ: Setting BASEPRI to 0 is not a bug. Please see + * http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html before disagreeing. + */ +#define portCLEAR_INTERRUPT_MASK() \ + __asm volatile \ + ( \ + " mov r0, #0 \n" \ + " msr basepri, r0 \n" \ + " isb\n" \ + :::"r0" \ + ) + +/* FAQ: Setting BASEPRI to 0 is not a bug. Please see +http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html before disagreeing. */ +#define portSET_INTERRUPT_MASK_FROM_ISR() 0;portSET_INTERRUPT_MASK() +#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) portCLEAR_INTERRUPT_MASK();(void)x + + +extern void vPortEnterCritical( void ); +extern void vPortExitCritical( void ); +extern bool vPortInCritical( void ); + +#define portDISABLE_INTERRUPTS() portSET_INTERRUPT_MASK() +#define portENABLE_INTERRUPTS() portCLEAR_INTERRUPT_MASK() +#define portENTER_CRITICAL() vPortEnterCritical() +#define portEXIT_CRITICAL() vPortExitCritical() +#define portIN_CRITICAL() vPortInCritical() +/*-----------------------------------------------------------*/ + +/* Task function macros as described on the FreeRTOS.org WEB site. */ +#define portTASK_FUNCTION_PROTO( vFunction, pvParameters ) void vFunction( void *pvParameters ) +#define portTASK_FUNCTION( vFunction, pvParameters ) void vFunction( void *pvParameters ) + +/* TCB setup */ +extern void portSetupTCB(void); +#define portSETUP_TCB(pxTCB) portSetupTCB() + +/* Tickless idle/low power functionality. */ +#ifndef portSUPPRESS_TICKS_AND_SLEEP + extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ); + #define portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ) vPortSuppressTicksAndSleep( xExpectedIdleTime ) +#endif + +extern uintptr_t ulPortGetStackedPC( StackType_t *pxTopOfStack ); +#define portGET_STACKED_PC( pxTopOfStack ) ulPortGetStackedPC( pxTopOfStack ) + +extern uintptr_t ulPortGetStackedLR( StackType_t *pxTopOfStack ); +#define portGET_STACKED_LR( pxTopOfStack ) ulPortGetStackedLR( pxTopOfStack ) + +// Call from the tick handler to bump up the tick count in case the system +// fell behind and missed some tick interrupts (i.e. while running in an emulator). +extern bool vPortCorrectTicks(void); + +//! The indexes of the registers as stored on a task's stack by FreeRTOS +typedef enum { + portTASK_REG_INDEX_CONTROL = 0, + portTASK_REG_INDEX_R4, + portTASK_REG_INDEX_R5, + portTASK_REG_INDEX_R6, + portTASK_REG_INDEX_R7, + portTASK_REG_INDEX_R8, + portTASK_REG_INDEX_R9, + portTASK_REG_INDEX_R10, + portTASK_REG_INDEX_R11, + portTASK_REG_EXC_RETURN, + portTASK_REG_INDEX_R0, + portTASK_REG_INDEX_R1, + portTASK_REG_INDEX_R2, + portTASK_REG_INDEX_R3, + portTASK_REG_INDEX_R12, + portTASK_REG_INDEX_LR, + portTASK_REG_INDEX_PC, + portTASK_REG_INDEX_XPSR, +} xTASK_REG; + +//! The indexes of the registers when stored in canonical form as stored in xPORT_TASK_INFO.registers +typedef enum { + portCANONICAL_REG_INDEX_R0 = 0, + portCANONICAL_REG_INDEX_R1, + portCANONICAL_REG_INDEX_R2, + portCANONICAL_REG_INDEX_R3, + portCANONICAL_REG_INDEX_R4, + portCANONICAL_REG_INDEX_R5, + portCANONICAL_REG_INDEX_R6, + portCANONICAL_REG_INDEX_R7, + portCANONICAL_REG_INDEX_R8, + portCANONICAL_REG_INDEX_R9, + portCANONICAL_REG_INDEX_R10, + portCANONICAL_REG_INDEX_R11, + portCANONICAL_REG_INDEX_R12, + portCANONICAL_REG_INDEX_SP, + portCANONICAL_REG_INDEX_LR, + portCANONICAL_REG_INDEX_PC, + portCANONICAL_REG_INDEX_XPSR, + portCANONICAL_REG_COUNT, +} xCANONICAL_REG; + +typedef struct PORT_TASK_INFO +{ + portCHAR const *pcName; + void *taskHandle; // Can be compared to xTaskGetCurrentTaskHandle() + unsigned portLONG registers[portCANONICAL_REG_COUNT]; // task registers +} xPORT_TASK_INFO; + +extern void vPortGetTaskInfo( void *xTaskHandle, char const * pcTaskName, StackType_t *pxTopOfStack, + xPORT_TASK_INFO *pxTaskInfo ); + +extern uint32_t ulPortGetStackedControl( StackType_t* pxTopOfStack ); +#define portGET_STACKED_CONTROL( pxTopOfStack ) ulPortGetStackedControl( pxTopOfStack ) + +#ifdef __cplusplus +} +#endif + +#endif /* PORTMACRO_H */ +