1/*
2 * Copyright (C) 2018 The Android Open Source Project
3 *
4 * Permission is hereby granted, free of charge, to any person
5 * obtaining a copy of this software and associated documentation
6 * files (the "Software"), to deal in the Software without
7 * restriction, including without limitation the rights to use, copy,
8 * modify, merge, publish, distribute, sublicense, and/or sell copies
9 * of the Software, and to permit persons to whom the Software is
10 * furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
18 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
19 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
20 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
21 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 * SOFTWARE.
23 */
24
25.text
26
27#define MAX_CPU_COUNT (4)
28#define STACK_SIZE (4096)
29
30#define GICD_BASE (0x08000000)
31#define GICD_SGIR (0xf00)
32
33.macro get_cpu_num, reg
34    /* Get cpu number */
35    mrs \reg, mpidr_el1
36    bic \reg, \reg, #(1<<24) /* Clear MT */
37    bic \reg, \reg, #(3<<30) /* Clear RES1 and U */
38.endm
39
40.macro get_secondary_cpu_count, reg
41    mrs \reg, S3_1_C11_C0_2  /*l2ctlr_el1*/
42    lsr \reg, \reg, #24
43.endm
44
45.globl _start
46_start:
47    /* Set exception vector base */
48    adr x0, _start
49
50    mrs x1, CurrentEL
51    and x1, x1, #0xc
52    cmp x1, #0x8
53    bne vbar_setup_not_el2
54    msr vbar_el2, x0
55    b vbar_setup_done
56vbar_setup_not_el2:
57    msr vbar_el1, x0
58
59vbar_setup_done:
60    get_cpu_num x0
61
62    /* Setup stack */
63    mov x1, #STACK_SIZE
64    mul x1, x1, x0
65    adr x2, stack
66    add x2, x2, x1
67    adr x1, stack_space_end
68    cmp x2, x1
69    bhi error
70
71    adr x1, cpu_ready
72    strb w0, [x1, x0] /* cpu_ready[cpuid] = cpuid */
73    dmb st
74    sev
75    mov sp, x2
76
77    /* Jump to c-code */
78    bl boot
79    /* fall-through */
80
81error:
82    mov x0, #0x18
83    adr x1, exit_params
84    /* fall-through */
85
86semihosting:
87    hlt 0xf000
88    ret
89
90exit_params:
91    .quad 0x20026 /* ADP_Stopped_ApplicationExit */
92    .quad 2 /* exit code */
93
94/*
95 * Catch exceptions (except sync-sp_el0 as we don't use that mode and it would
96 * jump to our entry point) and exit in case there is a bug.
97 */
98.macro exception, addr
99.org \addr
100    b error
101.endm
102
103exception 0x080
104exception 0x100
105exception 0x180
106exception 0x200
107exception 0x280
108exception 0x300
109exception 0x380
110exception 0x400
111exception 0x480
112exception 0x500
113exception 0x580
114exception 0x600
115exception 0x680
116exception 0x700
117exception 0x780
118
119.globl trusty_idle
120trusty_idle:
121    // x1 = event_poll
122    adr x0, skip_cpu0_wfi // x0 = &skip_cpu0_wfi
123    get_cpu_num x2 // x2 = cpunum
124    cbz x2, cpu0_idle // if cpunum == 0 goto cpu0_idle
125
126    // skip_cpu0_wfi = cpunum (any value non-0 would work)
127    stlr x2, [x0]
128
129#if GIC_VERSION > 2
130    // Send int 0 to cpu 0 to take it out of wfi
131    ldr x4, =(0 << 24) | 1
132    msr icc_sgi1r_el1, x4
133#else
134    // Send int 15 to cpu 0 to take it out of wfi
135    ldr x3, =GICD_BASE
136    ldr x4, =0x1800f
137    str x4, [x3, GICD_SGIR]
138#endif
139
140    cbnz x1, no_wfi // also use event_poll to skip wfi on secondary cpus
141    b wfi
142
143cpu0_idle:
144    cbz x1, wfi // only clear and skip event_poll wfi
145    ldaxr x3, [x0] // x1 = skip_cpu0_wfi
146    stxr w4, xzr, [x0] // skip_cpu0_wfi = 0
147    cbnz x3, no_wfi
148wfi:
149    wfi
150no_wfi:
151    ret
152
153#if GIC_VERSION > 2
154.globl trusty_local_irq_disable
155trusty_local_irq_disable:
156    /*
157     * Clear doorbell interrupt from trusty or ipi sent from secondary cores in
158     * trusty_idle above.
159     */
160    mrs x0, icc_iar1_el1
161    cmp x0, #1020
162    b.hs .trusty_local_irq_disable_done
163    msr icc_eoir1_el1, x0
164    b trusty_local_irq_disable
165.trusty_local_irq_disable_done:
166    ret
167#endif
168
169.globl arch_start_secondary_cpus
170arch_start_secondary_cpus:
171    sub sp, sp, #16
172    mov x1, #1
173    get_secondary_cpu_count x2
174    cmp x2, #MAX_CPU_COUNT
175    blo .cpu_count_ok
176    mov x2, #(MAX_CPU_COUNT - 1)
177.cpu_count_ok:
178    stp x1, x2, [sp]
179start_secondary_cpu_loop:
180    ldr x0, =0xC4000003 /* psci CPU_ON */
181    adr x2, _start
182    mov x3, #0
183    smc 0
184    cbnz x0, .start_secondary_cpu_failed
185    ldp x1, x2, [sp]
186    add x1, x1, #1
187    stp x1, x2, [sp]
188    cmp x1, x2
189    bls start_secondary_cpu_loop
190    add sp, sp, #16
191
192    mov x1, #0
193.wait_for_secondary_cpu_loop:
194    wfe
195    dmb ld
196    adr x4, cpu_ready
197    ldrb w3, [x4, x1]
198    cmp x3, x1
199    b.ne .wait_for_secondary_cpu_loop
200    add x1, x1, #1
201    cmp x1, x2
202    bls .wait_for_secondary_cpu_loop
203
204    mov x0, 0
205
206.start_secondary_cpu_failed:
207    ret
208
209.data
210    .space STACK_SIZE
211stack:
212    .space STACK_SIZE * (MAX_CPU_COUNT - 1)
213stack_space_end:
214
215cpu_ready:
216    .space MAX_CPU_COUNT
217
218    .align 3
219skip_cpu0_wfi:
220    .long 0
221