106 lines
2.8 KiB
ArmAsm
106 lines
2.8 KiB
ArmAsm
|
/* startup.S
|
||
|
|
*
|
||
|
|
* Copyright (C) 2014-2024 R. Stange <rsta2@o2online.de>
|
||
|
|
* Copyright (C) 2024 Romain Calascibetta <romain.calascibetta@gmail.com>
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <sysconfig.h>
|
||
|
|
|
||
|
|
.macro armv8_switch_to_el1_m, xreg1, xreg2
|
||
|
|
/* Initialize Generic Timers */
|
||
|
|
mrs \xreg1, cnthctl_el2
|
||
|
|
orr \xreg1, \xreg1, #0x3 /* Enable EL1 access to timers */
|
||
|
|
msr cnthctl_el2, \xreg1
|
||
|
|
msr cntvoff_el2, xzr
|
||
|
|
|
||
|
|
/* Initialize MPID/MIPDR registers */
|
||
|
|
mrs \xreg1, midr_el1
|
||
|
|
mrs \xreg2, mpidr_el1
|
||
|
|
msr vpidr_el2, \xreg1
|
||
|
|
msr vmpidr_el2, \xreg2
|
||
|
|
|
||
|
|
/* Disable coprocessor traps */
|
||
|
|
mov \xreg1, #0x33ff
|
||
|
|
msr cptr_el2, \xreg1 /* Disable coprocessor traps to EL2 */
|
||
|
|
msr hstr_el2, xzr /* Disable coprocessor trap to EL2 */
|
||
|
|
mov \xreg1, #3 << 20
|
||
|
|
msr cpacr_el1, \xreg1 /* Enable FP/SIMD at EL1 */
|
||
|
|
|
||
|
|
/* Initalize HCR_EL2 */
|
||
|
|
mov \xreg1, #(1 << 31) /* 64bit EL1 */
|
||
|
|
msr hcr_el2, \xreg1
|
||
|
|
|
||
|
|
/* SCTLR_EL1 initialization
|
||
|
|
*
|
||
|
|
* Setting RES1 bits [39, 28, 23, 22, 20, 11] to 1
|
||
|
|
* and RES0 bits [31, 30, 27, 21, 17, 13, 10, 6] to 1
|
||
|
|
* and UCI, EE, E0E, WXN, nTWE, nTWI, UCT, DZE, I, UMA, SED, ITD, CP15BEN,
|
||
|
|
* SA0, SA, C and M to 0
|
||
|
|
*/
|
||
|
|
mov \xreg1, #0x0800
|
||
|
|
movk \xreg1, #0x30d0, lsl #16
|
||
|
|
msr sctlr_el1, \xreg1
|
||
|
|
|
||
|
|
/* Return to the EL1_SP1 mode from EL2 */
|
||
|
|
mov \xreg1, #0x3c4
|
||
|
|
msr spsr_el2, \xreg1 /* EL1_SP0 | D | A | I | F */
|
||
|
|
adr \xreg1, 1f
|
||
|
|
msr elr_el2, \xreg1
|
||
|
|
eret
|
||
|
|
1:
|
||
|
|
|
||
|
|
.endm
|
||
|
|
|
||
|
|
.section .init
|
||
|
|
.globl _start
|
||
|
|
|
||
|
|
_start: // normally entered from armstub8 in EL2 after boot
|
||
|
|
// ldr x0, =MEM_KERNEL_STACK
|
||
|
|
// mov sp, x0
|
||
|
|
// b gilbraltar_sysinit
|
||
|
|
mrs x0, CurrentEL // Check if already in EL1t mode?
|
||
|
|
cmp x0, #4
|
||
|
|
beq 1f
|
||
|
|
|
||
|
|
ldr x0, =MEM_EXCEPTION_STACK // IRQ, FIQ and exception handler run in EL1h
|
||
|
|
msr sp_el1, x0
|
||
|
|
ldr x0, =VectorTable // init exception vector table for EL2
|
||
|
|
msr vbar_el2, x0
|
||
|
|
armv8_switch_to_el1_m x0, x1
|
||
|
|
1:
|
||
|
|
ldr x0, =MEM_KERNEL_STACK // main thread runs in EL1t and use sp_el0
|
||
|
|
mov sp, x0
|
||
|
|
ldr x0, =VectorTable // init exception vector table
|
||
|
|
msr vbar_el1, x0
|
||
|
|
b gilbraltar_sysinit
|
||
|
|
|
||
|
|
/* Multicore
|
||
|
|
* .globl _start_secondary
|
||
|
|
* _start_secondary:
|
||
|
|
* mrs x2, mpidr_el1 // read affinity
|
||
|
|
* lsr x2, x2, #8 // CPU ID is Aff1 in Cortex-A76
|
||
|
|
* and x2, x2, #CORES-1 // Get CPU ID
|
||
|
|
* mrs x0, CurrentEL
|
||
|
|
* cmp x0, #4
|
||
|
|
* beq 1f
|
||
|
|
*
|
||
|
|
* mov x1, #EXCEPTION_STACK_SIZE // calculate exception stack offset for core
|
||
|
|
* mul x1, x1, x2
|
||
|
|
* ldr x0, =MEM_EXCEPTION_STACK // IRC, FIQ, and exception handler run in EL1h
|
||
|
|
* add x0, x0, x1
|
||
|
|
* msr sp_el1, x0 // init their stack
|
||
|
|
*
|
||
|
|
* armv8_switch_to_el1_m x0, x1
|
||
|
|
* 1:
|
||
|
|
* mov x1, #KERNEL_STACK_SIZE // calculate kernel stack offset for the core
|
||
|
|
* mul x1, x1, x2
|
||
|
|
* ldr x0, =MEM_KERNEL_STACK // main thread runs in EL1t and use sp_el0
|
||
|
|
* add x0, x0, x1
|
||
|
|
* mov sp, x0 // init its stack
|
||
|
|
*
|
||
|
|
* ldr x0, =VectorTable // init exception vector table
|
||
|
|
* msr vbar_el1, x0
|
||
|
|
*
|
||
|
|
* b sysinit_secondary
|
||
|
|
*/
|