fill from linux

#include "linux_config.h"

.arm
.align 4
.code 32
.text

@ LCD Frambuffers stuff (Physical Addresses)
#define LCD_FB_PDC0           (0x10400400)
#define LCD_FB_PDC1           (0x10400500)
#define LCD_FB_A_ADDR_OFFSET  (0x68)
#define LCD_FB_B_ADDR_OFFSET  (0x94)
#define FB_TOP_SIZE           (400*240*3)
#define FB_BOT_SIZE           (320*240*3)
#define FB_TOP_LEFT1          (VRAM_BASE)
#define FB_TOP_LEFT2          (FB_TOP_LEFT1  + FB_TOP_SIZE)
#define FB_TOP_RIGHT1         (FB_TOP_LEFT2  + FB_TOP_SIZE)
#define FB_TOP_RIGHT2         (FB_TOP_RIGHT1 + FB_TOP_SIZE)
#define FB_BOT_1              (FB_TOP_RIGHT2 + FB_TOP_SIZE)
#define FB_BOT_2              (FB_BOT_1      + FB_BOT_SIZE)

@ This must be Position-independent Code

    .global linux_payloads_start
linux_payloads_start:

    .cpu arm946e-s

arm9_start:
    b arm9_init
    @ required by BRAHMA
    arm9ep_backup:  .long 0xFFFF0000
arm9_init:
    b linux_arm9_stage_start

@@@@@@@@@@@@@@@@@@@@@@@@ ARM9 Stage 0 @@@@@@@@@@@@@@@@@@@@@@@@

linux_arm9_stage_start:

    @ Disable MPU
    mrc p15, 0, r0, c1, c0, 0
    bic r0, r0, #1
    mcr p15, 0, r0, c1, c0, 0

    @ Disable IRQ and FIQ
    mrs r0, cpsr
    orr r0, r0, #(0x80 | 0x40)
    msr cpsr_c, r0


    @ Drain write buffer
    mcr p15, 0, r0, c7, c10, 4

    @ Map VRAM to region 7
    @ Region base: 0x18000000
    @ Region size: 8MB (0b10110)
    ldr r0, =0x1800002D
    mcr p15, 0, r0, c6, c7, 0

    @ Set region 7 permissions:
    @ Privileged: Read/write access
    @ User: Read/write access
    mrc p15, 0, r0, c5, c0, 2
    bic r0, r0, #(0b1111 << 28)
    orr r0, r0, #(0b0011 << 28)
    mcr p15, 0, r0, c5, c0, 2

    ldr r0, =SHARED_CHAR
    ldr r1, =0xFFFFFF
    str r1, [r0]


    @ The ARM9 code is loaded to 0x23F00000 so the
    @ linux_arm11_stage_start address will be at:
    @     0x23F00000 + ARM9_payload_size

    ldr r0, =PA_ARM11_CODE_ADDR
    ldr r1, linux_arm11_stage_pa
    str r1, [r0]

loop:
    @ldr r0, =SHARED_CHAR
    @ldr r0, [r0]
    @bl fill_screen

    b loop

@ r0 = color
fill_screen:
    mov r9, r0

    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_TOP_SIZE)
    add r2, r1, r0 @limit_addr
    mov r6, r1 @ ptr
_fill_for_1:
    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_1

    mov r0, r9

    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_RIGHT1)
    ldr r7, =(FB_TOP_SIZE)
    add r2, r1, r0 @limit_addr
    mov r6, r1 @ ptr
_fill_for_2:
    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_2

    bx lr


    .ltorg

linux_arm11_stage_pa:
    .long 0x23F00000 + (linux_arm11_stage_start - linux_payloads_start)


@@@@@@@@@@@@@@@@@@@@@@@@ ARM11 Stage 1 @@@@@@@@@@@@@@@@@@@@@@@@

    .cpu mpcore
linux_arm11_stage_start:

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

    @ Invalidate Entire Instruction Cache,
    @ also flushes the branch target cache
    mov r0, #0
    mcr p15, 0, r0, c7, c5, 0

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

    @ Disable the MMU and data cache
    @ (the MMU is already disabled)
    mrc p15, 0, r1, c1, c0, 0
    bic r1, r1, #0b101
    mcr p15, 0, r1, c1, c0, 0

    @ Clear exclusive records
    clrex

    @@@ Map FBs to the VRAM @@@

    @ Top screen
    ldr r0, =LCD_FB_PDC0

    @ Left eye
    ldr r1, =FB_TOP_LEFT1
    str r1, [r0, #(LCD_FB_A_ADDR_OFFSET + 0)]
    ldr r1, =FB_TOP_LEFT2
    str r1, [r0, #(LCD_FB_A_ADDR_OFFSET + 4)]

    @ Right eye
    ldr r1, =FB_TOP_RIGHT1
    str r1, [r0, #(LCD_FB_B_ADDR_OFFSET + 0)]
    ldr r1, =FB_TOP_RIGHT2
    str r1, [r0, #(LCD_FB_B_ADDR_OFFSET + 4)]

    @ Bottom screen
    ldr r0, =LCD_FB_PDC1

    ldr r1, =FB_BOT_1
    str r1, [r0, #(LCD_FB_A_ADDR_OFFSET + 0)]
    ldr r1, =FB_BOT_2
    str r1, [r0, #(LCD_FB_A_ADDR_OFFSET + 4)]

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

    @ Jump to the kernel!
    bx lr

    .ltorg

    .global linux_payloads_end
linux_payloads_end: