;-----------------------------------
; File:          firmware_remap_startup.s79
; Function:      Basic firmware startup code (with exception vectors remap)
; Supported chip(s):
;    - AT91SAM7XC128
;    - AT91SAM7XC256
; Supported toolchain(s):
;    - IAR Embedded Workbench
; Date created:  04 May 2006
; Created by:    JJo
;-----------------------------------

;-----------------------------------
; Includes

#include <AT91SAM7X256_inc.h>

;-----------------------------------
; Code start

;**
;* Begin a program module, with a 32-bit aligned relocatable code segment located
;* at address 0. Core is in ARM (32-bit) mode.
;**

  PROGRAM ?RESET           
  RSEG	  INTRAMEND_REMAP
  RSEG    INTRAMSTART
  RSEG	  INTVEC:CODE(2)     
  CODE32
  ORG	  0

;-----------------------------------
; Exception vectors
;
; Only IRQ support is required by the bootloader, so it cannot be modified.
; The other vectors can be adjusted depending on the client application.

reset:
  B       initReset           ; 0x00 Reset handler
undefvec:
  B       undef_handler       ; 0x04 Undefined Instruction
swivec:
  B       swi_handler         ; 0x08 Software Interrupt
pabtvec:
  B       pabt_handler        ; 0x0C Prefetch Abort
dabtvec:
  B       dabt_handler        ; 0x10 Data Abort
rsvdvec:
  B       rsvdvec             ; 0x14 reserved
irqvec:
  B       irq_handler         ; 0x18 IRQ		
fiqvec:
  B       fiq_handler         ; 0x1C FIQ

  EXTERN  blinkLed

;-- Blink led 0 and trigger a software interrupt
undef_handler:
  MSR     CPSR_C, #ARM_MODE_SVC
  LDR     r0, =0
  LDR     r1, =blinkLed
  MOV     lr, pc
  BX      r1
  SWI     0

;-- Blink led 1 and trigger a prefetch abort
swi_handler:
  MSR     CPSR_C, #ARM_MODE_SVC
  LDR     r0, =1
  LDR     r1, =blinkLed
  MOV     lr, pc
  BX      r1
  B       0x01000000

;-- Blink led 2 and trigger a data abort
pabt_handler:
  MSR     CPSR_C, #ARM_MODE_SVC
  LDR     r0, =2
  LDR     r1, =blinkLed
  MOV     lr, pc
  BX      r1
  LDR     r0, =0x40000000
  STR     r0, [r0]

;-- Blink led 3 and trigger an irq
dabt_handler:
  MSR     CPSR_c, #ARM_MODE_SVC
  LDR     r0, =3
  LDR     r1, =blinkLed
  MOV     lr, pc
  BX      r1
  LDR     r0, =AT91C_BASE_AIC
  MOV     r1, #0x100
  STR     r1, [r0, #AIC_ISCR]

;-- Blink led 2 and trigger a fiq
irq_handler:
  LDR     lr, =AT91C_BASE_AIC
  LDR     r0, [lr, #AIC_IVR]
  STR     r0, [lr, #AIC_EOICR]
  MSR     CPSR_c, #ARM_MODE_SVC
  LDR     r0, =2
  LDR     r1, =blinkLed
  MOV     lr, pc
  BX      r1
  LDR     r0, =AT91C_BASE_AIC
  MOV     r1, #0x1
  STR     r1, [r0, #AIC_ISCR]

;-- Blink led 1 and trigger a undefined instruction abort
fiq_handler:
  LDR     r0, [r8, #AIC_FVR]
  STR     r0, [r8, #AIC_EOICR]
  MSR     CPSR_c, #ARM_MODE_SVC
  LDR     r0, =1
  LDR     r1, =blinkLed
  MOV     lr, pc
  BX      r1
  DC32    0xEEEEEEEE

;-----------------------------------
; ICODE segment

  RSEG    ICODE:CODE(2)
  CODE32

;-----------------------------------
; Chip initialization
initReset:

;-- Retrieve end of RAM address to allocate stacks
__iramend EQU SFB(INTRAMEND_REMAP)

;-- Additional low-level initialization (in case the one in the bootloader
;-- has a bug)
;  EXTERN   AT91F_LowLevelInit

;  LDR    sp, =__iramend              
;  LDR	  r0, =AT91F_LowLevelInit
;  MOV    lr, pc
;  BX	  r0

;-- Constants
IRQ_STACK_SIZE          EQU     (3*8*4)
ARM_MODE_FIQ            EQU     0x11
ARM_MODE_IRQ            EQU     0x12
ARM_MODE_SVC            EQU     0x13
I_BIT                   EQU     0x80
F_BIT                   EQU     0x40

;-- Modes setup (stacks, ...)
  LDR     r0, =__iramend

;-- FIQ mode (put AIC address in r8)
  MSR     CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
  LDR     r8, =AT91C_BASE_AIC

;-- IRQ mode (stack)
  MSR     CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT
  MOV     sp, r0
  SUB     r0, r0, #IRQ_STACK_SIZE

;-- SVC mode, IT disabled (copy vectors in RAM, remap, stack)
  MSR     CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT
  MOV     sp, r0

;-- copy ICODE segment in RAM
__intVecStart EQU SFB(INTVEC)
__intVecEnd   EQU SFE(INTVEC)
__iramStart   EQU SFB(INTRAMSTART)

  LDR     r0, =__intVecStart
  LDR     r1, =__iramStart
  LDR     r2, =__intVecEnd

copy:
  LDR     r3, [r0], #4
  STR     r3, [r1], #4
  CMP     r0, r2
  BNE     copy

;-- Remap SRAM to have new exception vectors at address 0
  LDR     r0, =AT91C_BASE_MC
  LDR     r1, =AT91C_MC_RCB
  STR     r1, [r0, #MC_RCR]

;-- Unmask interrupts
  MSR     CPSR_c, #ARM_MODE_SVC

;-- Initialize C (CTR0)
  EXTERN  __segment_init

  LDR     r0, =__segment_init
  MOV     lr, pc
  BX      r0

;-- Branch on C main
  EXTERN	main
  PUBLIC	__main

?jump_to_main:
  LDR     lr, =?call_exit
  LDR     r0, =main

__main:
  BX      r0

;-- End of code, loop for ever
?call_exit:
end:
  B       end

  ENDMOD
  END