| // Copyright (c) 2019, XMOS Ltd, All rights reserved |
| |
| /* Scheduler includes. */ |
| #include "FreeRTOS.h" |
| #include "task.h" |
| #include <string.h> |
| #include <xs1.h> |
| #include <xcore/hwtimer.h> |
| #include <xcore/triggerable.h> |
| |
| static hwtimer_t xKernelTimer; |
| |
| uint32_t ulPortYieldRequired[ portMAX_CORE_COUNT ] = { pdFALSE }; |
| |
| /*-----------------------------------------------------------*/ |
| |
| void vIntercoreInterruptISR( void ) |
| { |
| int xCoreID; |
| |
| // debug_printf( "In KCALL: %u\n", ulData ); |
| xCoreID = rtos_core_id_get(); |
| ulPortYieldRequired[ xCoreID ] = pdTRUE; |
| } |
| /*-----------------------------------------------------------*/ |
| |
| DEFINE_RTOS_INTERRUPT_CALLBACK( pxKernelTimerISR, pvData ) |
| { |
| uint32_t ulLastTrigger; |
| uint32_t ulNow; |
| int xCoreID; |
| UBaseType_t uxSavedInterruptStatus; |
| |
| xCoreID = 0; |
| |
| configASSERT( xCoreID == rtos_core_id_get() ); |
| |
| /* Need the next interrupt to be scheduled relative to |
| * the current trigger time, rather than the current |
| * time. */ |
| ulLastTrigger = hwtimer_get_trigger_time( xKernelTimer ); |
| |
| /* Check to see if the ISR is late. If it is, we don't |
| * want to schedule the next interrupt to be in the past. */ |
| ulNow = hwtimer_get_time( xKernelTimer ); |
| if( ulNow - ulLastTrigger >= configCPU_CLOCK_HZ / configTICK_RATE_HZ ) |
| { |
| ulLastTrigger = ulNow; |
| } |
| |
| ulLastTrigger += configCPU_CLOCK_HZ / configTICK_RATE_HZ; |
| hwtimer_change_trigger_time( xKernelTimer, ulLastTrigger ); |
| |
| #if configUPDATE_RTOS_TIME_FROM_TICK_ISR == 1 |
| rtos_time_increment( RTOS_TICK_PERIOD( configTICK_RATE_HZ ) ); |
| #endif |
| |
| uxSavedInterruptStatus = taskENTER_CRITICAL_FROM_ISR(); |
| if( xTaskIncrementTick() != pdFALSE ) |
| { |
| ulPortYieldRequired[ xCoreID ] = pdTRUE; |
| } |
| taskEXIT_CRITICAL_FROM_ISR( uxSavedInterruptStatus ); |
| } |
| /*-----------------------------------------------------------*/ |
| |
| void vPortYieldOtherCore( int xOtherCoreID ) |
| { |
| int xCoreID; |
| |
| /* |
| * This function must be called from within a critical section. |
| */ |
| |
| xCoreID = rtos_core_id_get(); |
| |
| // debug_printf("%d->%d\n", xCoreID, xOtherCoreID); |
| |
| // debug_printf("Yield core %d from %d\n", xOtherCoreID, xCoreID ); |
| |
| rtos_irq( xOtherCoreID, xCoreID ); |
| } |
| /*-----------------------------------------------------------*/ |
| |
| static int prvCoreInit( void ) |
| { |
| int xCoreID; |
| |
| xCoreID = rtos_core_register(); |
| debug_printf( "Logical Core %d initializing as FreeRTOS Core %d\n", get_logical_core_id(), xCoreID ); |
| |
| asm volatile ( |
| "ldap r11, kexcept\n\t" |
| "set kep, r11\n\t" |
| : |
| : |
| : "r11" |
| ); |
| |
| rtos_irq_enable( configNUMBER_OF_CORES ); |
| |
| /* |
| * All threads wait here until all have enabled IRQs |
| */ |
| while( rtos_irq_ready() == pdFALSE ); |
| |
| if( xCoreID == 0 ) |
| { |
| uint32_t ulNow; |
| ulNow = hwtimer_get_time( xKernelTimer ); |
| // debug_printf( "The time is now (%u)\n", ulNow ); |
| |
| ulNow += configCPU_CLOCK_HZ / configTICK_RATE_HZ; |
| |
| triggerable_setup_interrupt_callback( xKernelTimer, NULL, RTOS_INTERRUPT_CALLBACK( pxKernelTimerISR ) ); |
| hwtimer_set_trigger_time( xKernelTimer, ulNow ); |
| triggerable_enable_trigger( xKernelTimer ); |
| } |
| |
| return xCoreID; |
| } |
| /*-----------------------------------------------------------*/ |
| |
| DEFINE_RTOS_KERNEL_ENTRY( void, vPortStartSchedulerOnCore, void ) |
| { |
| int xCoreID; |
| |
| xCoreID = prvCoreInit(); |
| |
| #if( configUSE_CORE_INIT_HOOK == 1 ) |
| { |
| extern void vApplicationCoreInitHook( BaseType_t xCoreID ); |
| |
| vApplicationCoreInitHook( xCoreID ); |
| } |
| #endif |
| |
| debug_printf( "FreeRTOS Core %d initialized\n", xCoreID ); |
| |
| /* |
| * Restore the context of the first thread |
| * to run and jump into it. |
| */ |
| asm volatile ( |
| "mov r6, %0\n\t" /* R6 must be the FreeRTOS core ID*/ |
| "ldaw r5, dp[pxCurrentTCBs]\n\t" /* R5 must be the TCB list which is indexed by R6 */ |
| "bu _freertos_restore_ctx\n\t" |
| : /* no outputs */ |
| : "r"(xCoreID) |
| : "r5", "r6" |
| ); |
| } |
| /*-----------------------------------------------------------*/ |
| |
| /*-----------------------------------------------------------*/ |
| /* Public functions required by all ports below: */ |
| /*-----------------------------------------------------------*/ |
| |
| /* |
| * See header file for description. |
| */ |
| StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) |
| { |
| //debug_printf( "Top of stack was %p for task %p\n", pxTopOfStack, pxCode ); |
| /* |
| * Grow the thread's stack by portTHREAD_CONTEXT_STACK_GROWTH |
| * so we can push the context onto it. |
| */ |
| pxTopOfStack -= portTHREAD_CONTEXT_STACK_GROWTH; |
| |
| uint32_t dp; |
| uint32_t cp; |
| |
| /* |
| * We need to get the current CP and DP pointers. |
| */ |
| asm volatile ( |
| "ldaw r11, cp[0]\n\t" /* get CP into R11 */ |
| "mov %0, r11\n\t" /* get R11 (CP) into cp */ |
| "ldaw r11, dp[0]\n\t" /* get DP into R11 */ |
| "mov %1, r11\n\t" /* get R11 (DP) into dp */ |
| : "=r"(cp), "=r"(dp) /* output 0 is cp, output 1 is dp */ |
| : /* there are no inputs */ |
| : "r11" /* R11 gets clobbered */ |
| ); |
| |
| /* |
| * Push the thread context onto the stack. |
| * Saved PC will point to the new thread's |
| * entry pointer. |
| * Interrupts will default to enabled. |
| * KEDI is also set to enable dual issue mode |
| * upon kernel entry. |
| */ |
| pxTopOfStack[ 1 ] = ( StackType_t ) pxCode; /* SP[1] := SPC */ |
| pxTopOfStack[ 2 ] = XS1_SR_IEBLE_MASK |
| | XS1_SR_KEDI_MASK; /* SP[2] := SSR */ |
| pxTopOfStack[ 3 ] = 0x00000000; /* SP[3] := SED */ |
| pxTopOfStack[ 4 ] = 0x00000000; /* SP[4] := ET */ |
| pxTopOfStack[ 5 ] = dp; /* SP[5] := DP */ |
| pxTopOfStack[ 6 ] = cp; /* SP[6] := CP */ |
| pxTopOfStack[ 7 ] = 0x00000000; /* SP[7] := LR */ |
| pxTopOfStack[ 8 ] = ( StackType_t ) pvParameters; /* SP[8] := R0 */ |
| pxTopOfStack[ 9 ] = 0x01010101; /* SP[9] := R1 */ |
| pxTopOfStack[ 10 ] = 0x02020202; /* SP[10] := R2 */ |
| pxTopOfStack[ 11 ] = 0x03030303; /* SP[11] := R3 */ |
| pxTopOfStack[ 12 ] = 0x04040404; /* SP[12] := R4 */ |
| pxTopOfStack[ 13 ] = 0x05050505; /* SP[13] := R5 */ |
| pxTopOfStack[ 14 ] = 0x06060606; /* SP[14] := R6 */ |
| pxTopOfStack[ 15 ] = 0x07070707; /* SP[15] := R7 */ |
| pxTopOfStack[ 16 ] = 0x08080808; /* SP[16] := R8 */ |
| pxTopOfStack[ 17 ] = 0x09090909; /* SP[17] := R9 */ |
| pxTopOfStack[ 18 ] = 0x10101010; /* SP[18] := R10 */ |
| pxTopOfStack[ 19 ] = 0x11111111; /* SP[19] := R11 */ |
| pxTopOfStack[ 20 ] = 0x00000000; /* SP[20] := vH and vSR */ |
| memset(&pxTopOfStack[21], 0, 32); /* SP[21 - 28] := vR */ |
| memset(&pxTopOfStack[29], 1, 32); /* SP[29 - 36] := vD */ |
| memset(&pxTopOfStack[37], 2, 32); /* SP[37 - 44] := vC */ |
| |
| //debug_printf( "Top of stack is now %p for task %p\n", pxTopOfStack, pxCode ); |
| |
| /* |
| * Returns the new top of the stack |
| */ |
| return pxTopOfStack; |
| } |
| /*-----------------------------------------------------------*/ |
| |
| void vPortStartSMPScheduler( void ); |
| |
| /* |
| * See header file for description. |
| */ |
| BaseType_t xPortStartScheduler( void ) |
| { |
| if( ( configNUMBER_OF_CORES > portMAX_CORE_COUNT ) || ( configNUMBER_OF_CORES <= 0 ) ) |
| { |
| return pdFAIL; |
| } |
| |
| rtos_locks_initialize(); |
| xKernelTimer = hwtimer_alloc(); |
| |
| vPortStartSMPScheduler(); |
| |
| return pdPASS; |
| } |
| /*-----------------------------------------------------------*/ |
| |
| void vPortEndScheduler( void ) |
| { |
| /* Do not implement. */ |
| } |
| /*-----------------------------------------------------------*/ |