• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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.global trusty_get_cpu_num
154trusty_get_cpu_num:
155    get_cpu_num x0
156    ret
157
158#if GIC_VERSION > 2
159.globl trusty_local_irq_disable
160trusty_local_irq_disable:
161    /*
162     * Clear doorbell interrupt from trusty or ipi sent from secondary cores in
163     * trusty_idle above.
164     */
165    mrs x0, icc_iar1_el1
166    cmp x0, #1020
167    b.hs .trusty_local_irq_disable_done
168    msr icc_eoir1_el1, x0
169    b trusty_local_irq_disable
170.trusty_local_irq_disable_done:
171    ret
172#endif
173
174.globl arch_start_secondary_cpus
175arch_start_secondary_cpus:
176    sub sp, sp, #16
177    mov x1, #1
178    get_secondary_cpu_count x2
179    cmp x2, #MAX_CPU_COUNT
180    blo .cpu_count_ok
181    mov x2, #(MAX_CPU_COUNT - 1)
182.cpu_count_ok:
183    stp x1, x2, [sp]
184start_secondary_cpu_loop:
185    ldr x0, =0xC4000003 /* psci CPU_ON */
186    adr x2, _start
187    mov x3, #0
188    smc 0
189    cbnz x0, .start_secondary_cpu_failed
190    ldp x1, x2, [sp]
191    add x1, x1, #1
192    stp x1, x2, [sp]
193    cmp x1, x2
194    bls start_secondary_cpu_loop
195    add sp, sp, #16
196
197    mov x1, #0
198.wait_for_secondary_cpu_loop:
199    wfe
200    dmb ld
201    adr x4, cpu_ready
202    ldrb w3, [x4, x1]
203    cmp x3, x1
204    b.ne .wait_for_secondary_cpu_loop
205    add x1, x1, #1
206    cmp x1, x2
207    bls .wait_for_secondary_cpu_loop
208
209    mov x0, 0
210
211.start_secondary_cpu_failed:
212    ret
213
214.data
215    .space STACK_SIZE
216stack:
217    .space STACK_SIZE * (MAX_CPU_COUNT - 1)
218stack_space_end:
219
220cpu_ready:
221    .space MAX_CPU_COUNT
222
223    .align 3
224skip_cpu0_wfi:
225    .long 0
226