@ This is a Startup.s file for SFB sketches.  Its jobs are:
@
@ (1)   To live at 0x5000, which is where the secondary bootloader,
@       at the moment, branches to when it wants to enter the
@       application,
@
@ (2)   To setup the sketch's initialized and uninitialized memory
@       areas
@
@ (3)   To branch to main() to start the sketch
@
@ Note that it does _not_ do all the other stuff that a
@ startup file would normally do --- set up the clocks,
@ stack, etc -- because this code knows it is running
@ after the secondary bootloader startup code has already
@ done all of that.

.equ UND_Stack_Size,     0x00000020
@ UNUSED: .equ SVC_Stack_Size,     0x00002000 (SVC stack gets all unallocated fastram)
.equ ABT_Stack_Size,     0x00000020
.equ FIQ_Stack_Size,     0x00000020
.equ IRQ_Stack_Size,     0x00000040
@ UNUSED: .equ USR_Stack_Size,     0x00000100 (Not using USR mode at all)

.equ Mode_USR,            0x10
.equ Mode_FIQ,            0x11
.equ Mode_IRQ,            0x12
.equ Mode_SVC,            0x13
.equ Mode_ABT,            0x17
.equ Mode_UND,            0x1B
.equ Mode_SYS,            0x1F

.equ I_Bit,               0x80            @ when I bit is set, IRQ is disabled
.equ F_Bit,               0x40            @ when F bit is set, FIQ is disabled

@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
@ We're just plopping this code into .text, but the
@ linker script arranges for it to be loaded first
@ so that it will land at the appropriate starting address

       .section .text._sketchSetup
	.globl _sketchSetup
_sketchSetup:

@  Setup Stack for each mode
        .extern __rom_fastram_data_start
        LDR     R0, =__ram_fastram_stack_top

@  Enter Undefined Instruction Mode and set its Stack Pointer
        MSR     CPSR_c, #Mode_UND | I_Bit | F_Bit
        MOV     SP, R0
        SUB     R0, R0, #UND_Stack_Size

@  Enter Abort Mode and set its Stack Pointer
        MSR     CPSR_c, #Mode_ABT | I_Bit | F_Bit
        MOV     SP, R0
        SUB     R0, R0, #ABT_Stack_Size

@  Enter FIQ Mode and set its Stack Pointer
        MSR     CPSR_c, #Mode_FIQ | I_Bit | F_Bit
        MOV     SP, R0
        SUB     R0, R0, #FIQ_Stack_Size

@  Enter IRQ Mode and set its Stack Pointer
        MSR     CPSR_c, #Mode_IRQ | I_Bit | F_Bit
        MOV     SP, R0
        SUB     R0, R0, #IRQ_Stack_Size

@  Enter Supervisor Mode and set its Stack Pointer
        MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit
        MOV     SP, R0

@ (Note we're staying in SVC mode; not switching to USR)

@@ Relocate .fastram_data section (Copy from ROM to RAM)
@ (Note this includes the interruptVectorTable defined below.)
        .extern __rom_fastram_data_start
        .extern __ram_fastram_data_start
        .extern __ram_fastram_data_end
        LDR     R1, = __rom_fastram_data_start
        LDR     R2, =__ram_fastram_data_start
        LDR     R3, =__ram_fastram_data_end
        CMP     R2, R3
        BEQ     1f
0:      CMP     R2, R3
        LDRLO   R0, [R1], #4
        STRLO   R0, [R2], #4
        BLO     0b
1:

@  Clear fastram .bss section
        .extern __ram_fastram_bss_start
        .extern __ram_fastram_bss_end
        MOV     R0, #0
        LDR     R1, =__ram_fastram_bss_start
        LDR     R2, =__ram_fastram_bss_end
        CMP     R1,R2
        BEQ     1f
0:      CMP     R1, R2
        STRLO   R0, [R1], #4
        BLO     0b
1:

@ Color in everything from end of fastram_bss up to the top of stack
@ (current SP), so we can measure its usage later.  Each stack word is
@ initialized to the negative of its own address as a flag value.
	
        LDR     R0, =__ram_fastram_bss_end
	MOV     R1, #0
0:      CMP     R0, SP
        BEQ     1f
	SUB     R2, R1, R0
        STR     R2, [R0], #4
        B       0b
1:      

@@ Initialize bodyram
        .extern __rom_bodyram_data_start
        .extern __ram_bodyram_data_start
        .extern __ram_bodyram_data_end
        LDR     R1, = __rom_bodyram_data_start
        LDR     R2, =__ram_bodyram_data_start
        LDR     R3, =__ram_bodyram_data_end
        CMP     R2, R3
        BEQ     1f
0:      CMP     R2, R3
        LDRLO   R0, [R1], #4
        STRLO   R0, [R2], #4
        BLO     0b
1:

@  Clear bodyram .bss section
        .extern __ram_bodyram_bss_start
        .extern __ram_bodyram_bss_end
        MOV     R0, #0
        LDR     R1, =__ram_bodyram_bss_start
        LDR     R2, =__ram_bodyram_bss_end
        CMP     R1,R2
        BEQ     1f
0:      CMP     R1, R2
        STRLO   R0, [R1], #4
        BLO     0b
1:

@@ Same deal for talkram
        .extern __rom_talkram_data_start
        .extern __ram_talkram_data_start
        .extern __ram_talkram_data_end
        LDR     R1, = __rom_talkram_data_start
        LDR     R2, =__ram_talkram_data_start
        LDR     R3, =__ram_talkram_data_end
        CMP     R2, R3
        BEQ     1f
0:      CMP     R2, R3
        LDRLO   R0, [R1], #4
        STRLO   R0, [R2], #4
        BLO     0b
1:

@  Clear talkram .bss section
        .extern __ram_talkram_bss_start
        .extern __ram_talkram_bss_end
        MOV     R0, #0
        LDR     R1, =__ram_talkram_bss_start
        LDR     R2, =__ram_talkram_bss_end
        CMP     R1,R2
        BEQ     1f
0:      CMP     R1, R2
        STRLO   R0, [R1], #4
        BLO     0b
1:

@ Enter the sketch dragon

        .extern main
         LDR R0, =main
        BX      R0


@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
@ The interrupt vector table, that we will link at the beginning of
@ user RAM for interrupt vector mode

.section .fastram.sfb_interrupt_vector.ram, "ax"
.arm

@ Exception Vectors
@  Mapped to Address 0x40000000
@  Absolute addressing mode must be used.
@  Dummy Handlers are implemented as infinite loops which can be modified.

	.globl interruptVectorTable
interruptVectorTable:
        LDR     PC, Reset_Addr
        LDR     PC, Undef_Addr
        LDR     PC, SWI_Addr
        LDR     PC, PAbt_Addr
        LDR     PC, DAbt_Addr
        NOP                            @ Reserved Vector
       LDR     PC, IRQ_Addr
@@        LDR     PC, [PC, #-0x0120]     @ Vector from VicVectAddr
        LDR     PC, FIQ_Addr

Reset_Addr:      .word     Reset_Handler
Undef_Addr:      .word     die_UNDEF
SWI_Addr:        .word     die_SWI
PAbt_Addr:       .word     die_PABT
DAbt_Addr:       .word     die_DABT
                 .word     0xB9206E28             @ Reserved Address
IRQ_Addr:        .word     IRQ_Handler
FIQ_Addr:        .word     FIQ_Handler

	.globl interruptVectorTableEnd
interruptVectorTableEnd:

.section .text
	.globl interruptVectorHandlers
interruptVectorHandlers:

        .extern dieUndefException
die_UNDEF:                   
	SUB     R0, R14, #4     @@ Arg1 is undefined instruction address from R14_und
        MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit
        LDR     SP, =__ram_fastram_stack_top
        LDR     R1, =dieUndefException
        BX      R1

        .extern dieDabtException
die_DABT:
	SUB     R0, R14, #8     @@ Arg1 is faulting instruction address from R14_abt
        MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit  @@ Switch from ABT mode to SVC
        LDR     SP, =__ram_fastram_stack_top
        LDR     R1, =dieDabtException
        BX      R1

        .extern diePabtException
die_PABT:
	SUB     R0, R14, #4     @@ Arg1 is address of aborted instruction from R14_abt
        MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit
        LDR     SP, =__ram_fastram_stack_top
        LDR     R1, =diePabtException
        BX      R1

        .extern dieSWIException
die_SWI:
       	SUB     R0, R14, #4     @@ Arg1 is address of SWI instruction from R14_svc
        MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit
        LDR     SP, =__ram_fastram_stack_top
        LDR     R1, =dieSWIException
        BX      R1

@@ handled via other means, hopefully
@@ IRQ_Handler:    B       IRQ_Handler
FIQ_Handler:     
        .globl die_FIQ
        .extern dieFIQTrap
die_FIQ:
        LDR     R0, =__ram_fastram_stack_top
        MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit
        MOV     SP, R0
        LDR R0, =dieFIQTrap
        BX      R0

@ Reset_Handler tries to return to bootloader memmap mode and then reset
MEMMAP_Addr:    .word 0xE01FC040
Reset_Handler:
        MOV     R0,#0          @ 0 is bootloader mode
        LDR     R1, MEMMAP_Addr @ Point at memmap
        STR     R0,[R1]        @ store it
        MOV     PC,#0          @ 0 is also where we want to branch


@@ IRQ_Wrapper based on Examples for
@@ AT91-ARM7TDMI AIC from Atmel,
@@ adapted to LPC23xx/24xx VIC by M. Thomas
@@ This wrapper avoids compiler-dependencies.

.set LPC_BASE_VIC, 0xFFFFF000
.set VIC_VectAddr, 0xF00

.arm
	.extern cISRWrapper
	.globl IRQ_Handler
IRQ_Handler:

@- Manage Exception Entry
@- Adjust and save LR_irq in IRQ stack
            sub         lr, lr, #4
            stmfd       sp!, {lr}

@- Save SPSR need to be saved for nested interrupt
            mrs         r14, SPSR
            stmfd       sp!, {r14}

@- Save and r0 in IRQ stack
            stmfd       sp!, {r0}

@- Load the ISR-Address from VICVectAddr
            ldr         r14, =LPC_BASE_VIC
            ldr         r0 , [r14, #VIC_VectAddr]

@- Switch in Supervisor Mode (keeping interrupts disabled)
           msr         CPSR_c, # I_Bit | Mode_SVC

@- Save scratch/used registers and LR in User Stack
            stmfd       sp!, { r1-r3, r12, r14 }

            ldr         r1,=cISRWrapper

@- Branch to C-level interrupt wrapper, passing the routine pointed by the VIC_VectAddr
            mov         r14, pc

            bx          r1

@- Restore scratch/used registers and LR from User Stack
            ldmia       sp!, { r1-r3, r12, r14 }

@- Disable Interrupt and switch back in IRQ mode
            msr         CPSR_c, #I_Bit | Mode_IRQ

@-  Mark the End of Interrupt on the VIC
@   by writing to VICVectAddr - not needed -- yes, needed, I think Wed Apr  1 20:42:56 2009 ACKLEY
@   here since already done in the ISRs
            ldr         r14, =LPC_BASE_VIC
            str         r14, [r14, #VIC_VectAddr]

@- Restore R0
            ldmia       sp!, {r0}

@- Restore SPSR_irq and r0 from IRQ stack
            ldmia       sp!, {r14}
            msr         SPSR_cxsf, r14

@- Restore adjusted  LR_irq from IRQ stack directly in the PC
            ldmia       sp!, {pc}^

@                END


        .end
