1 /* Copyright (c) 2019, XMOS Ltd, All rights reserved */
2
3 /* Scheduler includes. */
4 #include "FreeRTOS.h"
5 #include "task.h"
6 #include <string.h>
7 #include <xs1.h>
8 #include <xcore/hwtimer.h>
9 #include <xcore/triggerable.h>
10
11 static hwtimer_t xKernelTimer;
12
13 uint32_t ulPortYieldRequired[ portMAX_CORE_COUNT ] = { pdFALSE };
14
15 /*-----------------------------------------------------------*/
16
vIntercoreInterruptISR(void)17 void vIntercoreInterruptISR( void )
18 {
19 int xCoreID;
20
21 /* debug_printf( "In KCALL: %u\n", ulData ); */
22 xCoreID = rtos_core_id_get();
23 ulPortYieldRequired[ xCoreID ] = pdTRUE;
24 }
25 /*-----------------------------------------------------------*/
26
DEFINE_RTOS_INTERRUPT_CALLBACK(pxKernelTimerISR,pvData)27 DEFINE_RTOS_INTERRUPT_CALLBACK( pxKernelTimerISR, pvData )
28 {
29 uint32_t ulLastTrigger;
30 uint32_t ulNow;
31 int xCoreID;
32 UBaseType_t uxSavedInterruptStatus;
33
34 xCoreID = 0;
35
36 configASSERT( xCoreID == rtos_core_id_get() );
37
38 /* Need the next interrupt to be scheduled relative to
39 * the current trigger time, rather than the current
40 * time. */
41 ulLastTrigger = hwtimer_get_trigger_time( xKernelTimer );
42
43 /* Check to see if the ISR is late. If it is, we don't
44 * want to schedule the next interrupt to be in the past. */
45 ulNow = hwtimer_get_time( xKernelTimer );
46
47 if( ulNow - ulLastTrigger >= configCPU_CLOCK_HZ / configTICK_RATE_HZ )
48 {
49 ulLastTrigger = ulNow;
50 }
51
52 ulLastTrigger += configCPU_CLOCK_HZ / configTICK_RATE_HZ;
53 hwtimer_change_trigger_time( xKernelTimer, ulLastTrigger );
54
55 #if configUPDATE_RTOS_TIME_FROM_TICK_ISR == 1
56 rtos_time_increment( RTOS_TICK_PERIOD( configTICK_RATE_HZ ) );
57 #endif
58
59 uxSavedInterruptStatus = taskENTER_CRITICAL_FROM_ISR();
60
61 if( xTaskIncrementTick() != pdFALSE )
62 {
63 ulPortYieldRequired[ xCoreID ] = pdTRUE;
64 }
65
66 taskEXIT_CRITICAL_FROM_ISR( uxSavedInterruptStatus );
67 }
68 /*-----------------------------------------------------------*/
69
vPortYieldOtherCore(int xOtherCoreID)70 void vPortYieldOtherCore( int xOtherCoreID )
71 {
72 int xCoreID;
73
74 /*
75 * This function must be called from within a critical section.
76 */
77
78 xCoreID = rtos_core_id_get();
79
80 /* debug_printf("%d->%d\n", xCoreID, xOtherCoreID); */
81
82 /* debug_printf("Yield core %d from %d\n", xOtherCoreID, xCoreID ); */
83
84 rtos_irq( xOtherCoreID, xCoreID );
85 }
86 /*-----------------------------------------------------------*/
87
prvCoreInit(void)88 static int prvCoreInit( void )
89 {
90 int xCoreID;
91
92 xCoreID = rtos_core_register();
93 debug_printf( "Logical Core %d initializing as FreeRTOS Core %d\n", get_logical_core_id(), xCoreID );
94
95 asm volatile (
96 "ldap r11, kexcept\n\t"
97 "set kep, r11\n\t"
98 :
99 :
100 : "r11"
101 );
102
103 rtos_irq_enable( configNUMBER_OF_CORES );
104
105 /*
106 * All threads wait here until all have enabled IRQs
107 */
108 while( rtos_irq_ready() == pdFALSE )
109 {
110 }
111
112 if( xCoreID == 0 )
113 {
114 uint32_t ulNow;
115 ulNow = hwtimer_get_time( xKernelTimer );
116 /* debug_printf( "The time is now (%u)\n", ulNow ); */
117
118 ulNow += configCPU_CLOCK_HZ / configTICK_RATE_HZ;
119
120 triggerable_setup_interrupt_callback( xKernelTimer, NULL, RTOS_INTERRUPT_CALLBACK( pxKernelTimerISR ) );
121 hwtimer_set_trigger_time( xKernelTimer, ulNow );
122 triggerable_enable_trigger( xKernelTimer );
123 }
124
125 return xCoreID;
126 }
127 /*-----------------------------------------------------------*/
128
DEFINE_RTOS_KERNEL_ENTRY(void,vPortStartSchedulerOnCore,void)129 DEFINE_RTOS_KERNEL_ENTRY( void, vPortStartSchedulerOnCore, void )
130 {
131 int xCoreID;
132
133 xCoreID = prvCoreInit();
134
135 #if ( configUSE_CORE_INIT_HOOK == 1 )
136 {
137 extern void vApplicationCoreInitHook( BaseType_t xCoreID );
138
139 vApplicationCoreInitHook( xCoreID );
140 }
141 #endif
142
143 debug_printf( "FreeRTOS Core %d initialized\n", xCoreID );
144
145 /*
146 * Restore the context of the first thread
147 * to run and jump into it.
148 */
149 asm volatile (
150 "mov r6, %0\n\t" /* R6 must be the FreeRTOS core ID*/
151 "ldaw r5, dp[pxCurrentTCBs]\n\t" /* R5 must be the TCB list which is indexed by R6 */
152 "bu _freertos_restore_ctx\n\t"
153 : /* no outputs */
154 : "r" ( xCoreID )
155 : "r5", "r6"
156 );
157 }
158 /*-----------------------------------------------------------*/
159
160 /*-----------------------------------------------------------*/
161 /* Public functions required by all ports below: */
162 /*-----------------------------------------------------------*/
163
164 /*
165 * See header file for description.
166 */
pxPortInitialiseStack(StackType_t * pxTopOfStack,TaskFunction_t pxCode,void * pvParameters)167 StackType_t * pxPortInitialiseStack( StackType_t * pxTopOfStack,
168 TaskFunction_t pxCode,
169 void * pvParameters )
170 {
171 /*debug_printf( "Top of stack was %p for task %p\n", pxTopOfStack, pxCode ); */
172
173 /*
174 * Grow the thread's stack by portTHREAD_CONTEXT_STACK_GROWTH
175 * so we can push the context onto it.
176 */
177 pxTopOfStack -= portTHREAD_CONTEXT_STACK_GROWTH;
178
179 uint32_t dp;
180 uint32_t cp;
181
182 /*
183 * We need to get the current CP and DP pointers.
184 */
185 asm volatile (
186 "ldaw r11, cp[0]\n\t" /* get CP into R11 */
187 "mov %0, r11\n\t" /* get R11 (CP) into cp */
188 "ldaw r11, dp[0]\n\t" /* get DP into R11 */
189 "mov %1, r11\n\t" /* get R11 (DP) into dp */
190 : "=r" ( cp ), "=r" ( dp ) /* output 0 is cp, output 1 is dp */
191 : /* there are no inputs */
192 : "r11" /* R11 gets clobbered */
193 );
194
195 /*
196 * Push the thread context onto the stack.
197 * Saved PC will point to the new thread's
198 * entry pointer.
199 * Interrupts will default to enabled.
200 * KEDI is also set to enable dual issue mode
201 * upon kernel entry.
202 */
203 pxTopOfStack[ 1 ] = ( StackType_t ) pxCode; /* SP[1] := SPC */
204 pxTopOfStack[ 2 ] = XS1_SR_IEBLE_MASK
205 | XS1_SR_KEDI_MASK; /* SP[2] := SSR */
206 pxTopOfStack[ 3 ] = 0x00000000; /* SP[3] := SED */
207 pxTopOfStack[ 4 ] = 0x00000000; /* SP[4] := ET */
208 pxTopOfStack[ 5 ] = dp; /* SP[5] := DP */
209 pxTopOfStack[ 6 ] = cp; /* SP[6] := CP */
210 pxTopOfStack[ 7 ] = 0x00000000; /* SP[7] := LR */
211 pxTopOfStack[ 8 ] = ( StackType_t ) pvParameters; /* SP[8] := R0 */
212 pxTopOfStack[ 9 ] = 0x01010101; /* SP[9] := R1 */
213 pxTopOfStack[ 10 ] = 0x02020202; /* SP[10] := R2 */
214 pxTopOfStack[ 11 ] = 0x03030303; /* SP[11] := R3 */
215 pxTopOfStack[ 12 ] = 0x04040404; /* SP[12] := R4 */
216 pxTopOfStack[ 13 ] = 0x05050505; /* SP[13] := R5 */
217 pxTopOfStack[ 14 ] = 0x06060606; /* SP[14] := R6 */
218 pxTopOfStack[ 15 ] = 0x07070707; /* SP[15] := R7 */
219 pxTopOfStack[ 16 ] = 0x08080808; /* SP[16] := R8 */
220 pxTopOfStack[ 17 ] = 0x09090909; /* SP[17] := R9 */
221 pxTopOfStack[ 18 ] = 0x10101010; /* SP[18] := R10 */
222 pxTopOfStack[ 19 ] = 0x11111111; /* SP[19] := R11 */
223 pxTopOfStack[ 20 ] = 0x00000000; /* SP[20] := vH and vSR */
224 memset( &pxTopOfStack[ 21 ], 0, 32 ); /* SP[21 - 28] := vR */
225 memset( &pxTopOfStack[ 29 ], 1, 32 ); /* SP[29 - 36] := vD */
226 memset( &pxTopOfStack[ 37 ], 2, 32 ); /* SP[37 - 44] := vC */
227
228 /*debug_printf( "Top of stack is now %p for task %p\n", pxTopOfStack, pxCode ); */
229
230 /*
231 * Returns the new top of the stack
232 */
233 return pxTopOfStack;
234 }
235 /*-----------------------------------------------------------*/
236
237 void vPortStartSMPScheduler( void );
238
239 /*
240 * See header file for description.
241 */
xPortStartScheduler(void)242 BaseType_t xPortStartScheduler( void )
243 {
244 if( ( configNUMBER_OF_CORES > portMAX_CORE_COUNT ) || ( configNUMBER_OF_CORES <= 0 ) )
245 {
246 return pdFAIL;
247 }
248
249 rtos_locks_initialize();
250 xKernelTimer = hwtimer_alloc();
251
252 vPortStartSMPScheduler();
253
254 return pdPASS;
255 }
256 /*-----------------------------------------------------------*/
257
vPortEndScheduler(void)258 void vPortEndScheduler( void )
259 {
260 /* Do not implement. */
261 }
262 /*-----------------------------------------------------------*/
263
