98 lines
3.0 KiB
ArmAsm
98 lines
3.0 KiB
ArmAsm
/*
|
|
* Copyright (c) 2006-2020, RT-Thread Development Team
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* Date Author Notes
|
|
* 2020-01-15 bigmagic the first version
|
|
* 2020-08-10 SummerGift support clang compiler
|
|
*/
|
|
|
|
.section ".text.entrypoint","ax"
|
|
|
|
.globl _start
|
|
_start:
|
|
/* Read cpu id */
|
|
mrs x1, mpidr_el1
|
|
and x1, x1, #3
|
|
cbz x1, cpu_setup /* If cpu id > 0, stop slave cores */
|
|
|
|
cpu_idle:
|
|
wfe
|
|
b cpu_idle
|
|
|
|
cpu_setup:
|
|
ldr x1, =_start
|
|
|
|
mrs x0, CurrentEL /* CurrentEL Register. bit 2, 3. Others reserved */
|
|
and x0, x0, #12 /* Clear reserved bits */
|
|
|
|
/* Running at EL3? */
|
|
cmp x0, #12 /* EL3 value is 0b1100 */
|
|
bne cpu_not_in_el3
|
|
|
|
/* Should never be executed, just for completeness. (EL3) */
|
|
mov x0, #(1 << 0) /* EL0 and EL1 are in Non-Secure state */
|
|
orr x0, x0, #(1 << 4) /* RES1 */
|
|
orr x0, x0, #(1 << 5) /* RES1 */
|
|
orr x0, x0, #(1 << 7) /* SMC instructions are undefined at EL1 and above */
|
|
orr x0, x0, #(1 << 8) /* HVC instructions are enabled at EL1 and above */
|
|
orr x0, x0, #(1 << 10) /* The next lower level is AArch64 */
|
|
msr scr_el3, x2
|
|
|
|
mov x2, #0x3c9
|
|
msr spsr_el3, x2 /* 0b1111001001 */
|
|
adr x2, cpu_not_in_el3
|
|
msr elr_el3, x2
|
|
eret /* Exception Return: from EL3, continue from cpu_not_in_el3 */
|
|
|
|
cpu_not_in_el3: /* Running at EL2 or EL1 */
|
|
cmp x0, #4 /* EL1 = 0100 */
|
|
beq cpu_in_el1 /* Halt this core if running in El1 */
|
|
|
|
cpu_in_el2:
|
|
msr sp_el1, x1
|
|
|
|
/* Enable CNTP for EL1 */
|
|
mrs x0, cnthctl_el2 /* Counter-timer Hypervisor Control register */
|
|
orr x0, x0, #3
|
|
msr cnthctl_el2, x0
|
|
msr cntvoff_el2, xzr
|
|
|
|
mov x0, #(1 << 31) /* Enable AArch64 in EL1 */
|
|
orr x0, x0, #(1 << 1) /* SWIO hardwired on Pi3 */
|
|
msr hcr_el2, x0
|
|
|
|
/* Change execution level to EL1 */
|
|
mov x2, #0x3c4
|
|
msr spsr_el2, x2 /* 0b1111000100 */
|
|
adr x2, cpu_in_el1
|
|
msr elr_el2, x2
|
|
eret
|
|
|
|
cpu_in_el1:
|
|
mov sp, x1 /* Set sp in el1 */
|
|
|
|
/* Avoid trap from SIMD or float point instruction */
|
|
mov x1, #0x00300000 /* Don't trap any SIMD/FP instructions in both EL0 and EL1 */
|
|
msr cpacr_el1, x1
|
|
|
|
mrs x1, sctlr_el1
|
|
orr x1, x1, #(1 << 12) /* Enable Instruction */
|
|
bic x1, x1, #(3 << 3) /* Disable SP Alignment check */
|
|
bic x1, x1, #(1 << 1) /* Disable Alignment check */
|
|
msr sctlr_el1, x1
|
|
|
|
ldr x1, =__bss_start
|
|
ldr w2, =__bss_size
|
|
|
|
clean_bss_loop:
|
|
cbz w2, jump_to_entry
|
|
str xzr, [x1], #8
|
|
sub w2, w2, #1
|
|
cbnz w2, clean_bss_loop
|
|
|
|
jump_to_entry:
|
|
b rtthread_startup
|
|
b cpu_idle /* For failsafe, halt this core too */
|