.extern     main_func
.text 
.global _entry 
_entry:      
    b   SystemReset
UndefHandler:
    b   UndefHandler 
SwiHandler:
    b   SwiHandler
PrefetchHandler:
    b   PrefetchHandler
DataAbortHandler:
    b   DataAbortHandler
ReservedHandler:
    b   ReservedHandler
    b   IRQ_Entry
FIQHandler:
    b   FIQHandler

SystemReset:                  
    ldr stack_ptr, =0x1000
    bl  disable_watchdog
    
    msr cpsr_c, #0xd2
    ldr stack_ptr, =0xC00

    msr cpsr_c, #0xdf
    ldr stack_ptr, =0x1000

    bl  init_led_gpio
    bl  setup_irq
    msr cpsr_c, #0x5f
    
    ldr link_reg, =system_halt
    ldr pc, =main_func
system_halt:
    b   system_halt

IRQ_Entry:
    sub link_reg, link_reg, #4
    stmdb   stack_ptr!,    { reg0-reg12,link_reg }
    ldr link_reg, =isr_return
    ldr pc, =custom_isr
isr_return:
    ldmia   stack_ptr!,    { reg0-reg12,pc }^

.globl    __vectors_start
__vectors_start:
    swi    SYS_ERROR0
    b    undef_vector
    ldr    pc, .LCswi
    b    pabt_vector
    b    dabt_vector
    b    unused_vector
    b    irq_vector
    b    fiq_vector

.macro    vector_stub, mode, offset
\mode:
    sub    link_reg, link_reg, #4
    stmia    stack_ptr, {reg0, link_reg}    
    mrs    link_reg, spsr
    str    link_reg, [stack_ptr, #8]        
    mrs    reg0, cpsr
    eor    reg0, reg0, #(\mode ^ SVC_MODE)
    msr    spsr_cxsf, reg0
    and    link_reg, link_reg, #0x0f
    mov    reg0, stack_ptr
    ldr    link_reg, [pc, link_reg, lsl #2]
    movs    pc, link_reg
.endm

__usr_irq:
    usr_entry
    get_thread_info tsk_reg
    irq_handler
    mov    status_reg, #0
    b    user_return

.macro    irq_dispatcher
    get_irqnr_start r5, lr
loop1:
    get_irq_number reg0, r6, r5, lr
    movne    reg1, stack_ptr
    adrne    lr, loop1
    bne    process_irq
.endm

.macro    get_irq_number, irqid, status, base, tmp
    mov    \base, #IRQ_BASE_ADDR
    ldr    \status, [ \base, #INT_PENDING ]
    ldr    \irqid, [ \base, #INT_OFFSET ]
    adds    \irqid, \irqid, #BASE_IRQ_NUM
.endm