#include "config.h"
#include "frame.h"

	.text
	.align 4
	.global _start, ivt
	.extern _bss_start, _end, main, default_exception_handler, default_irq_handler

_start:
	b	reset
	b	und
	b	swi
	b	abt
	b	abt
	b	.
	b	irq
	b	fiq


und:
swi:
abt:
fiq:
	PUSHFRAMEINSVC
	mov		r0, sp
	bl		default_exception_handler
	PULLFRAMEANDEXIT
	
irq:
    PUSHFRAMEINSVC             		@ save context

    ldr     r2, =INTOFFSET          @ get IRQ number to r2
    ldr     r2, [r2]

    ldr     r3, =ivt      			@ IVT
    add     r3, r3, r2, lsl #2      @ ISR to r3

    mov     r0, sp                  @ 1st parameter
    mov     r1, r2                  @ 2nd parameter
    mov     lr, pc
    ldr     pc, [r3]                @ branch to ISR

    ldr     r2, =INTOFFSET          @ get IRQ number to r2, again
    ldr     r2, [r2]

    ldr     r0, =0x1                @ EOI
    mov     r0, r0, lsl r2
    ldr     r1, =SRCPND
    str     r0, [r1]
    ldr     r1, =INTPND
    str     r0, [r1]

    PULLFRAMEANDEXIT             			@ resume context

reset:
	@ disable all interrupts
	ldr	r1, =INTMSK
	ldr	r2, =0xffffffff
	str	r2, [r1]

	ldr	r1, =INTSUBMSK
	ldr	r2, =0x7ff
	str	r2, [r1]
	
	ldr	r1, =SRCPND
	ldr	r2, =0x0
	str	r2, [r1]

	ldr	r1, =SUBSRCPND
	ldr	r2, =0x0
	str	r2, [r1]
	
	ldr	r1, =INTMOD @ all irq mode
	ldr	r2, =0x0
	str	r2, [r1]
	
	ldr	r1, =PRIORITY
	ldr	r2, =0x0
	str	r2, [r1]
	
	ldr	r1, =INTPND
	ldr	r2, =0x0
	str	r2, [r1]
	
	@ zeroify bss
	ldr r0, =_bss_start
	ldr r1, =_end
	ldr r2, =0x0
zeroify:
	cmp		r1, r0
	ble		zeroify.1
	stmia	r0!, {r2}
	b		zeroify
zeroify.1:
	
	@
	@ Setup stack for various modes
	@
	mrs	r0, cpsr
	bic	r0, r0, #PSR_MODE_MASK

	orr	r1, r0, #PSR_MODE_FIQ|PSR_I|PSR_F
	msr	cpsr_cxsf, r1
	ldr	sp, =STACK_FIQ
								
	orr	r1, r0, #PSR_MODE_IRQ|PSR_I|PSR_F
	msr	cpsr_cxsf, r1
	ldr	sp, =STACK_IRQ
							
	orr	r1, r0, #PSR_MODE_SVC|PSR_I|PSR_F
	msr	cpsr_cxsf, r1
	ldr sp, =STACK_SVC

	orr	r1, r0, #PSR_MODE_ABT|PSR_I|PSR_F
	msr	cpsr_cxsf, r1
	ldr	sp, =STACK_ABT

	orr	r1, r0, #PSR_MODE_UND|PSR_I|PSR_F
	msr	cpsr_cxsf, r1
	ldr	sp, =STACK_UND

	orr	r1, r0, #PSR_MODE_SYS|PSR_I|PSR_F
	msr	cpsr_cxsf, r1
	ldr sp, =STACK_SYS

	@
	@ USER mode stack can not be initialized here
	@

	@ enable IRQs
	bl sti

	@ call main
	bl main

die:
	b die

	.global	sti, cli
sti:							@ enable IRQs
	stmfd	sp!, {r0, lr}
	mrs		r0, cpsr
	bic		r0, r0, #PSR_I
	msr		cpsr_cxsf, r0
	ldmfd	sp!, {r0, pc}

cli:							@ disable IRQs
	stmfd	sp!, {r0, lr}
	mrs		r0, cpsr
	orr		r0, r0, #PSR_I
	msr		cpsr_cxsf, r0
	ldmfd	sp!, {r0, pc}

@
@ Interrupt Vector Table
@
ivt:
    .long   default_irq_handler /*0*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler /*4*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler /*8*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler /*12*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler /*16*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler /*20*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler /*24*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler
    .long   default_irq_handler /*28*/
    .long   default_irq_handler 
    .long   default_irq_handler 
    .long   default_irq_handler