/* Copyright (c) 2019, XMOS Ltd, All rights reserved */ /* Scheduler includes. */ #include "FreeRTOS.h" #include "task.h" #include #include #include #include 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. */ } /*-----------------------------------------------------------*/