/*
 * 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:
    /*=============================================================*/
    /*      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 CPUs                                         */
    /*=============================================================*/
    wfe
    b       .L__current_cpu_idle

.L__cpu_0:

    /*=============================================================*/
    /*      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 the SMP bit.                                    */
    /*=============================================================*/
    mrs x0, S3_1_C15_C2_1
    orr x0, x0, #(1<<6)
    msr S3_1_C15_C2_1, x0

    /*=============================================================*/
    /*      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
    orr x0, x0, #(1 << 10)          /* Enable AARCH64 */
    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, #0b00101          /* AARCH64_EL1 */
    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. */
    msr spsr_el3, x0

    /*=============================================================*/
    /*      Initialize elr_el3                                     */
    /*      Jump to Secure EL1 from EL3.                   */
    /*=============================================================*/
    adr x0, .aarch64_code     /* Exception return to aarch64_code  */
    msr elr_el3, x0
    eret

.aarch64_code:
    ldr     x1, =_start
    mov     sp, x1

    /*=============================================================*/
    /*      clear bbs                                             */
    /*=============================================================*/
    ldr     x1, =__bss_start
    ldr     w2, =__bss_size

.L__clean_bss_loop:
    cbz     w2, .L__jump_to_entry
    str     xzr, [x1], #8
    sub     w2, w2, #1
    cbnz    w2, .L__clean_bss_loop

    /*=============================================================*/
    /*     jump to C code                                          */
    /*=============================================================*/
.L__jump_to_entry:

    bl      entry

    /*=============================================================*/
    /*     for failsafe, halt this core too                        */
    /*=============================================================*/

    b       .L__current_cpu_idle