;******************** (C) COPYRIGHT 2003 STMicroelectronics ********************
;* File Name          : 71x_vect.s
;* Author             : MCD Application Team
;* Date First Issued  : 16/05/2003
;* Description        : This file used to initialize the exception and IRQ
;*                      vectors, and to enter/return to/from exceptions handlers.
;*******************************************************************************
;* History:
;*  16/05/03 : Creation.
;*******************************************************************************
	IMPORT  OSRunning		; External references
	IMPORT  OSPrioCur
	IMPORT  OSPrioHighRdy
	IMPORT  OSTCBCur
	IMPORT  OSTCBHighRdy
	IMPORT  OSIntNesting
	IMPORT  OSIntExit
	IMPORT  OSTaskSwHook


	PRESERVE8
	AREA Vect, CODE, READONLY
EIC_base_addr	EQU	0xFFFFF800	; EIC base address.
CICR_off_addr	EQU	0x04		; Current Interrupt Channel Register.
IVR_off_addr	EQU	0x18		; Interrupt Vector Register.
IPR_off_addr	EQU	0x40		; Interrupt Pending Register.

NO_INT		EQU	0xC0		; Mask used to disable interrupts (Both FIR and IRQ)
MODE_SYS	EQU	0x1F		; available on ARM Arch 4 and later
MODE_FIQ	EQU	0x11
MODE_IRQ	EQU	0x12

;I_Bit		EQU	0x80		; when I bit is set, IRQ is disabled
;F_Bit		EQU	0x40		; when F bit is set, FIQ is disabled

ENTRY

;*******************************************************************************
;              Import  the Reset_Handler address from 71x_init.s
;*******************************************************************************

        IMPORT  Reset_Handler

;*******************************************************************************
;                      Import exception handlers
;*******************************************************************************

        IMPORT  Undefined_Handler
        IMPORT  SWI_Handler
        IMPORT  Prefetch_Handler
        IMPORT  Abort_Handler
        IMPORT  FIQ_Handler

;*******************************************************************************
;                   Import IRQ handlers from 71x_it.c
;*******************************************************************************

        IMPORT  T0TIMI_IRQHandler
        IMPORT  FLASH_IRQHandler
        IMPORT  RCCU_IRQHandler
        IMPORT  RTC_IRQHandler
        IMPORT  WDG_IRQHandler
        IMPORT  XTI_IRQHandler
        IMPORT  USBHP_IRQHandler
        IMPORT  I2C0ITERR_IRQHandler
        IMPORT  I2C1ITERR_IRQHandler
        IMPORT  UART0_IRQHandler
        IMPORT  UART1_IRQHandler
        IMPORT  UART2_IRQHandler
        IMPORT  UART3_IRQHandler
        IMPORT  BSPI0_IRQHandler
        IMPORT  BSPI1_IRQHandler
        IMPORT  I2C0_IRQHandler
        IMPORT  I2C1_IRQHandler
        IMPORT  CAN_IRQHandler
        IMPORT  ADC12_IRQHandler
        IMPORT  T1TIMI_IRQHandler
        IMPORT  T2TIMI_IRQHandler
        IMPORT  T3TIMI_IRQHandler
        IMPORT  HDLC_IRQHandler
        IMPORT  USBLP_IRQHandler
        IMPORT  T0TOI_IRQHandler
        IMPORT  T0OC1_IRQHandler
        IMPORT  T0OC2_IRQHandler

;*******************************************************************************
;            Export Peripherals IRQ handlers table address
;*******************************************************************************

        EXPORT  T0TIMI_Addr
        EXPORT  IRQHandler

;*******************************************************************************
;
;*******************************************************************************
        EXPORT  vector_begin
        EXPORT  vector_end


vector_begin

;*******************************************************************************
;                        Exception vectors
;*******************************************************************************

        LDR     PC, Reset_Addr
        LDR     PC, Undefined_Addr
        LDR     PC, SWI_Addr
        LDR     PC, Prefetch_Addr
        LDR     PC, Abort_Addr
        NOP                             ; Reserved vector
        LDR     PC, IRQ_Addr
        LDR     PC, FIQ_Addr

;*******************************************************************************
;               Exception handlers address table
;*******************************************************************************

Reset_Addr      DCD     Reset_Handler
Undefined_Addr  DCD     UndefinedHandler
SWI_Addr        DCD     SWIHandler
Prefetch_Addr   DCD     PrefetchAbortHandler
Abort_Addr      DCD     DataAbortHandler
                DCD     0               ; Reserved vector
IRQ_Addr        DCD     IRQHandler
FIQ_Addr        DCD     FIQHandler

;*******************************************************************************
;              Peripherals IRQ handlers address table
;*******************************************************************************

T0TIMI_Addr     DCD  T0TIMIIRQHandler
FLASH_Addr      DCD  FLASHIRQHandler
RCCU_Addr       DCD  RCCUIRQHandler
RTC_Addr        DCD  RTCIRQHandler
WDG_Addr        DCD  WDGIRQHandler
XTI_Addr        DCD  XTIIRQHandler
USBHP_Addr      DCD  USBHPIRQHandler
I2C0ITERR_Addr  DCD  I2C0ITERRIRQHandler
I2C1ITERR_ADDR  DCD  I2C1ITERRIRQHandler
UART0_Addr      DCD  UART0IRQHandler
UART1_Addr      DCD  UART1IRQHandler
UART2_ADDR      DCD  UART2IRQHandler
UART3_ADDR      DCD  UART3IRQHandler
BSPI0_ADDR      DCD  BSPI0IRQHandler
BSPI1_Addr      DCD  BSPI1IRQHandler
I2C0_Addr       DCD  I2C0IRQHandler
I2C1_Addr       DCD  I2C1IRQHandler
CAN_Addr        DCD  CANIRQHandler
ADC12_Addr      DCD  ADC12IRQHandler
T1TIMI_Addr     DCD  T1TIMIIRQHandler
T2TIMI_Addr     DCD  T2TIMIIRQHandler
T3TIMI_Addr     DCD  T3TIMIIRQHandler
                DCD  0                  ; reserved
                DCD  0                  ; reserved
                DCD  0                  ; reserved
HDLC_Addr       DCD  HDLCIRQHandler
USBLP_Addr      DCD  USBLPIRQHandler
                DCD  0                  ; reserved
                DCD  0                  ; reserved
T0TOI_Addr      DCD  T0TOIIRQHandler
T0OC1_Addr      DCD  T0OC1IRQHandler
T0OC2_Addr      DCD  T0OC2IRQHandler

vector_end
;*******************************************************************************
;                         Exception Handlers
;*******************************************************************************

;*******************************************************************************
;* Macro Name     : SaveContext
;* Description    : This macro used to save the context before entering
;                   an exception handler.
;* Input          : The range of registers to store.
;* Output         : none
;*******************************************************************************
        MACRO
        SaveContext $reg1,$reg2
        STMFD  sp!,{$reg1-$reg2,lr} ; Save The workspace plus the current return
                              ; address lr_ mode into the stack.
        MRS    r1,spsr        ; Save the spsr_mode into r1.
        STMFD  sp!,{r1}       ; Save spsr.
        MEND
;*******************************************************************************
;* Macro Name     : RestoreContext
;* Description    : This macro used to restore the context to return from
;                   an exception handler and continue the program execution.
;* Input          : The range of registers to restore.
;* Output         : none
;*******************************************************************************
        MACRO
        RestoreContext $reg1,$reg2
        LDMFD   sp!,{r1}        ; Restore the saved spsr_mode into r1.
        MSR     spsr_cxsf,r1    ; Restore spsr_mode.
        LDMFD   sp!,{$reg1-$reg2,pc}^; Return to the instruction following...
                                ; ...the exception interrupt.
        MEND
;*******************************************************************************
;* Function Name  : UndefinedHandler
;* Description    : This function called when undefined instruction
;                   exception is entered.
;* Input          : none
;* Output         : none
;*******************************************************************************
UndefinedHandler
        SaveContext r0,r12    ; Save the workspace plus the current
                              ; return address lr_ und and spsr_und.
        BL      Undefined_Handler; Branch to Undefined_Handler.
        RestoreContext r0,r12 ; Return to the instruction following...
                              ; ...the undefined instruction.
;*******************************************************************************
;* Function Name  : SWIHandler
;* Description    : This function called when SWI instruction executed.
;* Input          : none
;* Output         : none
;*******************************************************************************
SWIHandler
        SaveContext r0,r12    ; Save the workspace plus the current
                              ; return address lr_ svc and spsr_svc.
        BL       SWI_Handler  ; Branch to SWI_Handler.
        RestoreContext r0,r12 ; Return to the instruction following...
                              ; ...the SWI instruction.
;*******************************************************************************
;* Function Name  : IRQHandler
;* Description    : This function called when IRQ exception is entered.
;* Input          : none
;* Output         : none
;*******************************************************************************
IRQHandler
;	SUB	lr,lr,#4			; Update the link register
;	SaveContext r0,r12			; Save the workspace plus the current of return address lr_ irq and spsr_irq.
;	MSR	CPSR_c,#(NO_INT | MODE_SYS)	; Change to SYS mode
	STMFD   SP!,{R1-R3}			; Push working registers onto IRQ stack
	MOV     R1,SP				; Save   IRQ stack pointer
	ADD     SP,SP,#12			; Adjust IRQ stack pointer 
	SUB     R2,LR,#4			; Adjust PC for return address to task
	MRS     R3,SPSR				; Copy SPSR (i.e. interrupted task's CPSR) to R3
	MSR     CPSR_c,#(NO_INT | MODE_SYS)	; Change to SYS mode
; Save task's context onto task's stack
	STMFD   SP!,{R2}			;    Push task's Return PC 
	STMFD   SP!,{R4-R12,LR}			;    Push task's LR,R12-R4

	LDMFD   R1!,{R4-R6}
	STMFD   SP!,{R4-R6}			;    Move task's R1-R3 from IRQ stack to SYS stack
	STMFD   SP!,{R0}			;    Push task's R0    onto task's stack
	STMFD   SP!,{R3}			;    Push task's CPSR (i.e. IRQ's SPSR)

; Handle nesting counter
	LDR	R0,_OS_IntNesting		; OSIntNesting++;
	LDRB	R1,[R0]
	ADD	R1,R1,#1