207 lines
3.7 KiB
ArmAsm
207 lines
3.7 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
|
|
|
|
.text
|
|
|
|
.globl _start
|
|
.globl _exit.crt0
|
|
.globl watchdogTrigger
|
|
|
|
|
|
_start:
|
|
/* set stack and frame pointer */
|
|
ldr x1, .SP
|
|
mov x0, #0xf
|
|
bic x1, x1, x0
|
|
mov sp, x1
|
|
mov x29, #0x0
|
|
|
|
ldr x0, .ISR_VECTOR_BASE
|
|
switch_el x1, 3f, 2f, 1f
|
|
3: /* EL3 */
|
|
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
|
|
|
|
bss_init:
|
|
/* clear .bss-section */
|
|
ldr x4, .BSS_S
|
|
ldr x5, .BSS_E
|
|
mov x0, #0
|
|
b bss_check
|
|
bss_clear:
|
|
strb w0, [x4]
|
|
add x4, x4, #1
|
|
bss_check:
|
|
cmp x5, x4
|
|
bne bss_clear
|
|
bss_done:
|
|
|
|
/* initialize Watchdog */
|
|
ldr x3, .WDG_INIT
|
|
cmp x3, #0
|
|
beq 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 x4, .INIT_S
|
|
ldr x5, .INIT_E
|
|
b init_check
|
|
init:
|
|
ldr x3, [x4]
|
|
bl crt0_veneer
|
|
add x4, x4, #8
|
|
init_check:
|
|
cmp x5, x4
|
|
bne 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
|
|
|
|
//.org 0xF8
|
|
_start_secondary:
|
|
ldr x1, .STACK_SIZE
|
|
mul x0, x0, x1
|
|
ldr x1, .SP_MP
|
|
add 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_array_start
|
|
.INIT_E:
|
|
.quad __init_array_end
|
|
.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
|
|
|
|
.section .isr_vector,"ax"
|
|
isr_vector_base:
|
|
.org 0x000
|
|
b _hang /* Current EL Synchronous Thread */
|
|
|
|
.org 0x080
|
|
b _hang /* Current EL IRQ Thread */
|
|
|
|
.org 0x100
|
|
b _hang /* Current EL FIQ Thread */
|
|
|
|
.org 0x180
|
|
b _hang /* Current EL Error Thread */
|
|
|
|
.org 0x200
|
|
b _hang /* Current EL Synchronous Handler */
|
|
|
|
.org 0x280
|
|
b _hang /* Current EL IRQ Handler */
|
|
|
|
.org 0x300
|
|
b _hang /* Current EL FIQ Handler */
|
|
|
|
.org 0x380
|
|
b _hang /* Current EL Error Handler */
|
|
|
|
.section .data
|
|
.align 3
|
|
watchdogTrigger:
|
|
.quad 0x0
|
|
|
|
.section .text
|
|
.weak background
|
|
background:
|
|
nop
|
|
1:
|
|
wfe
|
|
b 1b
|