DAMN ARM11

.section .text
.arm

@ 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".

@ Before jumping to the Linux entry point we have
@ to do a few things:
@    * Disable the IRQs and FIQs
@    * Get SVC mode
@    * Disable the MMU
@    * Disable the data cache
@    * Put the CPU1 into WFI
@ And the CPU registers must contain this values:
@    * r0 = 0
@    * r1 = machine type number
@    * r2 = physical address of tagged list in system RAM
@ (We don't support DTS yet)

#define MAGIC_SYNC_ADDR (0x1FFFFFF0)

#define MAGIC_ADDR  (0x1FFFFFF8)
#define MAGIC2_ADDR (0x1FFFFFFC)
#define MAGIC3_ADDR (0x1FFFFFF4)
#define RED    0xFF0000
#define GREEN  0x00FF00
#define BLUE   0x0000FF
#define CYAN   0x00FFFF
#define BLACK  0x000000
#define WHITE  0xFFFFFF

@ Linux settings
@ (All these are Physical Address)
#define MACHINE_NUMBER (7500)
#define ZIMAGE_ADDR    (0x20008000)
#define ATAG_ADDR      (0x20000100)
#define INITRD_ADDR    (0x20800000)

@ STUFF

#define PRIV_MEM_BASE                   (0xFFFEE000)
    #define SCU_BASE_ADDR                  (PRIV_MEM_BASE+0x000)
        #define SCU_CONTROL_ADDR               (SCU_BASE_ADDR+0x00)
        #define SCU_CONFIGURATION_ADDR         (SCU_BASE_ADDR+0x04)
        #define SCU_CPU_STATUS_ADDR            (SCU_BASE_ADDR+0x08)
    #define CPU0_INTERRUPT_INTERFACE_ADDR  (PRIV_MEM_BASE+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+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+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+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+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_stage0_start
arm11_stage0_start:

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

    mrc p15, 0, r0, c0, c0, 5
    cmp r0, #0
    bxne lr

    @ Invalidate Entire Instruction Cache. Also flushes the branch target cache
    mov r0, #0
    mcr p15, 0, r0, c7, c5, 0
    MCR p15, 0, r0, c7, c5, 4
    MCR p15, 0, r0, c7, c5, 6

    @ Invalidate Entire Data Cache
    mcr p15, 0, r0, c7, c6, 0

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

    @ Data Synchronization Barrier
    mcr p15, 0, r0, c7, c10, 4

    WFI

    wat:

        @ Get CPU ID
        mrc p15, 0, r0, c0, c0, 5

        mov r1, #0xEFFFFFF8
        cmp r0, #0
        add r0, r0, r0
        add r2, r1, r0
        ldrb r0, [r2]
        addeq r0, r0, #1
        subne r0, r0, #1
        strb r0, [r2]

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


        b wat

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

    mov r0, #0x20000000

    @ Disable the MMU
    mrc p15, 0, r1, c1, c0, 0
    bic r1, r1, #0b1
    mcr p15, 0, r1, c1, c0, 0

    @ Jump to 0x20000000!
    mov pc, r0

    .global arm11_stage0_end
arm11_stage0_end:


    .global arm11_stage1_start
arm11_stage1_start:

    @ Disable SCU
    ldr r0, =SCU_CONTROL_ADDR
    ldr r1, =0b1111111111110
    str r1, [r0]


    @ Setup the stack
    mov sp, #0x27FFFFFC

loop_forever:

    @ Get CPU ID
    mrc p15, 0, r0, c0, c0, 5
    ldr r1, =MAGIC_ADDR
    cmp r0, #0
    add r0, r0, r0
    add r2, r1, r0
    ldrb r0, [r2]
    addeq r0, r0, #1
    subne 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, =0x10146000 @HID
    ldr r2, [r2]
    tst r2, #0b1  @ KEY_A (eq=press)

    bleq send_interrupt

    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, =0x10000000
    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, =0x5000000
    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, =SOFTWARE_INTERRUPT_ADDR
    ldr r5, =(0b0100000000000000<<9 | 1023)
    str r5, [r4]

    ldmfd sp!, {pc}










    and r0, r0, #0xF

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

    @ Disable the data cache
    mrc p15, 0, r0, c1, c0, 0
    bic r0, r0, #0b100
    mcr p15, 0, r0, c1, c0, 0


    @ Check which CPU we are,
    @ jump to CPU0_cont if CPU0
    mrc p15, 0, r0, c0, c0, 5
    and r0, r0, #0xF
    mov r1, #0
    cmp r0, r1
    beq CPU0_cont

    @ CPU1 code:

    @ Flush pending interrupts:
    @ if we don't do this, and
    @ there are pending interrupts,
    @ the CPU1 will wake up just
    @ after entering WFI
    @ *TODO*
    _loop:
        @WFI
        @bl blue_screen
        b _loop


    @ Enable IRQs:
    @ This have to be done because
    @ we want the CPU1 to jump to
    @ the exception handlers when
    @ waking from WFI
    @ mrs r0, cpsr
    @ bic r0, r0, #0x80
    @ msr cpsr_c, r0
    @ WFI


    @ CPU0 code:

CPU0_cont:

lele:

    b lele

    @ Setup the registers before
    @ jumping to the kernel entry
    mov r0, #0
    ldr r1, =MACHINE_NUMBER
    ldr r2, =ATAG_ADDR
    ldr r3, =ZIMAGE_ADDR

    @ Disable the MMU
    mrc p15, 0, r4, c1, c0, 0
    bic r4, r4, #0b1
    mcr p15, 0, r4, c1, c0, 0

    @Jump to the kernel!
    bx r3


#define FB_TOP_LEFT1  (0x20184E60)
#define FB_TOP_LEFT2  (0x201CB370)
#define FB_DIFF       (FB_TOP_LEFT2-FB_TOP_LEFT1)
#define FB_TOP_SIZE   (0x46500)
#define LCD_FB_PDC0 (0x10400400)
#define LCD_FB_PDC1 (0x10400500)
#define LCD_FB_A_ADDR_OFFSET  0x68
#define LCD_FB_B_ADDR_OFFSET  0x94

.global fill_screen
@ r0 = color
fill_screen:
    and r3, r0, #0xFF
    lsr r4, r0, #8
    and r4, r4, #0xFF
    lsr r5, r0, #16
    and r5, r5, #0xFF

    ldr r0, =FB_TOP_SIZE
    ldr r1, =FB_TOP_LEFT1
    ldr r7, =FB_DIFF
    add r2, r1, r0 @limit_addr
    mov r6, r1 @ ptr
_fill_for:
    strb r3, [r6, #0]
    strb r4, [r6, #1]
    strb r5, [r6, #2]
    add r8, r6, r7
    strb r3, [r8, #0]
    strb r4, [r8, #1]
    strb r5, [r8, #2]
    add r6, r6, #3
    cmp r6, r2
    blt _fill_for
    bx lr

    .ltorg @ We want the memory pool here

    .global arm11_stage1_end
arm11_stage1_end: