| /* |
| * Copyright (C) 2018 The Android Open Source Project |
| * |
| * Permission is hereby granted, free of charge, to any person |
| * obtaining a copy of this software and associated documentation |
| * files (the "Software"), to deal in the Software without |
| * restriction, including without limitation the rights to use, copy, |
| * modify, merge, publish, distribute, sublicense, and/or sell copies |
| * of the Software, and to permit persons to whom the Software is |
| * furnished to do so, subject to the following conditions: |
| * |
| * The above copyright notice and this permission notice shall be |
| * included in all copies or substantial portions of the Software. |
| * |
| * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
| * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
| * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
| * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS |
| * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN |
| * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN |
| * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| * SOFTWARE. |
| */ |
| |
| .text |
| |
| #define CPU_COUNT (4) |
| #define STACK_SIZE (4096) |
| |
| #define GICD_BASE (0x08000000) |
| #define GICD_SGIR (0xf00) |
| |
| .macro get_cpu_num, reg |
| /* Get cpu number */ |
| mrs \reg, mpidr_el1 |
| bic \reg, \reg, #(1<<24) /* Clear MT */ |
| bic \reg, \reg, #(3<<30) /* Clear RES1 and U */ |
| .endm |
| |
| .globl _start |
| _start: |
| /* Set exception vector base */ |
| adr x0, _start |
| |
| mrs x1, CurrentEL |
| and x1, x1, #0xc |
| cmp x1, #0x8 |
| bne vbar_setup_not_el2 |
| msr vbar_el2, x0 |
| b vbar_setup_done |
| vbar_setup_not_el2: |
| msr vbar_el1, x0 |
| |
| vbar_setup_done: |
| get_cpu_num x0 |
| |
| /* Setup stack */ |
| mov x1, #STACK_SIZE |
| mul x1, x1, x0 |
| adr x2, stack |
| add x2, x2, x1 |
| adr x1, stack_space_end |
| cmp x2, x1 |
| bhi error |
| mov sp, x2 |
| |
| /* Jump to c-code */ |
| bl boot |
| /* fall-through */ |
| |
| error: |
| mov x0, #0x18 |
| adr x1, exit_params |
| /* fall-through */ |
| |
| .globl semihosting |
| semihosting: |
| hlt 0xf000 |
| ret |
| |
| exit_params: |
| .quad 0x20026 /* ADP_Stopped_ApplicationExit */ |
| .quad 2 /* exit code */ |
| |
| /* |
| * Catch exceptions (except sync-sp_el0 as we don't use that mode and it would |
| * jump to our entry point) and exit in case there is a bug. |
| */ |
| .macro exception, addr |
| .org \addr |
| b error |
| .endm |
| |
| exception 0x080 |
| exception 0x100 |
| exception 0x180 |
| exception 0x200 |
| exception 0x280 |
| exception 0x300 |
| exception 0x380 |
| exception 0x400 |
| exception 0x480 |
| exception 0x500 |
| exception 0x580 |
| exception 0x600 |
| exception 0x680 |
| exception 0x700 |
| exception 0x780 |
| |
| .globl trusty_idle |
| trusty_idle: |
| // x1 = event_poll |
| adr x0, skip_cpu0_wfi // x0 = &skip_cpu0_wfi |
| get_cpu_num x2 // x2 = cpunum |
| cbz x2, cpu0_idle // if cpunum == 0 goto cpu0_idle |
| |
| // skip_cpu0_wfi = cpunum (any value non-0 would work) |
| stlr x2, [x0] |
| |
| // Send int 15 to cpu 0 to take it out of wfi |
| ldr x3, =GICD_BASE |
| ldr x4, =0x1800f |
| str x4, [x3, GICD_SGIR] |
| |
| b wfi |
| |
| cpu0_idle: |
| cbz x1, wfi // only clear and skip event_poll wfi |
| ldaxr x3, [x0] // x1 = skip_cpu0_wfi |
| stxr w4, xzr, [x0] // skip_cpu0_wfi = 0 |
| cbnz x3, no_wfi |
| wfi: |
| wfi |
| no_wfi: |
| ret |
| |
| .globl arch_start_secondary_cpus |
| arch_start_secondary_cpus: |
| sub sp, sp, #8 |
| mov x1, #1 |
| str x1, [sp] |
| start_secondary_cpu_loop: |
| ldr x0, =0xC4000003 /* psci CPU_ON */ |
| adr x2, _start |
| mov x3, #0 |
| smc 0 |
| ldr x1, [sp] |
| add x1, x1, #1 |
| str x1, [sp] |
| cmp x1, #CPU_COUNT |
| blo start_secondary_cpu_loop |
| add sp, sp, #8 |
| ret |
| |
| .data |
| .space STACK_SIZE |
| stack: |
| .space STACK_SIZE * (CPU_COUNT - 1) |
| stack_space_end: |
| |
| skip_cpu0_wfi: |
| .long 0 |