1/*
2 * FreeRTOS Kernel V10.2.0
3 * Copyright (C) 2019 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy of
6 * this software and associated documentation files (the "Software"), to deal in
7 * the Software without restriction, including without limitation the rights to
8 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
9 * the Software, and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in all
13 * copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
17 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
18 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
19 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
20 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21 *
22 * http://www.FreeRTOS.org
23 * http://aws.amazon.com/freertos
24 *
25 * 1 tab == 4 spaces!
26 */
27
28    .text
29
30    /* Variables and functions. */
31    .extern ullMaxAPIPriorityMask
32    .extern pxCurrentTCB
33    .extern _freertos_vector_table
34
35    .globl FreeRTOS_SWI_Handler
36    .globl FreeRTOS_IRQ_Handler
37    .globl vPortRestoreTaskContext
38
39; /**********************************************************************/
40
41.macro portSAVE_CONTEXT
42
43    /* Switch to use the EL0 stack pointer. */
44    MSR     SPSEL, #0
45
46    /* Save the entire context. */
47    STP     X0, X1, [SP, #-0x10]!
48    STP     X2, X3, [SP, #-0x10]!
49    STP     X4, X5, [SP, #-0x10]!
50    STP     X6, X7, [SP, #-0x10]!
51    STP     X8, X9, [SP, #-0x10]!
52    STP     X10, X11, [SP, #-0x10]!
53    STP     X12, X13, [SP, #-0x10]!
54    STP     X14, X15, [SP, #-0x10]!
55    STP     X16, X17, [SP, #-0x10]!
56    STP     X18, X19, [SP, #-0x10]!
57    STP     X20, X21, [SP, #-0x10]!
58    STP     X22, X23, [SP, #-0x10]!
59    STP     X24, X25, [SP, #-0x10]!
60    STP     X26, X27, [SP, #-0x10]!
61    STP     X28, X29, [SP, #-0x10]!
62    STP     X30, XZR, [SP, #-0x10]!
63
64    /* Save the SPSR. */
65    MRS     X3, SPSR_EL3
66    MRS     X2, ELR_EL3
67
68    STP     X2, X3, [SP, #-0x10]!
69
70    /* Save the critical section nesting depth. */
71    LDR     X0, ullCriticalNestingConst
72    LDR     X3, [X0]
73
74    /* Save the FPU context indicator. */
75    LDR     X0, ullPortTaskHasFPUContextConst
76    LDR     X2, [X0]
77
78    /* Save the FPU context, if any (32 128-bit registers). */
79    CMP     X2, #0
80    B.EQ    1f
81    STP     Q0, Q1, [SP,#-0x20]!
82    STP     Q2, Q3, [SP,#-0x20]!
83    STP     Q4, Q5, [SP,#-0x20]!
84    STP     Q6, Q7, [SP,#-0x20]!
85    STP     Q8, Q9, [SP,#-0x20]!
86    STP     Q10, Q11, [SP,#-0x20]!
87    STP     Q12, Q13, [SP,#-0x20]!
88    STP     Q14, Q15, [SP,#-0x20]!
89    STP     Q16, Q17, [SP,#-0x20]!
90    STP     Q18, Q19, [SP,#-0x20]!
91    STP     Q20, Q21, [SP,#-0x20]!
92    STP     Q22, Q23, [SP,#-0x20]!
93    STP     Q24, Q25, [SP,#-0x20]!
94    STP     Q26, Q27, [SP,#-0x20]!
95    STP     Q28, Q29, [SP,#-0x20]!
96    STP     Q30, Q31, [SP,#-0x20]!
97
981:
99    /* Store the critical nesting count and FPU context indicator. */
100    STP     X2, X3, [SP, #-0x10]!
101
102    LDR     X0, pxCurrentTCBConst
103    LDR     X1, [X0]
104    MOV     X0, SP   /* Move SP into X0 for saving. */
105    STR     X0, [X1]
106
107    /* Switch to use the ELx stack pointer. */
108    MSR     SPSEL, #1
109
110    .endm
111
112
113.macro portRESTORE_CONTEXT
114
115    /* Switch to use the EL0 stack pointer. */
116    MSR     SPSEL, #0
117
118    /* Set the SP to point to the stack of the task being restored. */
119    LDR     X0, pxCurrentTCBConst
120    LDR     X1, [X0]
121    LDR     X0, [X1]
122    MOV     SP, X0
123
124    LDP     X2, X3, [SP], #0x10  /* Critical nesting and FPU context. */
125
126    /* Set the PMR register to be correct for the current critical nesting
127    depth. */
128    /* X0 holds the address of ullCriticalNesting. */
129    LDR     X0, ullCriticalNestingConst
130    /* Restore the task's critical nesting count. */
131    STR     X3, [X0]
132
133    /* Restore the FPU context indicator. */
134    LDR     X0, ullPortTaskHasFPUContextConst
135    STR     X2, [X0]
136
137    /* Restore the FPU context, if any. */
138    CMP     X2, #0
139    B.EQ    1f
140    LDP     Q30, Q31, [SP], #0x20
141    LDP     Q28, Q29, [SP], #0x20
142    LDP     Q26, Q27, [SP], #0x20
143    LDP     Q24, Q25, [SP], #0x20
144    LDP     Q22, Q23, [SP], #0x20
145    LDP     Q20, Q21, [SP], #0x20
146    LDP     Q18, Q19, [SP], #0x20
147    LDP     Q16, Q17, [SP], #0x20
148    LDP     Q14, Q15, [SP], #0x20
149    LDP     Q12, Q13, [SP], #0x20
150    LDP     Q10, Q11, [SP], #0x20
151    LDP     Q8, Q9, [SP], #0x20
152    LDP     Q6, Q7, [SP], #0x20
153    LDP     Q4, Q5, [SP], #0x20
154    LDP     Q2, Q3, [SP], #0x20
155    LDP     Q0, Q1, [SP], #0x20
1561:
157    LDP     X2, X3, [SP], #0x10  /* SPSR and ELR. */
158
159    /* Restore the SPSR. */
160    MSR     SPSR_EL3, X3
161    /* Restore the ELR. */
162    MSR     ELR_EL3, X2
163
164    LDP     X30, XZR, [SP], #0x10
165    LDP     X28, X29, [SP], #0x10
166    LDP     X26, X27, [SP], #0x10
167    LDP     X24, X25, [SP], #0x10
168    LDP     X22, X23, [SP], #0x10
169    LDP     X20, X21, [SP], #0x10
170    LDP     X18, X19, [SP], #0x10
171    LDP     X16, X17, [SP], #0x10
172    LDP     X14, X15, [SP], #0x10
173    LDP     X12, X13, [SP], #0x10
174    LDP     X10, X11, [SP], #0x10
175    LDP     X8, X9, [SP], #0x10
176    LDP     X6, X7, [SP], #0x10
177    LDP     X4, X5, [SP], #0x10
178    LDP     X2, X3, [SP], #0x10
179    LDP     X0, X1, [SP], #0x10
180
181    /* Switch to use the ELx stack pointer.  _RB_ Might not be required. */
182    MSR     SPSEL, #1
183
184    ERET
185
186    .endm
187
188/******************************************************************************
189 * FreeRTOS_SWI_Handler handler is used to perform a context switch.
190 *****************************************************************************/
191.align 8
192.type FreeRTOS_SWI_Handler, %function
193FreeRTOS_SWI_Handler:
194    /* Save the context of the current task and select a new task to run. */
195    portSAVE_CONTEXT
196    MRS     X0, ESR_EL3
197
198    LSR     X1, X0, #26
199
200    CMP     X1, #0x15   /* 0x15 = SVC instruction. */
201
202    B.NE    FreeRTOS_Abort
203    BL      vTaskSwitchContext
204
205    portRESTORE_CONTEXT
206
207FreeRTOS_Abort:
208    /* Full ESR is in X0, exception class code is in X1. */
209    B       .
210
211/******************************************************************************
212 * vPortRestoreTaskContext is used to start the scheduler.
213 *****************************************************************************/
214.align 8
215.type vPortRestoreTaskContext, %function
216vPortRestoreTaskContext:
217.set freertos_vector_base,  _freertos_vector_table
218
219    /* Install the FreeRTOS interrupt handlers. */
220    LDR     X1, =freertos_vector_base
221    MSR     VBAR_EL3, X1
222    DSB     SY
223    ISB     SY
224
225    /* Start the first task. */
226    portRESTORE_CONTEXT
227
228/******************************************************************************
229 * FreeRTOS_IRQ_Handler handles IRQ entry and exit.
230 *****************************************************************************/
231.align 8
232.type FreeRTOS_IRQ_Handler, %function
233FreeRTOS_IRQ_Handler:
234    /* Save volatile registers. */
235    STP     X0, X1, [SP, #-0x10]!
236    STP     X2, X3, [SP, #-0x10]!
237    STP     X4, X5, [SP, #-0x10]!
238    STP     X6, X7, [SP, #-0x10]!
239    STP     X8, X9, [SP, #-0x10]!
240    STP     X10, X11, [SP, #-0x10]!
241    STP     X12, X13, [SP, #-0x10]!
242    STP     X14, X15, [SP, #-0x10]!
243    STP     X16, X17, [SP, #-0x10]!
244    STP     X18, X19, [SP, #-0x10]!
245    STP     X29, X30, [SP, #-0x10]!
246
247    /* Save the SPSR and ELR. */
248    MRS     X3, SPSR_EL3
249    MRS     X2, ELR_EL3
250    STP     X2, X3, [SP, #-0x10]!
251
252    /* Increment the interrupt nesting counter. */
253    LDR     X5, ullPortInterruptNestingConst
254    LDR     X1, [X5]    /* Old nesting count in X1. */
255    ADD     X6, X1, #1
256    STR     X6, [X5]    /* Address of nesting count variable in X5. */
257
258    /* Maintain the interrupt nesting information across the function call. */
259    STP     X1, X5, [SP, #-0x10]!
260
261#if 0
262    /* Read Cor0 interrupt Source */
263    ldr     x2, ulCORE0_INT_SRCConst
264    ldr     x3, [x2]
265    ldr     w0, [x3] /* set parametor for handler */
266#endif
267
268    /* Call the C handler. */
269    BL vApplicationIRQHandler
270
271    /* Disable interrupts. */
272    MSR     DAIFSET, #1 /* IRQ -> FIQ */
273    DSB     SY
274    ISB     SY
275
276    /* Restore the critical nesting count. */
277    LDP     X1, X5, [SP], #0x10
278    STR     X1, [X5]
279
280    /* Has interrupt nesting unwound? */
281    CMP     X1, #0
282    B.NE    Exit_IRQ_No_Context_Switch
283
284    /* Is a context switch required? */
285    LDR     X0, ullPortYieldRequiredConst
286    LDR     X1, [X0]
287    CMP     X1, #0
288    B.EQ    Exit_IRQ_No_Context_Switch
289
290    /* Reset ullPortYieldRequired to 0. */
291    MOV     X2, #0
292    STR     X2, [X0]
293
294    /* Restore volatile registers. */
295    LDP     X4, X5, [SP], #0x10  /* SPSR and ELR. */
296    MSR     SPSR_EL3, X5
297    MSR     ELR_EL3, X4
298    DSB     SY
299    ISB     SY
300
301    LDP     X29, X30, [SP], #0x10
302    LDP     X18, X19, [SP], #0x10
303    LDP     X16, X17, [SP], #0x10
304    LDP     X14, X15, [SP], #0x10
305    LDP     X12, X13, [SP], #0x10
306    LDP     X10, X11, [SP], #0x10
307    LDP     X8, X9, [SP], #0x10
308    LDP     X6, X7, [SP], #0x10
309    LDP     X4, X5, [SP], #0x10
310    LDP     X2, X3, [SP], #0x10
311    LDP     X0, X1, [SP], #0x10
312
313    /* Save the context of the current task and select a new task to run. */
314    portSAVE_CONTEXT
315    BL vTaskSwitchContext
316    portRESTORE_CONTEXT
317
318Exit_IRQ_No_Context_Switch:
319    /* Restore volatile registers. */
320    LDP     X4, X5, [SP], #0x10  /* SPSR and ELR. */
321    MSR     SPSR_EL3, X5
322    MSR     ELR_EL3, X4
323    DSB     SY
324    ISB     SY
325
326    LDP     X29, X30, [SP], #0x10
327    LDP     X18, X19, [SP], #0x10
328    LDP     X16, X17, [SP], #0x10
329    LDP     X14, X15, [SP], #0x10
330    LDP     X12, X13, [SP], #0x10
331    LDP     X10, X11, [SP], #0x10
332    LDP     X8, X9, [SP], #0x10
333    LDP     X6, X7, [SP], #0x10
334    LDP     X4, X5, [SP], #0x10
335    LDP     X2, X3, [SP], #0x10
336    LDP     X0, X1, [SP], #0x10
337
338    ERET
339
340
341.align 8
342pxCurrentTCBConst: .dword pxCurrentTCB
343ullCriticalNestingConst: .dword ullCriticalNesting
344ullPortTaskHasFPUContextConst: .dword ullPortTaskHasFPUContext
345
346vApplicationIRQHandlerConst: .word vApplicationIRQHandler
347    .word 0
348ullPortInterruptNestingConst: .dword ullPortInterruptNesting
349ullPortYieldRequiredConst: .dword ullPortYieldRequired
350
351ulCORE0_INT_SRCConst: .dword ulCORE0_INT_SRC
352.end
353