229 lines
7.9 KiB
NASM
229 lines
7.9 KiB
NASM
|
/*
|
||
|
|
* Copyright (c) 2006-2020, RT-Thread Development Team
|
||
|
|
*
|
||
|
|
* SPDX-License-Identifier: Apache-2.0
|
||
|
|
*
|
||
|
|
* Date Author Notes
|
||
|
|
* 2021-06-29 Wayne the first version
|
||
|
|
*/
|
||
|
|
|
||
|
|
/* GICv2 - Distributor Registers */
|
||
|
|
#define GICD_CTLR 0x0000
|
||
|
|
#define GICD_TYPER 0x0004
|
||
|
|
#define GICD_IIDR 0x0008
|
||
|
|
#define GICD_STATUSR 0x0010
|
||
|
|
#define GICD_SETSPI_NSR 0x0040
|
||
|
|
#define GICD_CLRSPI_NSR 0x0048
|
||
|
|
#define GICD_SETSPI_SR 0x0050
|
||
|
|
#define GICD_CLRSPI_SR 0x0058
|
||
|
|
#define GICD_SEIR 0x0068
|
||
|
|
#define GICD_IGROUPRn 0x0080
|
||
|
|
#define GICD_ISENABLERn 0x0100
|
||
|
|
#define GICD_ICENABLERn 0x0180
|
||
|
|
#define GICD_ISPENDRn 0x0200
|
||
|
|
#define GICD_ICPENDRn 0x0280
|
||
|
|
#define GICD_ISACTIVERn 0x0300
|
||
|
|
#define GICD_ICACTIVERn 0x0380
|
||
|
|
#define GICD_IPRIORITYRn 0x0400
|
||
|
|
#define GICD_ITARGETSRn 0x0800
|
||
|
|
#define GICD_ICFGR 0x0c00
|
||
|
|
#define GICD_IGROUPMODRn 0x0d00
|
||
|
|
#define GICD_NSACRn 0x0e00
|
||
|
|
#define GICD_SGIR 0x0f00
|
||
|
|
#define GICD_CPENDSGIRn 0x0f10
|
||
|
|
#define GICD_SPENDSGIRn 0x0f20
|
||
|
|
#define GICD_IROUTERn 0x6000
|
||
|
|
|
||
|
|
/* GICv2 - CPU Interface Memory Mapped Registers */
|
||
|
|
#define GICC_CTLR 0x0000
|
||
|
|
#define GICC_PMR 0x0004
|
||
|
|
#define GICC_BPR 0x0008
|
||
|
|
#define GICC_IAR 0x000C
|
||
|
|
#define GICC_EOIR 0x0010
|
||
|
|
#define GICC_RPR 0x0014
|
||
|
|
#define GICC_HPPIR 0x0018
|
||
|
|
#define GICC_ABPR 0x001c
|
||
|
|
#define GICC_AIAR 0x0020
|
||
|
|
#define GICC_AEOIR 0x0024
|
||
|
|
#define GICC_AHPPIR 0x0028
|
||
|
|
#define GICC_APRn 0x00d0
|
||
|
|
#define GICC_NSAPRn 0x00e0
|
||
|
|
#define GICC_IIDR 0x00fc
|
||
|
|
#define GICC_DIR 0x1000
|
||
|
|
|
||
|
|
.section ".text.entrypoint"
|
||
|
|
.global _start
|
||
|
|
|
||
|
|
_start:
|
||
|
|
/* Give execution address for secondary CPU */
|
||
|
|
adr x20, .
|
||
|
|
mov sp, x20
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Enable the SMP bit. */
|
||
|
|
/*=============================================================*/
|
||
|
|
mrs x0, S3_1_C15_C2_1
|
||
|
|
orr x0, x0, #(1<<6)
|
||
|
|
msr S3_1_C15_C2_1, x0
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Read CPU id */
|
||
|
|
/* Primary core(id=0): Help Secondary core leaving. */
|
||
|
|
/* Secondary core(id>0): Notice 'Ready' to Primary core. */
|
||
|
|
/*=============================================================*/
|
||
|
|
/* MPIDR_EL1: Multi-Processor Affinity Register */
|
||
|
|
mrs x1, mpidr_el1
|
||
|
|
and x1, x1, #3
|
||
|
|
cbz x1, .L__cpu_0
|
||
|
|
|
||
|
|
.L__current_cpu_idle:
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Secondary CPU notification */
|
||
|
|
/*=============================================================*/
|
||
|
|
wfe /* Wait for Primary CPU's notification */
|
||
|
|
|
||
|
|
mov x0, #0x48 /* if (*(0x40460048)==0) */
|
||
|
|
movk x0, #0x4046, LSL #16 /* goto L__current_cpu_idle */
|
||
|
|
ldr x1, [x0] /* else */
|
||
|
|
cmp x1, #0 /* *(0x4046004C)=_start */
|
||
|
|
b.eq .L__current_cpu_idle /* goto L__cpus_trans_state */
|
||
|
|
add x0, x0, #4
|
||
|
|
str w1, [x0]
|
||
|
|
|
||
|
|
b .L__cpus_trans_state
|
||
|
|
|
||
|
|
.L__cpu_0:
|
||
|
|
|
||
|
|
// *(0x4046004C) = 0
|
||
|
|
mov x0, #0x4C
|
||
|
|
movk x0, #0x4046, LSL #16
|
||
|
|
mov x1, xzr
|
||
|
|
str w1, [x0]
|
||
|
|
|
||
|
|
mov x10, #0x1000 /* 4096 times looping */
|
||
|
|
.L__cpu_0_loop:
|
||
|
|
sub x10, x10, #0x1
|
||
|
|
cmp x10, #0
|
||
|
|
b.eq .L__cpus_trans_state
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Help CPU-1 to leave IBR. */
|
||
|
|
/*=============================================================*/
|
||
|
|
mov x0, #0x48 /* *(0x40460048) = _start */
|
||
|
|
movk x0, #0x4046, LSL #16
|
||
|
|
mov x1, x20
|
||
|
|
str w1, [x0]
|
||
|
|
|
||
|
|
sev /* Wakeup Secondary CPU */
|
||
|
|
|
||
|
|
add x0, x0, #4 /* if(*(0x4046004C)!=_start) */
|
||
|
|
ldr w2, [x0] /* goto L__cpu_0_loop */
|
||
|
|
cmp x1, x2
|
||
|
|
bne .L__cpu_0_loop
|
||
|
|
|
||
|
|
.L__cpus_trans_state:
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Initialize Gtimer. Set frequency to 12MHz. */
|
||
|
|
/*=============================================================*/
|
||
|
|
mov x0, #0x1B00
|
||
|
|
movk x0, #0xB7, LSL #16
|
||
|
|
msr CNTFRQ_EL0, x0
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Enable GICv2. */
|
||
|
|
/* Assign all IRQs to secure group. */
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Route to secure Group */
|
||
|
|
mov x0, #0x1000
|
||
|
|
movk x0, #0x5080, LSL #16
|
||
|
|
mov w9, #0x3
|
||
|
|
str w9, [x0, GICD_CTLR]
|
||
|
|
ldr w9, [x0, GICD_TYPER]
|
||
|
|
and w10, w9, #0x1f
|
||
|
|
cbz w10, 1f
|
||
|
|
add x11, x0, GICD_IGROUPRn
|
||
|
|
mov w9, #0
|
||
|
|
str w9, [x11], #0x04
|
||
|
|
0: str w9, [x11], #0x04
|
||
|
|
sub w10, w10, #0x1
|
||
|
|
cbnz w10, 0b
|
||
|
|
|
||
|
|
mov x1, #0x2000
|
||
|
|
movk x1, #0x5080, LSL #16
|
||
|
|
mov w0, #3
|
||
|
|
str w0, [x1]
|
||
|
|
|
||
|
|
mov w0, #1 << 7
|
||
|
|
str w0, [x1, #4]
|
||
|
|
1:
|
||
|
|
mov x0, #0x1000
|
||
|
|
movk x0, #0x5080, LSL #16
|
||
|
|
mov x1, #0x2000
|
||
|
|
movk x1, #0x5080, LSL #16
|
||
|
|
|
||
|
|
mov w9, #0
|
||
|
|
str w9, [x0, GICD_IGROUPRn]
|
||
|
|
mov w9, #0x1
|
||
|
|
str w9, [x0, GICD_ISENABLERn]
|
||
|
|
|
||
|
|
mov w9, #0x1e7
|
||
|
|
str w9, [x1, GICC_CTLR]
|
||
|
|
|
||
|
|
mov w9, #0x1 << 7
|
||
|
|
str w9, [x1, GICC_PMR]
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Enable FP/SIMD at EL1 */
|
||
|
|
/*=============================================================*/
|
||
|
|
mov x0, #(3 << 20)
|
||
|
|
msr cpacr_el1, x0 /* Enable FP/SIMD at EL1 */
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Initialize sctlr_el1 */
|
||
|
|
/*=============================================================*/
|
||
|
|
mov x0, xzr
|
||
|
|
orr x0, x0, #(1 << 29) /* Enable LSMAOE at EL1 */
|
||
|
|
orr x0, x0, #(1 << 28) /* Enable nTLSMD at EL1 */
|
||
|
|
orr x0, x0, #(1 << 23) /* Enable SPAN at EL1 */
|
||
|
|
orr x0, x0, #(1 << 22) /* Enable EIS at EL1 */
|
||
|
|
orr x0, x0, #(1 << 20) /* Enable TSCXT at EL1 */
|
||
|
|
orr x0, x0, #(1 << 11) /* Enable EOS at EL1 */
|
||
|
|
msr sctlr_el1, x0
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Initialize scr_el3 */
|
||
|
|
/*=============================================================*/
|
||
|
|
mov x0, xzr
|
||
|
|
/* RW, Lower levels are all AArch32. */
|
||
|
|
orr x0, x0, #(1 << 9) /* Enable SIF */
|
||
|
|
orr x0, x0, #(1 << 8) /* Enable HCE */
|
||
|
|
orr x0, x0, #(1 << 7) /* Enable SMD */
|
||
|
|
orr x0, x0, #(1 << 5) /* RES1[5:4] */
|
||
|
|
orr x0, x0, #(1 << 4)
|
||
|
|
/* Disable FIQ routing */
|
||
|
|
/* Disable IRQ routing */
|
||
|
|
/* Disable NS */
|
||
|
|
msr scr_el3, x0
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Initialize spsr_el3 */
|
||
|
|
/*=============================================================*/
|
||
|
|
mov x0, xzr
|
||
|
|
mov x0, #0b00011 /* AARCH32_SVC */
|
||
|
|
orr x0, x0, #(1 << 8) /* Enable SError and External Abort. */
|
||
|
|
orr x0, x0, #(1 << 7) /* IRQ interrupt Process state mask. */
|
||
|
|
orr x0, x0, #(1 << 6) /* FIQ interrupt Process state mask. */
|
||
|
|
orr x0, x0, #(1 << 4) /* FIQ interrupt Process state mask. */
|
||
|
|
msr spsr_el3, x0
|
||
|
|
|
||
|
|
/*=============================================================*/
|
||
|
|
/* Initialize elr_el3 */
|
||
|
|
/* Jump to Secure AARCH32_SVC from EL3. */
|
||
|
|
/*=============================================================*/
|
||
|
|
adr x0, .aarch32_code /* Exception return to aarch32_code */
|
||
|
|
msr elr_el3, x0
|
||
|
|
eret
|
||
|
|
|
||
|
|
.aarch32_code:
|
||
|
|
|