#include <arm_const.h>

#define CPUID_ARM_VIRT_SHIFT            12
#define CPUID_ARM_VIRT_MASK             (0xF << CPUID_ARM_VIRT_SHIFT)
#define CPUID_ARM_GENTIMER_SHIFT        16
#define CPUID_ARM_GENTIMER_MASK         (0xF << CPUID_ARM_GENTIMER_SHIFT)

#define BCM2708_PERI_BASE 0x3f000000

.arch_extension sec
.arch_extension virt

.section .init
.globl _start
/* the vector table for secure state and HYP mode */
_start:
        b jmp_loader    /* reset */
        .word 0 /* undef */
        adr pc, _secure_monitor
        .word 0
        .word 0
        .word 0
        .word 0
        .word 0

/*
 * secure monitor handler
 * U-boot calls this "software interrupt" in start.S
 * This is executed on a "smc" instruction, we use a "smc #0" to switch
 * to non-secure state.
 * We use only r0 and r1 here, due to constraints in the caller.
 */
_secure_monitor:
        mrc     p15, 0, r1, c1, c1, 0           @ read SCR
        bic     r1, r1, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
        orr     r1, r1, #0x31                   @ enable NS, AW, FW bits

        @mrc    p15, 0, r0, c0, c1, 1           @ read ID_PFR1
        @and    r0, r0, #CPUID_ARM_VIRT_MASK    @ mask virtualization bits
        @cmp    r0, #(1 << CPUID_ARM_VIRT_SHIFT)
        orr     r1, r1, #0x100                  @ allow HVC instruction

        mcr     p15, 0, r1, c1, c1, 0           @ write SCR (with NS bit set)

        mrc     p15, 0, r0, c12, c0, 1          @ get MVBAR value
        mcr     p15, 4, r0, c12, c0, 0          @ write HVBAR

        @ Reset CNTVOFF to 0 before leaving monitor mode
        mov     r0, #0
        mcrr    p15, 4, r0, r0, c14             @ Reset CNTVOFF to zero
1:
@        movs pc, lr
@ by using bx lr instead of movs pc, lr, CPSR is not updated, so I stay in monitor (NS) mode
        bx lr

jmp_loader:
        mrc p15, 0, r0, c1, c0, 0 @ Read System Control Register
        bic r0, r0, #(1<<2)       @ cache disable
        bic r0, r0, #(1<<12)      @ icache disable
        bic r0, r0, #(1<<0)       @ MMU disable
        mcr p15, 0, r0, c1, c0, 0 @ Write System Control Register

        mrc p15, 0, r0, c1, c0, 1 @ Read Auxiliary Control Register
        orr r0, r0, #(1<<6)       @ SMP
        mcr p15, 0, r0, c1, c0, 1 @ Write Auxiliary Control Register

        mov r0, #1
        mcr p15, 0, r0, c14, c3, 1 @ CNTV_CTL (enable=1, imask=0)

@ set to non-sec
        movw    r1, #0x3fff
        movt    r1, #0x0006
        mcr     p15, 0, r1, c1, c1, 2           @ NSACR = all copros to non-sec
@ timer frequency
        ldr     r1, =19200000
        mcr     p15, 0, r1, c14, c0, 0          @ write CNTFRQ

        adr     r1, _start
        mcr     p15, 0, r1, c12, c0, 1          @ set MVBAR to secure vectors
        mrc     p15, 0, ip, c12, c0, 0          @ save secure copy of VBAR

        isb

@        smc     #0                              @ call into MONITOR mode
@ remove the smc call and inline the content of secure_monitor
@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
        cps     #CPSR_MONITOR_MODE

        mrc     p15, 0, r1, c1, c1, 0           @ read SCR
        bic     r1, r1, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
        orr     r1, r1, #0x31                   @ enable NS, AW, FW bits
        orr     r1, r1, #0x100                  @ allow HVC instruction
        mcr     p15, 0, r1, c1, c1, 0           @ write SCR (with NS bit set)

        mrc     p15, 0, r0, c12, c0, 1          @ get MVBAR value
        mcr     p15, 4, r0, c12, c0, 0          @ write HVBAR

        @ Reset CNTVOFF to 0 before leaving monitor mode
        mov     r0, #0
        mcrr    p15, 4, r0, r0, c14             @ Reset CNTVOFF to zero
@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
        @ Still in Monitor mode, but now in Non-Secure state

        mov     r4, #0x8000
        mrc     p15, 0, r0, c0, c0, 5       @ MPIDR
        ubfx    r0, r0, #0, #2
        cmp     r0, #0
        beq     9f

        cmp     r0, #0xff
        bge     10f

        ldr     r5, =0x4000008C         @ mbox
        ldr     r3, =0x00000000         @ magic
        str     r3, [r5, r0, lsl #4]

        ldr     r5, =0x400000CC         @ mbox
1:
        ldr     r4, [r5, r0, lsl #4]
        cmp     r4, r3
        beq     1b

@ clear mailbox
        str     r4, [r5, r0, lsl #4]

9:
        @ldr    r5, = (BCM2708_PERI_BASE + 0x300c)              @ stc0
        @ldr    r6, = 0xaff0aff0
        @str    r4, [r5, r0, lsl #2]


@       msr     cpsr_fsxc, #0xd3
        mov     r0, #0
        ldr     r1, =3138               @ BCM2708 machine id
        ldr     r2, =0x100              @ ATAGS
        bx      r4
10:
        wfi
        b       10b