wakeup CPU0!!

@ This code will be run by the ARM11 processor.
@ The ARM9 processor overwrites the ARM11 exception
@ vectors with a jump to here, so when an ARM11
@ exception raises, our code gets "called".

#define PRIV_MEM_BASE_VA                   (0xFFFEE000)
    #define CPU0_INTERRUPT_INTERFACE_ADDR  (PRIV_MEM_BASE_VA+0x0200)
        #define CPU0_INT_INTR_CTRL_ADDR        (CPU0_INTERRUPT_INTERFACE_ADDR+0x00)
        #define CPU0_INT_INTR_PRIOMASK_ADDR    (CPU0_INTERRUPT_INTERFACE_ADDR+0x04)
        #define CPU0_INT_INTR_BINPOINT_ADDR    (CPU0_INTERRUPT_INTERFACE_ADDR+0x08)
    #define CPU1_INTERRUPT_INTERFACE_ADDR  (PRIV_MEM_BASE_VA+0x0300)
        #define CPU1_INT_INTR_CTRL_ADDR        (CPU1_INTERRUPT_INTERFACE_ADDR+0x00)
        #define CPU1_INT_INTR_PRIOMASK_ADDR    (CPU1_INTERRUPT_INTERFACE_ADDR+0x04)
        #define CPU1_INT_INTR_BINPOINT_ADDR    (CPU1_INTERRUPT_INTERFACE_ADDR+0x08)
    #define CPU0_TIMER_WATCHDOG_ADDR       (PRIV_MEM_BASE_VA+0x0700)
        #define CPU0_TIMER_LOAD_ADDR           (CPU0_TIMER_WATCHDOG_ADDR+0x00)
        #define CPU0_TIMER_COUNTER_ADDR        (CPU0_TIMER_WATCHDOG_ADDR+0x04)
        #define CPU0_TIMER_CONTROL_ADDR        (CPU0_TIMER_WATCHDOG_ADDR+0x08)
        #define CPU0_TIMER_INT_STATUS_ADDR     (CPU0_TIMER_WATCHDOG_ADDR+0x0C)
        #define CPU0_WATCHDOG_COUNTER_ADDR     (CPU0_TIMER_WATCHDOG_ADDR+0x24)
    #define CPU1_TIMER_WATCHDOG_ADDR       (PRIV_MEM_BASE_VA+0x0800)
        #define CPU1_TIMER_LOAD_ADDR           (CPU1_TIMER_WATCHDOG_ADDR+0x00)
        #define CPU1_TIMER_COUNTER_ADDR        (CPU1_TIMER_WATCHDOG_ADDR+0x04)
        #define CPU1_TIMER_CONTROL_ADDR        (CPU1_TIMER_WATCHDOG_ADDR+0x08)
        #define CPU1_TIMER_INT_STATUS_ADDR     (CPU1_TIMER_WATCHDOG_ADDR+0x0C)
        #define CPU1_WATCHDOG_COUNTER_ADDR     (CPU1_TIMER_WATCHDOG_ADDR+0x24)
    #define INTERRUPT_DISTRIBUTOR_ADDR     (PRIV_MEM_BASE_VA+0x1000)
        #define INTERRUPT_DISTR_CTRL_ADDR      (INTERRUPT_DISTRIBUTOR_ADDR+0x000)
        #define INTERRUPT_ENABLE_SET_ADDR      (INTERRUPT_DISTRIBUTOR_ADDR+0x100)
        #define INTERRUPT_ENABLE_CLEAR_ADDR    (INTERRUPT_DISTRIBUTOR_ADDR+0x180)
        #define INTERRUPT_PENDING_SET_ADDR     (INTERRUPT_DISTRIBUTOR_ADDR+0x200)
        #define INTERRUPT_PENDING_CLEAR_ADDR   (INTERRUPT_DISTRIBUTOR_ADDR+0x280)
        #define SOFTWARE_INTERRUPT_ADDR        (INTERRUPT_DISTRIBUTOR_ADDR+0xF00)


    .global arm11_start
arm11_start:

    SEV

    @ Disable FIQs, IRQs,
    @ imprecise aborts,
    @ and enter SVC mode
    CPSID aif, #0x13

    @ Invalidate Both Caches.
    @ Also flushes the branch target cache
    @ mov r0, #0
    @ mcr p15, 0, r0, c7, c5, 4

loop_forever:
    @ Get CPU ID
    mrc p15, 0, r0, c0, c0, 5
    ldr r1, =MAGIC_ADDR
    add r0, r0, r0
    add r2, r1, r0
    ldrb r0, [r2]
    add r0, r0, #1
    strb r0, [r2]

    @ Get CPU0 timer value
    ldr r0, =CPU0_TIMER_COUNTER_ADDR
    ldr r0, [r0]
    ldr r1, =MAGIC2_ADDR
    str r0, [r1]

    @ Get CPU1 timer value
    ldr r0, =CPU1_TIMER_COUNTER_ADDR
    ldr r0, [r0]
    ldr r1, =MAGIC3_ADDR
    str r0, [r1]

    mov r0, #BLUE
    bl fill_screen

    ldr r2, =0xFFFD4000 @HID
    ldr r2, [r2]
    tst r2, #0b1  @ KEY_A (eq=press)

    bleq send_interrupt

    @ Clean and Invalidate entire Data Cache
    mov r0, #0
    mcr p15, 0, r0, c7, c14, 0

    b loop_forever

send_interrupt:
    stmfd sp!, {lr}

    mov r0, #RED
    bl fill_screen

    @ Set Enable bits
    ldr r0, =INTERRUPT_ENABLE_SET_ADDR
    ldr r1, =0xFFFFFFFF
    str r1, [r0, #0x00]
    str r1, [r0, #0x04]
    str r1, [r0, #0x08]
    str r1, [r0, #0x0C]
    str r1, [r0, #0x10]
    str r1, [r0, #0x14]
    str r1, [r0, #0x18]
    str r1, [r0, #0x1C]

    @ Enable GIC
    ldr r0, =INTERRUPT_DISTR_CTRL_ADDR
    ldr r1, =1
    str r1, [r0]

    @ Enable CPU0 interrupt interface
    ldr r0, =CPU0_INT_INTR_CTRL_ADDR
    ldr r1, =1
    str r1, [r0]

    @ Enable CPU1 interrupt interface
    ldr r0, =CPU1_INT_INTR_CTRL_ADDR
    ldr r1, =1
    str r1, [r0]

    @ CPU0 Mask all interrupts
    ldr r0, =CPU0_INT_INTR_PRIOMASK_ADDR
    ldr r1, =0xF0
    str r1, [r0]

    @ CPU1 Mask all interrupts
    ldr r0, =CPU1_INT_INTR_PRIOMASK_ADDR
    ldr r1, =0xF0
    str r1, [r0]

    @ CPU0 all bits compared for pre-emption
    ldr r0, =CPU0_INT_INTR_BINPOINT_ADDR
    ldr r1, =0b011
    str r1, [r0]

    @ CPU1 all bits compared for pre-emption
    ldr r0, =CPU1_INT_INTR_BINPOINT_ADDR
    ldr r1, =0b011
    str r1, [r0]

    @ Enable CPU0 timer
    ldr r0, =CPU0_TIMER_LOAD_ADDR
    ldr r1, =0x40000000
    str r1, [r0]

    ldr r0, =CPU0_TIMER_CONTROL_ADDR
    ldr r1, [r0]
    orr r1, r1, #0b111
    str r1, [r0]

    @ Enable CPU1 timer
    ldr r0, =CPU1_TIMER_LOAD_ADDR
    ldr r1, =0x40000000
    str r1, [r0]

    ldr r0, =CPU1_TIMER_CONTROL_ADDR
    ldr r1, [r0]
    orr r1, r1, #0b111
    str r1, [r0]

    @ Signal the other core
    @ SEV

    @ Send software interrupt ¿?
    @ ldr r4, =SIR_REG_ADDR
    @ ldr r5, =(0b0100000000000000<<9)

    ldmfd sp!, {pc}