ORG 0
ivt_start:
    b reset         ; RESET 0x00000000, Reset
    b undef_short   ; UNDEF 0x00000004, Undefined Instruction
    b swi_short     ; SWI   0x00000008, Software Interrupt
    b pabt_short    ; PABT  0x0000000C, Prefetch Abort

    b dabt_short    ; DABT  0x00000010, Data Abort
    b rsv_short     ; RSV   0x00000014, Reserved
    b irq_short     ; IRQ   0x00000018, Interrupt Request
    b fig_short     ; FIQ   0x0000001C, Fast Interrupt Request

    undef_isr_addr: dd hang
    swi_isr_addr:   dd hang
    pabt_isr_addr:  dd hang
    dabt_isr_addr:  dd hang
    rsv_isr_addr:   dd hang
    irq_isr_addr:   dd irq_isr
    fig_isr_addr:   dd hang

undef_short:
    mov r0, 0x10000
    ldr r1, [undef_isr_addr]
    add r1, r0
    mov pc, r1
swi_short:
    mov r0, 0x10000
    ldr r1, [swi_isr_addr]
    add r1, r0
    mov pc, r1
pabt_short:
    mov r0, 0x10000
    ldr r1, [pabt_isr_addr]
    add r1, r0
    mov pc, r1
dabt_short:
    mov r0, 0x10000
    ldr r1, [dabt_isr_addr]
    add r1, r0
    mov pc, r1
rsv_short:
    mov r0, 0x10000
    ldr r1, [rsv_isr_addr]
    add r1, r0
    mov pc, r1
irq_short:
    mov r0, 0x10000
    ldr r1, [irq_isr_addr]
    add r1, r0
    mov pc, r1
fig_short:
    mov r0, 0x10000
    ldr r1, [fig_isr_addr]
    add r1, r0
    mov pc, r1
ivt_end:
    ;***************
    ;* Timer MMIO  *
    ;***************
    TIMER_CVR    EQU 0x04 ; Current Value Register, TimerXValue
    TIMER_CR     EQU 0x08 ; Control Register, TimerXControl
    TIMER_INTCLR EQU 0x0C ; Interrupt Clear Register, TimerXIntClr
    TIMER_BGLOAD EQU 0x18 ; Background Load Register
    ;*************
    ;* Uart MMIO *
    ;*************
    UART_CR      EQU 0x30 ; Control Register
    UART_ICR     EQU 0x44 ; Interrupt Clear Register
    UART_FR      EQU 0x18 ; Flag Register
    UART_IMSC    EQU 0x38 ; Interrupt Mask Register
    UART_ICR     EQU 0x44 ; Interrupt Clear Register

;*********************************
;* IRQ Interrupt Service Routine *
;*********************************
irq_isr:
    push {r0-r12, r14}
    ldr r0, [VIC_IRQSTATUS]
    ldr r1, [r0]
    ; Timer0 IRQ Source Check
    ands r1, #0x10
    bne irq_isr_timer
    ldr r1, [r0]
    ands r1, #0x1000
    bne irq_isr_uart
    b halt
irq_isr_uart:
    mov r1, #0x31
    bl uart_put32

    ldr r0, [UART_BASE]
    mov r1, #0x10
    str r1, [r0, UART_ICR]  ; // THIS RIGHT HERE CHANGES VALUE OF SP TO BULLSHIT
q: b q

    pop {r0-r12, r14}
    subs pc, r14, #4
irq_isr_timer:
    mov r1, 0x30

halt: b halt
    pop {r0-r12, r14}
    subs pc, r14, #4

reset:
    mov sp, #0x20000
    mov r0, #0x10000
    mov r1, #0
    mov r10, ivt_end - ivt_start
    lsr r10, #3
relocate:
    ldmia r0!, {r2, r3, r4, r5, r6, r7, r8, r9}
    stmia r1!, {r2, r3, r4, r5, r6, r7, r8, r9}
    subs r10, #1
    bne relocate
    ;ldmia r0!, {r2, r3, r4, r5, r6, r7, r8, r9}
    ;stmia r1!, {r2, r3, r4, r5, r6, r7, r8, r9}

;****************************************
;*         Enable Timer0 and IRQs       *
;****************************************
    mrs r0, cpsr
    bic r0, r0, #0x80 ; Clear IRQ Disable
    msr cpsr_c, r0

    ldr r1, [TIMER_BASE]
    add r1, TIMER_CR
    mov r0, #0
    str r0, [r1]

    ldr r0, [TIMER_COUNT]
    ldr r1, [TIMER_BASE]
    mov r2, TIMER_BGLOAD
    add r2, r1
    str r0, [r1]
    str r0, [r2]

    ldr r1, [TIMER_BASE]
    add r1, TIMER_INTCLR
    mov r2, #0
    str r2, [r1]

    mov r0, #0xE2 ; Timer Enable, Periodic Mode, IRQ ON, Prescale = 0, 32 bit, Wrapping Mode
    ldr r1, [TIMER_BASE]
    add r1, TIMER_CR
    str r0, [r1]

    mov r0, #0x10
    ldr r1, [VIC_INTENABLE]
;    str r0, [r1]

;****************************************
;*             Enable UART              *
;****************************************
    ldr r0, [UART_BASE]
    mov r1, #0x10 ; Enable Rx Interrupt
    str r1, [r0, UART_IMSC]

    mov r0, #0x1000
    ldr r1, [VIC_INTENABLE]
    str r0, [r1]
    b  mainloop
    UART_BASE: dd 0x101F1000
    TIMER_BASE: dd 0x101E2000
    TIMER_COUNT: dd 1000000
    VIC_BASE: dd 0x10140000
    VIC_IRQSTATUS: dd (0x10140000 + 0x00)
    VIC_FIGSTATUS: dd (0x10140000 + 0x04)
    VIC_RAWINTR:   dd (0x10140000 + 0x08)
    VIC_INTSELECT: dd (0x10140000 + 0x0C)
    VIC_INTENABLE: dd (0x10140000 + 0x10)
    VIC_INTCLEAR:  dd (0x10140000 + 0x14)
    VIC_SOFTINT:   dd (0x10140000 + 0x18)

hang:
    b hang
;****************************************
;*            Kernel Mainloop           *
;****************************************
mainloop:
;    ldr r1, [pc, 4]
;    bl uart_put32
    b mainloop
    dd 0x50505050

;     ldr r0, [TIMER_BASE]
;     add r0, TIMER_CVR
;     ldr r1, [r0]
;     bl uart_put32
;     b mainloop
;****************************************
;* UART Write 32 Bit Value              *
;* Input: R1 Value to be written        *
;* Output: None                         *
;****************************************
uart_put32:
    ldr r0, [pc, 4]   ; PC + 16
    str r1, [r0]
    bx lr
    dd 0x101f1000