1/*
2 * Copyright (c) 2006-2020, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Date           Author       Notes
7 * 2021-06-29     Wayne        the first version
8 */
9
10/* GICv2 - Distributor Registers */
11#define GICD_CTLR        0x0000
12#define GICD_TYPER       0x0004
13#define GICD_IIDR        0x0008
14#define GICD_STATUSR     0x0010
15#define GICD_SETSPI_NSR  0x0040
16#define GICD_CLRSPI_NSR  0x0048
17#define GICD_SETSPI_SR   0x0050
18#define GICD_CLRSPI_SR   0x0058
19#define GICD_SEIR        0x0068
20#define GICD_IGROUPRn    0x0080
21#define GICD_ISENABLERn  0x0100
22#define GICD_ICENABLERn  0x0180
23#define GICD_ISPENDRn    0x0200
24#define GICD_ICPENDRn    0x0280
25#define GICD_ISACTIVERn  0x0300
26#define GICD_ICACTIVERn  0x0380
27#define GICD_IPRIORITYRn 0x0400
28#define GICD_ITARGETSRn  0x0800
29#define GICD_ICFGR       0x0c00
30#define GICD_IGROUPMODRn 0x0d00
31#define GICD_NSACRn      0x0e00
32#define GICD_SGIR        0x0f00
33#define GICD_CPENDSGIRn  0x0f10
34#define GICD_SPENDSGIRn  0x0f20
35#define GICD_IROUTERn    0x6000
36
37/* GICv2 - CPU Interface Memory Mapped Registers */
38#define GICC_CTLR        0x0000
39#define GICC_PMR         0x0004
40#define GICC_BPR         0x0008
41#define GICC_IAR         0x000C
42#define GICC_EOIR        0x0010
43#define GICC_RPR         0x0014
44#define GICC_HPPIR       0x0018
45#define GICC_ABPR        0x001c
46#define GICC_AIAR        0x0020
47#define GICC_AEOIR       0x0024
48#define GICC_AHPPIR      0x0028
49#define GICC_APRn        0x00d0
50#define GICC_NSAPRn      0x00e0
51#define GICC_IIDR        0x00fc
52#define GICC_DIR         0x1000
53
54.section ".text.entrypoint"
55.global _start
56
57_start:
58    /* Give execution address for secondary CPU */
59    adr     x20, .
60    mov     sp, x20
61
62    /*=============================================================*/
63    /*      Enable the SMP bit.                                    */
64    /*=============================================================*/
65    mrs x0, S3_1_C15_C2_1
66    orr x0, x0, #(1<<6)
67    msr S3_1_C15_C2_1, x0
68
69    /*=============================================================*/
70    /*      Read CPU id                                            */
71    /*      Primary core(id=0): Help Secondary core leaving.       */
72    /*      Secondary core(id>0): Notice 'Ready' to Primary core.  */
73    /*=============================================================*/
74    /* MPIDR_EL1: Multi-Processor Affinity Register */
75    mrs     x1, mpidr_el1
76    and     x1, x1, #3
77    cbz     x1, .L__cpu_0
78
79.L__current_cpu_idle:
80    /*=============================================================*/
81    /*      Secondary CPU notification                             */
82    /*=============================================================*/
83    wfe                     /* Wait for Primary CPU's notification */
84
85    mov     x0, #0x48              /* if (*(0x40460048)==0)        */
86    movk    x0, #0x4046, LSL #16   /*     goto L__current_cpu_idle */
87    ldr     x1, [x0]               /* else                         */
88    cmp     x1, #0                 /*     *(0x4046004C)=_start     */
89    b.eq    .L__current_cpu_idle   /*     goto L__cpus_trans_state */
90    add     x0, x0, #4
91    str     w1, [x0]
92
93    b       .L__cpus_trans_state
94
95.L__cpu_0:
96
97    // *(0x4046004C) = 0
98    mov     x0, #0x4C
99    movk    x0, #0x4046, LSL #16
100    mov     x1, xzr
101    str     w1, [x0]
102
103    mov     x10, #0x1000          /* 4096 times looping */
104.L__cpu_0_loop:
105    sub     x10, x10, #0x1
106    cmp     x10, #0
107    b.eq    .L__cpus_trans_state
108    /*=============================================================*/
109    /*      Help CPU-1 to leave IBR.                               */
110    /*=============================================================*/
111    mov     x0, #0x48            /* *(0x40460048) = _start    */
112    movk    x0, #0x4046, LSL #16
113    mov     x1, x20
114    str     w1, [x0]
115
116    sev                          /* Wakeup Secondary CPU      */
117
118    add     x0, x0, #4           /* if(*(0x4046004C)!=_start) */
119    ldr     w2, [x0]             /*     goto L__cpu_0_loop    */
120    cmp     x1, x2
121    bne     .L__cpu_0_loop
122
123.L__cpus_trans_state:
124
125    /*=============================================================*/
126    /*      Initialize Gtimer. Set frequency to 12MHz.             */
127    /*=============================================================*/
128    mov     x0, #0x1B00
129    movk    x0, #0xB7, LSL #16
130    msr     CNTFRQ_EL0, x0
131
132    /*=============================================================*/
133    /*      Enable GICv2.                                          */
134    /*      Assign all IRQs to secure group.                       */
135    /*=============================================================*/
136    /* Route to secure Group */
137    mov     x0, #0x1000
138    movk    x0, #0x5080, LSL #16
139    mov     w9, #0x3
140    str     w9, [x0, GICD_CTLR]
141    ldr     w9, [x0, GICD_TYPER]
142    and     w10, w9, #0x1f
143    cbz     w10, 1f
144    add     x11, x0, GICD_IGROUPRn
145    mov     w9, #0
146    str     w9, [x11], #0x04
1470:  str     w9, [x11], #0x04
148    sub     w10,  w10, #0x1
149    cbnz    w10,  0b
150
151    mov     x1, #0x2000
152    movk    x1, #0x5080, LSL #16
153    mov     w0,  #3
154    str     w0,  [x1]
155
156    mov     w0, #1 << 7
157    str     w0, [x1, #4]
1581:
159    mov     x0, #0x1000
160    movk    x0, #0x5080, LSL #16
161    mov     x1, #0x2000
162    movk    x1, #0x5080, LSL #16
163
164    mov     w9, #0
165    str     w9, [x0, GICD_IGROUPRn]
166    mov     w9, #0x1
167    str     w9, [x0, GICD_ISENABLERn]
168
169    mov     w9, #0x1e7
170    str     w9, [x1, GICC_CTLR]
171
172    mov     w9, #0x1 << 7
173    str     w9, [x1, GICC_PMR]
174
175    /*=============================================================*/
176    /*      Enable FP/SIMD at EL1                                  */
177    /*=============================================================*/
178    mov	x0, #(3 << 20)
179    msr	cpacr_el1, x0               /* Enable FP/SIMD at EL1 */
180
181    /*=============================================================*/
182    /*      Initialize sctlr_el1                                   */
183    /*=============================================================*/
184    mov x0, xzr
185    orr x0, x0, #(1 << 29)          /* Enable LSMAOE at EL1 */
186    orr x0, x0, #(1 << 28)          /* Enable nTLSMD at EL1 */
187    orr x0, x0, #(1 << 23)          /* Enable SPAN at EL1 */
188    orr x0, x0, #(1 << 22)          /* Enable EIS at EL1 */
189    orr x0, x0, #(1 << 20)          /* Enable TSCXT at EL1 */
190    orr x0, x0, #(1 << 11)          /* Enable EOS at EL1 */
191    msr sctlr_el1, x0
192
193    /*=============================================================*/
194    /*      Initialize scr_el3                                     */
195    /*=============================================================*/
196    mov x0, xzr
197    /* RW, Lower levels are all AArch32. */
198    orr x0, x0, #(1 << 9)           /* Enable SIF */
199    orr x0, x0, #(1 << 8)           /* Enable HCE */
200    orr x0, x0, #(1 << 7)           /* Enable SMD */
201    orr x0, x0, #(1 << 5)           /* RES1[5:4] */
202    orr x0, x0, #(1 << 4)
203    /* Disable FIQ routing */
204    /* Disable IRQ routing */
205    /* Disable NS */
206    msr scr_el3, x0
207
208    /*=============================================================*/
209    /*      Initialize spsr_el3                                    */
210    /*=============================================================*/
211    mov x0, xzr
212    mov x0, #0b00011          /* AARCH32_SVC */
213    orr x0, x0, #(1 << 8)     /* Enable SError and External Abort. */
214    orr x0, x0, #(1 << 7)     /* IRQ interrupt Process state mask. */
215    orr x0, x0, #(1 << 6)     /* FIQ interrupt Process state mask. */
216    orr x0, x0, #(1 << 4)     /* FIQ interrupt Process state mask. */
217    msr spsr_el3, x0
218
219    /*=============================================================*/
220    /*      Initialize elr_el3                                     */
221    /*      Jump to Secure AARCH32_SVC from EL3.                   */
222    /*=============================================================*/
223    adr x0, .aarch32_code     /* Exception return to aarch32_code  */
224    msr elr_el3, x0
225    eret
226
227.aarch32_code:
228
229