287 lines
5.6 KiB
ArmAsm
287 lines
5.6 KiB
ArmAsm
|
|
/*
|
|
* Branch according to exception level
|
|
* [taken from U-Boot]
|
|
*/
|
|
.macro switch_el, xreg, el3_label, el2_label, el1_label
|
|
mrs \xreg, CurrentEL
|
|
cmp \xreg, 0xc
|
|
b.eq \el3_label
|
|
cmp \xreg, 0x8
|
|
b.eq \el2_label
|
|
cmp \xreg, 0x4
|
|
b.eq \el1_label
|
|
.endm
|
|
|
|
/*
|
|
* Define two symbols - one weak and one strong with _vector suffix
|
|
*/
|
|
.macro WEAK_VECTOR name
|
|
.weak \name
|
|
\name :
|
|
\name\()_vector:
|
|
.endm
|
|
|
|
.text
|
|
|
|
.globl _start
|
|
.globl _exit.crt0
|
|
.globl watchdogTrigger
|
|
.globl isr_vector_base
|
|
|
|
|
|
_start:
|
|
/* set stack and frame pointer */
|
|
ldr x1, .SP
|
|
mov x0, #0xf
|
|
bic x1, x1, x0
|
|
msr spsel, #1
|
|
mov sp, x1
|
|
mov x29, #0x0
|
|
|
|
ldr x0, .ISR_VECTOR_BASE
|
|
switch_el x1, 3f, 2f, 1f
|
|
3: /* EL3 */
|
|
#if !defined(__SOFTFP__)
|
|
msr cptr_el3, xzr
|
|
#endif
|
|
msr vbar_el3, x0
|
|
b 0f
|
|
|
|
2: /* EL2 */
|
|
msr vbar_el2, x0
|
|
#if !defined(__SOFTFP__)
|
|
/* enable FPU */
|
|
mov x0, #0x33ff
|
|
msr cptr_el2, x0
|
|
#endif
|
|
b 0f
|
|
|
|
1: /* EL1 */
|
|
msr vbar_el1, x0
|
|
#if !defined(__SOFTFP__)
|
|
/* enable FPU */
|
|
mov x0, #3 << 20
|
|
msr cpacr_el1, x0
|
|
#endif
|
|
|
|
0:
|
|
/* Check MultiprocessorID */
|
|
mrs x0, mpidr_el1
|
|
ands x0, x0, #0xf
|
|
b.ne _start_secondary
|
|
|
|
/* copy data from FLASH to SRAM */
|
|
ldr x4, .DATA_S
|
|
ldr x5, .DATA_LADDR
|
|
ldr x6, .DATA_SIZE
|
|
cmp x4, x5
|
|
b.eq data_done
|
|
data_copy:
|
|
ldrb w0, [x5], #0x1
|
|
strb w0, [x4], #0x1
|
|
subs x6, x6, #0x1
|
|
b.ne data_copy
|
|
data_done:
|
|
|
|
bss_init:
|
|
/* clear .bss-section */
|
|
ldr x4, .BSS_S
|
|
ldr x5, .BSS_E
|
|
b bss_check
|
|
bss_clear:
|
|
strb wzr, [x4], #0x1
|
|
bss_check:
|
|
cmp x5, x4
|
|
b.ne bss_clear
|
|
bss_done:
|
|
|
|
/* initialize Watchdog */
|
|
ldr x3, .WDG_INIT
|
|
cmp x3, #0
|
|
b.eq wdg_init_done
|
|
bl crt0_veneer
|
|
wdg_init_done:
|
|
/* register the WatchdogHook */
|
|
ldr x3, .WDG_TRIGGER
|
|
ldr x4, .WDG_HOOK
|
|
str x3, [x4]
|
|
|
|
/* initialization of e.g. static objects */
|
|
ldr x19, .INIT_S
|
|
ldr x20, .INIT_LADDR
|
|
ldr x21, .INIT_SIZE
|
|
add x22, x21, #0x8
|
|
cmp x19, x20
|
|
b.eq init_check
|
|
init_copy:
|
|
subs x21, x21, 0x1
|
|
ldrb w0, [x20, x21]
|
|
strb w0, [x19, x21]
|
|
b.ne init_copy
|
|
b init_check
|
|
init:
|
|
ldr x3, [x19], #0x8
|
|
bl crt0_veneer
|
|
init_check:
|
|
subs x22, x22, #0x8
|
|
b.ne init
|
|
init_done:
|
|
|
|
gomain:
|
|
ldr x3, .MAIN
|
|
bl crt0_veneer
|
|
nop
|
|
|
|
_exit.crt0:
|
|
b _exit.crt0
|
|
|
|
_hang:
|
|
b _hang
|
|
|
|
crt0_veneer:
|
|
/* make a generic veneer call to r3 */
|
|
/* lr is set by preceding bl instruction */
|
|
mov x0, #0
|
|
mov x1, #0
|
|
br x3
|
|
|
|
_start_secondary:
|
|
ldr x1, .STACK_SIZE
|
|
mul x0, x0, x1
|
|
ldr x1, .SP_MP
|
|
add x0, x0, x1
|
|
mov x1, #0xf
|
|
bic x0, x0, x1
|
|
mov sp, x0
|
|
mov x29, #0
|
|
ldr x3, .BACKGROUND
|
|
bl crt0_veneer
|
|
nop
|
|
b _exit.crt0
|
|
.align 3
|
|
.SP:
|
|
.quad __stack_end
|
|
.SP_MP:
|
|
.quad __stack_start_mp
|
|
.STACK_SIZE:
|
|
.quad __stack_size
|
|
.BSS_S:
|
|
.quad __bss_start
|
|
.BSS_E:
|
|
.quad __bss_end
|
|
.INIT_S:
|
|
.quad __init_vaddr
|
|
.INIT_LADDR:
|
|
.quad __init_laddr
|
|
.INIT_SIZE:
|
|
.quad __init_size
|
|
.DATA_S:
|
|
.quad __data_vaddr
|
|
.DATA_LADDR:
|
|
.quad __data_laddr
|
|
.DATA_SIZE :
|
|
.quad __data_size
|
|
#ifndef WATCHDOG
|
|
.WDG_INIT:
|
|
.quad 0x0
|
|
.WDG_TRIGGER:
|
|
.quad 0x0
|
|
#else
|
|
.WDG_INIT:
|
|
.quad WatchdogInit
|
|
.WDG_TRIGGER:
|
|
.quad WatchdogTrigger
|
|
#endif
|
|
.WDG_HOOK:
|
|
.quad watchdogTrigger
|
|
.MAIN:
|
|
.quad main
|
|
.BACKGROUND:
|
|
.quad background
|
|
.ISR_VECTOR_BASE:
|
|
.quad isr_vector_base
|
|
|
|
/*
|
|
* generate a exception vector table
|
|
*/
|
|
.section .isr_vector,"ax"
|
|
isr_vector_base:
|
|
.org 0x000
|
|
WEAK_VECTOR syncExceptionSP0
|
|
b syncExceptionSP0 /* Current EL Synchronous Thread SP0 */
|
|
|
|
.org 0x080
|
|
WEAK_VECTOR irqSP0
|
|
b irqSP0 /* Current EL IRQ Thread SP0 */
|
|
|
|
.org 0x100
|
|
WEAK_VECTOR fiqSP0
|
|
b fiqSP0 /* Current EL FIQ Thread SP0 */
|
|
|
|
.org 0x180
|
|
WEAK_VECTOR errorSP0
|
|
b errorSP0 /* Current EL Error Thread SP0 */
|
|
|
|
.org 0x200
|
|
WEAK_VECTOR syncExceptionSPx
|
|
b syncExceptionSPx /* Current EL Synchronous Handler SPx */
|
|
|
|
.org 0x280
|
|
WEAK_VECTOR irqSPx
|
|
b irqSPx /* Current EL IRQ Handler SPx */
|
|
|
|
.org 0x300
|
|
WEAK_VECTOR fiqSPx
|
|
b fiqSPx /* Current EL FIQ Handler SPx */
|
|
|
|
.org 0x380
|
|
WEAK_VECTOR errorSPx
|
|
b errorSPx /* Current EL Error Handler SPx */
|
|
|
|
.org 0x400
|
|
WEAK_VECTOR syncExceptionA64
|
|
b syncExceptionA64 /* Lower EL Synchronous Handler AARCH64 */
|
|
|
|
.org 0x480
|
|
WEAK_VECTOR irqA64
|
|
b irqA64 /* Lower EL IRQ Handler AARCH64 */
|
|
|
|
.org 0x500
|
|
WEAK_VECTOR fiqA64
|
|
b fiqA64 /* Lower EL FIQ Handler AARCH64 */
|
|
|
|
.org 0x580
|
|
WEAK_VECTOR errorA64
|
|
b errorA64 /* Lower EL Error Handler AARCH64 */
|
|
|
|
.org 0x600
|
|
WEAK_VECTOR syncExceptionA32
|
|
b syncExceptionA32 /* Lower EL Synchronous Handler AARCH32 */
|
|
|
|
.org 0x680
|
|
WEAK_VECTOR irqA32
|
|
b irqA32 /* Lower EL IRQ Handler AARCH32 */
|
|
|
|
.org 0x700
|
|
WEAK_VECTOR fiqA32
|
|
b fiqA32 /* Lower EL FIQ Handler AARCH32 */
|
|
|
|
.org 0x780
|
|
WEAK_VECTOR errorA32
|
|
b errorA32 /* Lower EL Error Handler AARCH32 */
|
|
|
|
|
|
.section .data
|
|
.align 3
|
|
watchdogTrigger:
|
|
.quad 0x0
|
|
|
|
.section .text
|
|
.weak background
|
|
background:
|
|
nop
|
|
1:
|
|
wfe
|
|
b 1b
|