;********************************************************************************************************
;						uC/OS-II
;					The Real-Time Kernel
;
;				(c) Copyright 1992-2004, STMicroelectronics
;					    All Rights Reserved
;
;					uC/OS Generic Port for STR710
;						C Compiler
;
; File : OS_CPU_A.S
; By   : Yujun Huang
;********************************************************************************************************

	IMPORT  OSRunning		; External references
	IMPORT  OSPrioCur
	IMPORT  OSPrioHighRdy
	IMPORT  OSTCBCur
	IMPORT  OSTCBHighRdy
	IMPORT  OSIntNesting
	IMPORT  OSIntExit
	IMPORT  OSTaskSwHook

	EXPORT  OS_CPU_ICR_Save		; Functions declared in this file
	EXPORT  OS_CPU_ICR_Restore
	EXPORT  OSStartHighRdy
	EXPORT  OSCtxSw
	EXPORT  OSIntCtxSw

	PRESERVE8
	AREA OS_CPU, CODE, READONLY

EIC_ICR		EQU	0xFFFFF800

;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

CLOSE_INT	EQU	0x03	; Mask used to disable interrupts using STR7 register
I_Bit		EQU	0x80	; when I bit is set, IRQ is disabled
F_Bit		EQU	0x40	; when F bit is set, FIQ is disabled

;*********************************************************************************************************
;				CRITICAL SECTION METHOD 3 FUNCTIONS
;
; Description: Disable/Enable interrupts by preserving the state of interrupts.  Generally speaking you
;              would store the state of the interrupt disable flag in the local variable 'cpu_sr' and then
;              disable interrupts.  'cpu_sr' is allocated in all of uC/OS-II's functions that need to 
;              disable interrupts.  You would restore the interrupt disable state by copying back 'cpu_sr'
;              into the CPU's status register.
;
; Prototypes :     OS_CPU_SR  OS_CPU_SR_Save(void);
;                  void       OS_CPU_SR_Restore(OS_CPU_SR cpu_sr);
;
;
; Note(s)    : 1) These functions are used in general like this:
;
;                 void Task (void *pdata)
;                 {
;                 #if OS_CRITICAL_METHOD == 3          // Allocate storage for CPU status register
;                     OS_CPU_SR  cpu_sr;
;                 #endif
;
;                          :
;                          :
;                     OS_ENTER_CRITICAL();             // cpu_sr = OS_CPU_SaveSR();
;                          :
;                          :
;                     OS_EXIT_CRITICAL();              // OS_CPU_RestoreSR(cpu_sr);
;                          :
;                          :
;                 }
;
;              2) OS_CPU_SaveSR() is implemented as recommended by Atmel's application note:
;
;                    "Disabling Interrupts at Processor Level"
;*********************************************************************************************************
OS_CPU_ICR_Save
	LDR	R1,=EIC_ICR		; Get the address of Interrupt Control Register
	LDR	R0,[R1]			; Resd the register to R0
	ORR	r2,r0,#CLOSE_INT	; Disable the interrupt
	STR	r2,[R1]			; store the data to register
	LDR	r2,[R1]			; Confirm that Register contains the proper interrupt disable flags
	AND	r2,r2,#CLOSE_INT
	CMP	r2,#CLOSE_INT
	BNE	OS_CPU_ICR_Save		; Not properly disabled (try again)
	MOV	pc,lr

OS_CPU_ICR_Restore
        LDR	R1,=EIC_ICR
	STR	R0,[R1]			; store the data to register
	MOV	pc,lr

;*********************************************************************************************************
;                                          START MULTITASKING
;                                       void OSStartHighRdy(void)
;
; Note(s) : 1) OSStartHighRdy() MUST:
;              a) Call OSTaskSwHook() then,
;              b) Set OSRunning to TRUE,
;              c) Switch to the highest priority task.
;*********************************************************************************************************
	EXPORT 	OSCtxSw
	IMPORT	OSPrioCur
	IMPORT	OSPrioHighRdy
	IMPORT	OSTCBCur
	IMPORT	OSTaskSwHook
	IMPORT	OSTCBHighRdy

OSStartHighRdy
	BL	OSTaskSwHook		; OSTaskSwHook();

	LDR	R4,_OS_Running
	MOV	R5,#1
	STRB	R5,[R4]			; OSRunning --> TRUE

					; Switch to highest priority task
	LDR	R4,_OS_TCBHighRdy	;    Get highest priority task TCB address
	LDR	R4,[R4]			;    get stack pointer
	LDR	SP,[R4]			;    switch to the new stack

	LDMFD	SP!,{R4}		;    pop new task's CPSR
	MSR	CPSR_cxsf,R4
	LDMFD	SP!,{R0-R12,LR,PC}	;    pop new task's R0-R12,LR & PC

;*********************************************************************************************************
;				PERFORM A CONTEXT SWITCH (From task level) - OSCtxSw()
;
; Note(s) : 1) OSCtxSw() is called in SYS mode with BOTH FIQ and IRQ interrupts DISABLED
;
;           2) The pseudo-code for OSCtxSw() is:
;              a) Save the current task's context onto the current task's stack
;              b) OSTCBCur->OSTCBStkPtr = SP;
;              c) OSTaskSwHook();
;              d) OSPrioCur             = OSPrioHighRdy;
;              e) OSTCBCur              = OSTCBHighRdy;
;              f) SP                    = OSTCBHighRdy->OSTCBStkPtr;
;              g) Restore the new task's context from the new task's stack
;              h) Return to new task's code
;
;           3) Upon entry: 
;              OSTCBCur      points to the OS_TCB of the task to suspend
;              OSTCBHighRdy  points to the OS_TCB of the task to resume
;*********************************************************************************************************
OSCtxSw
					; Save current task's context
	STMFD	SP!,{LR}		;    push PC (lr should be pushed in place of PC)
	STMFD	SP!,{R0-R12,LR}		;    push LR & register file
	MRS	R4,CPSR
	STMFD	SP!,{R4}		;    push current CPSR

	LDR	R4,_OS_TCBCur		; OSTCBCur->OSTCBStkPtr = SP;
	LDR	R5,[R4]
	STR	SP,[R5]

	BL	OSTaskSwHook		; OSTaskSwHook();

	LDR	R4,_OS_PrioCur
	LDR	R5,_OS_PrioHighRdy
	LDRB	R6,[R5]
	STRB	R6,[R4]			; OSPrioCur <-- OSPrioHighRdy
	
	LDR	R4,_OS_TCBCur
	LDR	R6,_OS_TCBHighRdy
	LDR	R6,[R6]
	STR	R6,[R4]			; OSTCBCur <-- OSTCBHighRdy;

	LDR	SP,[R6]			; SP = OSTCBHighRdy->OSTCBStkPtr;

					; Restore new task's context
	LDMFD	SP!,{R4}		;    pop new task's CPSR
	MSR	CPSR_cxsf,r4
	LDMFD	SP!,{R0-R12,LR,PC}	;    pop new task's R0-R12,LR & PC

;*********************************************************************************************************
;			PERFORM A CONTEXT SWITCH (From interrupt level) - OSIntCtxSw()
;
; Note(s) : 1) OSIntCtxSw() is called in SYS mode with BOTH FIQ and IRQ interrupts DISABLED
;
;           2) The pseudo-code for OSCtxSw() is:
;              a) OSTaskSwHook();
;              b) OSPrioCur             = OSPrioHighRdy;
;              c) OSTCBCur              = OSTCBHighRdy;
;              d) SP                    = OSTCBHighRdy->OSTCBStkPtr;
;              e) Restore the new task's context from the new task's stack
;              f) Return to new task's code
;
;           3) Upon entry: 
;              OSTCBCur      points to the OS_TCB of the task to suspend
;              OSTCBHighRdy  points to the OS_TCB of the task to resume
;*********************************************************************************************************
OSIntCtxSw
	BL	OSTaskSwHook		; OSTaskSwHook();

	LDR	R4,_OS_PrioCur		; OSPrioCur = OSPrioHighRdy
	LDR	R5,_OS_PrioHighRdy
	LDRB	R6,[R5]
	STRB	R6,[R4]

	LDR	R4,_OS_TCBCur		; OSTCBCur  = OSTCBHighRdy;
	LDR	R6,_OS_TCBHighRdy
	LDR	R6,[R6]
	STR	R6,[R4]

	LDR	SP,[R6]			; SP = OSTCBHighRdy->OSTCBStkPtr;

					; Restore new task's context
	LDMFD	SP!,{R4}		;    pop new task's CPSR
	MSR	CPSR_cxsf,r4
	LDMFD	SP!,{R0-R12,LR,PC}	;    pop new task's R0-R12,LR & PC

;*********************************************************************************************************
;				FIQ Interrupt Service Routine
;*********************************************************************************************************
OS_CPU_FIQ_ISR


;*********************************************************************************************************
;				POINTERS TO VARIABLES
;*********************************************************************************************************
_OS_IntNesting	DCD	OSIntNesting

_OS_PrioCur	DCD	OSPrioCur

_OS_PrioHighRdy	DCD	OSPrioHighRdy

_OS_Running	DCD	OSRunning

_OS_TCBCur	DCD	OSTCBCur

_OS_TCBHighRdy	DCD	OSTCBHighRdy

	END