1/*
2 * Copyright (c) 2006-2019, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date           Author       Notes
8 * 2013-07-05     Bernard      the first version
9 * 2019-07-28     zdzn         add smp support
10 */
11
12#include "../rtconfig.h"
13.equ Mode_USR,        0x10
14.equ Mode_FIQ,        0x11
15.equ Mode_IRQ,        0x12
16.equ Mode_SVC,        0x13
17.equ Mode_ABT,        0x17
18.equ Mode_UND,        0x1B
19.equ Mode_SYS,        0x1F
20
21.equ I_Bit,           0x80            @ when I bit is set, IRQ is disabled
22.equ F_Bit,           0x40            @ when F bit is set, FIQ is disabled
23
24#ifdef RT_USING_FPU
25.equ UND_Stack_Size,     0x00000400
26#else
27.equ UND_Stack_Size,     0x00000000
28#endif
29.equ SVC_Stack_Size,     0x00000400
30.equ ABT_Stack_Size,     0x00000000
31.equ RT_FIQ_STACK_PGSZ,  0x00000000
32.equ RT_IRQ_STACK_PGSZ,  0x00000800
33.equ USR_Stack_Size,     0x00000400
34
35#define ISR_Stack_Size  (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + \
36                 RT_FIQ_STACK_PGSZ + RT_IRQ_STACK_PGSZ)
37
38.section .data.share.isr
39/* stack */
40
41#ifdef RT_USING_SMP
42.globl stack_start0
43.globl stack_top0
44.globl stack_start1
45.globl stack_top1
46.globl stack_start2
47.globl stack_top2
48.globl stack_start3
49.globl stack_top3
50stack_start0:
51.rept ISR_Stack_Size
52.byte 0
53.endr
54stack_top0:
55
56stack_start1:
57.rept ISR_Stack_Size
58.byte 0
59.endr
60stack_top1:
61
62stack_start2:
63.rept ISR_Stack_Size
64.byte 0
65.endr
66stack_top2:
67
68stack_start3:
69.rept ISR_Stack_Size
70.byte 0
71.endr
72stack_top3:
73
74.globl boot_indicate
75boot_indicate:
76.rept 16
77.byte 0
78.endr
79
80#else
81.globl stack_start
82.globl stack_top
83stack_start:
84.rept ISR_Stack_Size
85.byte 0
86.endr
87stack_top:
88#endif
89
90
91.text
92/* reset entry */
93.globl _reset
94_reset:
95
96    /* Disable IRQ & FIQ */
97    cpsid if
98
99    /* Check for HYP mode */
100    mrs r0, cpsr_all
101    and r0, r0, #0x1F
102    mov r8, #0x1A
103    cmp r0, r8
104    beq overHyped
105    b continue
106
107overHyped: /* Get out of HYP mode */
108    ldr r1, =continue
109    msr ELR_hyp, r1
110    mrs r1, cpsr_all
111    and r1, r1, #0x1f    ;@ CPSR_MODE_MASK
112    orr r1, r1, #0x13    ;@ CPSR_MODE_SUPERVISOR
113    msr SPSR_hyp, r1
114    eret
115
116continue:
117
118    /* disable mmu */
119    bl      rt_cpu_mmu_disable
120    /* set the cpu to SVC32 mode and disable interrupt */
121    mrs     r0, cpsr
122    bic     r0, r0, #0x1f
123    orr     r0, r0, #0x13
124    msr     cpsr_c, r0
125#ifdef RT_USING_SMP
126    mrc     p15, 0, r0, c0, c0, 5
127    ubfx    r0, r0, #0, #2
128    cmp     r0, #0
129    beq     1f
130    /* write boot indicate */
131    ldr     r5, = boot_indicate
132    str     r0, [r5, r0, lsl #2]
133    bl secondary_cpu_start
134    b .
1351:
136#endif
137    /* setup stack */
138#ifdef RT_USING_SMP
139    ldr     r0, =stack_top0
140#else
141    ldr     r0, =stack_top
142#endif
143    bl      stack_setup
144
145    /* clear .bss */
146    mov     r0,#0                   /* get a zero                       */
147    ldr     r1,=__bss_start         /* bss start                        */
148    ldr     r2,=__bss_end           /* bss end                          */
149
150bss_loop:
151    cmp     r1,r2                   /* check if data to clear           */
152    strlo   r0,[r1],#4              /* clear 4 bytes                    */
153    blo     bss_loop                /* loop until done                  */
154    bl rt_hw_init_mmu_table
155    bl init_mbox_mmu_map
156    bl rt_hw_mmu_init
157
158    /* start RT-Thread Kernel */
159    ldr     pc, _rtthread_startup
160_rtthread_startup:
161    .word rtthread_startup
162
163stack_setup:
164
165    @  Set the startup stack for svc
166    mov     sp, r0
167
168    @  Enter Undefined Instruction Mode and set its Stack Pointer
169    msr     cpsr_c, #Mode_UND|I_Bit|F_Bit
170    mov     sp, r0
171    sub     r0, r0, #UND_Stack_Size
172
173    @  Enter Abort Mode and set its Stack Pointer
174    msr     cpsr_c, #Mode_ABT|I_Bit|F_Bit
175    mov     sp, r0
176    sub     r0, r0, #ABT_Stack_Size
177
178    @  Enter FIQ Mode and set its Stack Pointer
179    msr     cpsr_c, #Mode_FIQ|I_Bit|F_Bit
180    mov     sp, r0
181    sub     r0, r0, #RT_FIQ_STACK_PGSZ
182
183    @  Enter IRQ Mode and set its Stack Pointer
184    msr     cpsr_c, #Mode_IRQ|I_Bit|F_Bit
185    mov     sp, r0
186    sub     r0, r0, #RT_IRQ_STACK_PGSZ
187
188    /* come back to SVC mode */
189    msr     cpsr_c, #Mode_SVC|I_Bit|F_Bit
190    bx      lr
191
192.text
193
194/* exception handlers: undef, swi, padt, dabt, resv, irq, fiq          */
195.section .text.isr, "ax"
196    .align  5
197.globl vector_fiq
198vector_fiq:
199    stmfd   sp!,{r0-r7,lr}
200    bl      rt_hw_trap_fiq
201    ldmfd   sp!,{r0-r7,lr}
202    subs    pc, lr, #4
203
204.globl      rt_interrupt_enter
205.globl      rt_interrupt_leave
206.globl      rt_thread_switch_interrupt_flag
207.globl      rt_interrupt_from_thread
208.globl      rt_interrupt_to_thread
209
210.globl      rt_current_thread
211.globl      vmm_thread
212.globl      vmm_virq_check
213
214    .align  5
215.globl vector_irq
216vector_irq:
217#ifdef RT_USING_SMP
218    clrex
219
220    stmfd   sp!, {r0, r1}
221    cps     #Mode_SVC
222    mov     r0, sp          /* svc_sp */
223    mov     r1, lr          /* svc_lr */
224
225    cps     #Mode_IRQ
226    sub     lr, lr, #4
227    stmfd   r0!, {r1, lr}       /* svc_lr, svc_pc */
228    stmfd   r0!, {r2 - r12}
229    ldmfd   sp!, {r1, r2}     /* original r0, r1 */
230    stmfd   r0!, {r1 - r2}
231    mrs     r1,  spsr         /* original mode */
232    stmfd   r0!, {r1}
233
234#ifdef RT_USING_SMART
235    stmfd   r0, {r13, r14}^ /* usr_sp, usr_lr */
236    sub   r0, #8
237#endif
238#ifdef RT_USING_FPU
239    /* fpu context */
240    vmrs r6, fpexc
241    tst  r6, #(1<<30)
242    beq 1f
243    vstmdb r0!, {d0-d15}
244    vstmdb r0!, {d16-d31}
245    vmrs r5, fpscr
246    stmfd r0!, {r5}
2471:
248    stmfd r0!, {r6}
249#endif
250    mov     r8, r0
251
252    bl      rt_interrupt_enter
253    bl      rt_hw_trap_irq
254    bl      rt_interrupt_leave
255
256    cps     #Mode_SVC
257    mov     sp, r8
258    mov     r0, r8
259
260    bl      rt_scheduler_do_irq_switch
261
262    b       rt_hw_context_switch_exit
263
264#else
265    stmfd   sp!, {r0-r12,lr}
266
267    bl      rt_interrupt_enter
268    bl      rt_hw_trap_irq
269    bl      rt_interrupt_leave
270
271    @ if rt_thread_switch_interrupt_flag set, jump to
272    @ rt_hw_context_switch_interrupt_do and don't return
273    ldr     r0, =rt_thread_switch_interrupt_flag
274    ldr     r1, [r0]
275    cmp     r1, #1
276    beq     rt_hw_context_switch_interrupt_do
277
278    ldmfd   sp!, {r0-r12,lr}
279    subs    pc,  lr, #4
280
281rt_hw_context_switch_interrupt_do:
282    mov     r1,  #0         @ clear flag
283    str     r1,  [r0]
284
285    mov     r1, sp          @ r1 point to {r0-r3} in stack
286    add     sp, sp, #4*4
287    ldmfd   sp!, {r4-r12,lr}@ reload saved registers
288    mrs     r0,  spsr       @ get cpsr of interrupt thread
289    sub     r2,  lr, #4     @ save old task's pc to r2
290
291    @ Switch to SVC mode with no interrupt. If the usr mode guest is
292    @ interrupted, this will just switch to the stack of kernel space.
293    @ save the registers in kernel space won't trigger data abort.
294    msr     cpsr_c, #I_Bit|F_Bit|Mode_SVC
295
296    stmfd   sp!, {r2}       @ push old task's pc
297    stmfd   sp!, {r4-r12,lr}@ push old task's lr,r12-r4
298    ldmfd   r1,  {r1-r4}    @ restore r0-r3 of the interrupt thread
299    stmfd   sp!, {r1-r4}    @ push old task's r0-r3
300    stmfd   sp!, {r0}       @ push old task's cpsr
301
302#ifdef RT_USING_SMART
303    stmfd sp, {r13, r14}^  @push usr_sp, usr_lr
304    sub sp, #8
305#endif
306#ifdef RT_USING_FPU
307    /* fpu context */
308    vmrs r6, fpexc
309    tst  r6, #(1<<30)
310    beq 1f
311    vstmdb sp!, {d0-d15}
312    vstmdb sp!, {d16-d31}
313    vmrs r5, fpscr
314    stmfd sp!, {r5}
3151:
316    stmfd sp!, {r6}
317#endif
318
319    ldr     r4,  =rt_interrupt_from_thread
320    ldr     r5,  [r4]
321    str     sp,  [r5]       @ store sp in preempted tasks's TCB
322
323    ldr     r6,  =rt_interrupt_to_thread
324    ldr     r6,  [r6]
325    ldr     sp,  [r6]       @ get new task's stack pointer
326
327#ifdef RT_USING_FPU
328/* fpu context */
329    ldmfd sp!, {r6}
330    vmsr fpexc, r6
331    tst  r6, #(1<<30)
332    beq 1f
333    ldmfd sp!, {r5}
334    vmsr fpscr, r5
335    vldmia sp!, {d16-d31}
336    vldmia sp!, {d0-d15}
3371:
338#endif
339
340#ifdef RT_USING_SMART
341    ldmfd sp, {r13, r14}^  @pop usr_sp, usr_lr
342    add sp, #8
343#endif
344
345    ldmfd   sp!, {r4}       @ pop new task's cpsr to spsr
346    msr     spsr_cxsf, r4
347
348    ldmfd   sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr
349
350#endif
351
352.macro push_svc_reg
353    sub     sp, sp, #17 * 4         @/* Sizeof(struct rt_hw_exp_stack)  */
354    stmia   sp, {r0 - r12}          @/* Calling r0-r12                  */
355    mov     r0, sp
356    mrs     r6, spsr                @/* Save CPSR                       */
357    str     lr, [r0, #15*4]         @/* Push PC                         */
358    str     r6, [r0, #16*4]         @/* Push CPSR                       */
359    cps     #Mode_SVC
360    str     sp, [r0, #13*4]         @/* Save calling SP                 */
361    str     lr, [r0, #14*4]         @/* Save calling PC                 */
362.endm
363
364    .align  5
365    .globl  vector_swi
366vector_swi:
367    push_svc_reg
368    bl      rt_hw_trap_swi
369    b       .
370
371    .align  5
372    .globl  vector_undef
373vector_undef:
374    push_svc_reg
375    cps #Mode_UND
376    bl      rt_hw_trap_undef
377#ifdef RT_USING_FPU
378    ldr     lr, [sp, #15*4]
379    ldmia   sp, {r0 - r12}
380    add     sp, sp, #17 * 4
381    movs    pc, lr
382#endif
383    b       .
384
385    .align  5
386    .globl  vector_pabt
387vector_pabt:
388    push_svc_reg
389    bl      rt_hw_trap_pabt
390    b       .
391
392    .align  5
393    .globl  vector_dabt
394vector_dabt:
395    push_svc_reg
396    bl      rt_hw_trap_dabt
397    b       .
398
399    .align  5
400    .globl  vector_resv
401vector_resv:
402    push_svc_reg
403    bl      rt_hw_trap_resv
404    b       .
405
406#ifdef RT_USING_SMP
407
408.global secondary_cpu_start
409secondary_cpu_start:
410    /* set vector base */
411    mrc p15, 0, r0, c1, c0, 0
412    bic r0, #(1<<13)
413    mcr p15, 0, r0, c1, c0, 0
414
415    /* setup stack */
416    mrc p15, 0, r0, c0, c0, 5
417    ubfx    r0, r0, #0, #2
418    ldr r1, =stack_top0
419    ldr r2, =ISR_Stack_Size
420    mul r3, r2, r0
421    add r0, r1, r3
422    bl  stack_setup
423    /* initialize the mmu table and enable mmu */
424    bl  rt_hw_mmu_init
425    b   secondary_cpu_c_start
426
427#endif
428
429;@ void arm_smp_enable(void);
430.globl arm_smp_enable
431arm_smp_enable:
432    mrc p15, 0, r0, c1, c0, 1   ;@ set SMP bit in ACTLR
433    orr r0, r0, #0x40
434    mcr p15, 0, r0, c1, c0, 1
435    bx lr
436/*
437    mrrc p15, 1, r0, r1, c15
438    orr r0, r0, #0x40
439    mcrr p15, 1, r0, r1, c15
440    dsb
441    isb
442    bx lr
443*/
444.text
445;@ void arm_smp_disable(void);
446.globl arm_smp_disable
447
448arm_smp_disable:
449    mrc p15, 0, r0, c1, c0, 1   ;@ clear SMP bit in ACTLR
450    bic r0, r0, #0x40
451    mcr p15, 0, r0, c1, c0, 1
452    bx lr
453/*
454    mrrc p15, 1, r0, r1, c15
455    bic r0, r0, #0x40
456    mcrr p15, 1, r0, r1, c15
457    bx lr
458*/
459
460