;********************************************************************************************************
;                                               uC/OS-II
;                                         The Real-Time Kernel
;
;                        (c) Copyright 1992-1998, Jean J. Labrosse, Plantation, FL
;                                          All Rights Reserved
;
;
;                                           ARM Specific code
;
; File : OS_CPU_A.s
; By   : N. Douglas (3rd April 2000)
;********************************************************************************************************

	AREA	OS_CPU_A, CODE, READONLY

	EXPORT	OSStartHighRdy
	EXPORT	OSCtxSw
	EXPORT	OSIntCtxSw
	EXPORT	OSTaskSw
	EXPORT	OSStackAndDisableInts
	EXPORT	OSRestoreStackedIntState

	IMPORT	OSTaskSwHook
	IMPORT	OSRunning
	IMPORT	OSTCBHighRdy
	IMPORT	OSTCBCur
	IMPORT	OSPrioHighRdy
	IMPORT	OSPrioCur

; Handy SWI definitions
OS_EnterOS	EQU	0x16

	[ 1=0
; Handy register dump macro (NEEDS NEDHAL)
	IMPORT	DbgIO_WriteF
	MACRO
	regdump		$txt, $src
	; R0=reg list (R14_usr, R13_usr, R12-R0, PC_usr, SPSR_irq), $txt=msg to print
	; corrupts R0-R5
	MOV	R4,$src
	B	%f03
1
	DCB	"$txt"
	DCB	" using task 0x%x",10,0
	ALIGN
2
	DCB	"R%d=0x%x",10,0
	ALIGN
3	ADR	R0,%b01
	LDR	R1,=OSTCBCur
	LDR	R1,[R1]
	BL	DbgIO_WriteF
	MVN	R5,#1
4	ADR	R0,%b02
	MOV	R1,R5
	LDMFD	R4!,{R2}
	BL	DbgIO_WriteF
	ADD	R5,R5,#1
	CMP	R5,#14
	BLE	%b04
	MEND
	]

;*********************************************************************************************************
;                                          START MULTITASKING
;                                       void OSStartHighRdy(void)
;
; Assumes context is saved as:
; First R14_usr, R13_usr, R12-R0
; Then PC_usr and SPSR_irq (ie; CPSR_usr)
;*********************************************************************************************************

OSStartHighRdy
	; Need to be in a privilaged mode first
	SWI	OS_EnterOS

	; Call OSTaskSwHook()
	BL	OSTaskSwHook

	; Set OSRunning to TRUE
	MOV	R0,#1
	LDR	R1,=OSRunning
	STRB	R0,[R1]
	

	; Get stack ptr for task to resume (lives in OSTCBHighRdy->OSTCBStkPtr)
	LDR	R1,=OSTCBHighRdy
	LDR	R1,[R1]
	LDR	R0,[R1,#0]				; OSTCBStkPtr is at +0
	;regdump	"Restoring context (OSStartHighRdy)", R0

	; Restore saved context & return to task
	LDMFD	R0!,{R1,R14}				; SPSR & return addr
	MSR	SPSR_all,R1
	LDMFD	R0,{R0-R14}^				; Restore USR mode regs R0-R14
	NOP
	MOVS	PC,R14					; PC=ret addr & CPSR=SPSR


;*********************************************************************************************************
;                                PERFORM A CONTEXT SWITCH (From task level)
;                                           void OSCtxSw(void)
;
;*********************************************************************************************************

OSCtxSw
	STMFD	R13!,{R14}				; Stack return addr

	; Save current task context
	STMDB	R13,{R13}^				; Determine R13_usr
	LDMDB	R13,{R14}
	STMFD	R14,{R0-R14}^				; Stack USR mode regs R0-R14
	MRS	R1, SPSR
	SUB	R0,R14,#15*4
	LDMFD	R13!,{R14}
	STMFD	R0!,{R1,R14}				; Stack SPSR & return addr

	; Store stack ptr of task (lives in OSTCBCur->OSTCBStkPtr)
	LDR	R1,=OSTCBCur
	LDR	R1,[R1]
	STR	R0,[R1,#0]				; OSTCBStkPtr is at +0
	; regdump	"Saving context (OSCtxSw)", R0
	
	; Call user definable OSTaskSwHook()
	BL	OSTaskSwHook

	; Set current task to new task
	LDR	R2,=OSTCBHighRdy
	LDR	R3,=OSTCBCur
	LDR	R4,=OSPrioHighRdy
	LDR	R5,=OSPrioCur
	LDR	R0,[R2]
	LDRB	R1,[R4]
	STR	R0,[R3]
	STRB	R1,[R5]

	; Get stack ptr for task to resume (lives in OSTCBHighRdy->OSTCBStkPtr)
	LDR	R1,=OSTCBHighRdy
	LDR	R1,[R1]
	LDR	R0,[R1,#0]				; OSTCBStkPtr is at +0
	; regdump	"Restoring context (OSCtxSw)", R0

	; Restore saved context & return to task
	LDMFD	R0!,{R1,R14}				; SPSR & return addr
	MSR	SPSR_all,R1
	LDMFD	R0,{R0-R14}^				; Restore USR mode regs R0-R14
	NOP
	MOVS	PC,R14					; PC=ret addr & CPSR=SPSR


;*********************************************************************************************************
;                                PERFORM A CONTEXT SWITCH (From an ISR)
;                                        void OSIntCtxSw(void)
;
;*********************************************************************************************************

OSIntCtxSw
	; Remove all registers on IRQ stack so far
	; This means the order in which things were called before calling this
	; In our case, this means OSIntExit, the ISR routine and then anything which
	; called the ISR (eg; a HAL like NedHAL)
	; Regs stacked WILL BE DIFFERENT DEPENDING ON COMPILER VERSION

	; Remove OSIntExit() stacked regs
	; v2.5: {r14}
	; v2.0: {r11,r12,r14,pc} 
	;ADD	R13,R13,#4		; v2.5
	ADD	R13,R13,#4*4

	; Remove ISR() stacked regs (KEEP SIMILAR)
	; v2.5: {r4,r14}
	; v2.0: {r0,r4,r11,r12,r14,pc}
	;LDMFD	R13!,{R4,R14}		; v2.5
	ADD	R13,R13,#4*6

	; Remove HAL stacked regs (KEEP SIMILAR)
	; {spsr,r0-r12,r14}
	LDMFD	R13!,{R0}
	MSR	SPSR,R0
	LDMFD	R13!,{R0-R12,R14}

	; Perform normal task swap
	B	OSCtxSw


;*********************************************************************************************************
;                                        GENERATE SOFT INTERRUPT
;                                          void OSTaskSw(void)
;
;*********************************************************************************************************

OSTaskSw
	IMPORT	NedHAL_ISR

	MOV	R2,R14
	MRS	R1,CPSR					; Take copy of current PSR
	SWI	OS_EnterOS				; enter SVC mode
	BIC	R0,R1,#0x1f
	ORR	R0,R0,#0x92				; IRQ32 + interrupts off
	MSR	CPSR_all,R0
	NOP
	MSR	SPSR_all,R1				; SPSR=old PSR
	ADD	R14,R2,#4				; R14_irq=return addr
	; Now in a state just like as if at 0x18
	B	OSCtxSw


;*********************************************************************************************************
;                                  STACK IRQ STATE AND DISABLE INTERRUPTS
;                                    void OSStackAndDisableInts(void)
;
; NOTE: Be careful calling from C code in SVC
;*********************************************************************************************************

OSStackAndDisableInts
	; Corrupts R0-R2 (okay during APCS use)
	MOV	R2,R14					; remember r14 (might be usr, irq, svc etc)
	SWI	OS_EnterOS				; enter SVC mode
	MRS	R0,SPSR					; disable interrupts in the SPSR
	ORR	R1,R0,#(1<<7)
	MSR	SPSR_all,R1
	STMFD	R13!,{R0}				; stack the old SPSR on SVC stack
	MOVS	PC,R2					; CPSR=SPSR & PC=ret addr


;*********************************************************************************************************
;                                  STACK IRQ STATE AND DISABLE INTERRUPTS
;                                    void OSRestoreStackedIntState(void)
;
; NOTE: Be careful calling from C code in SVC
;*********************************************************************************************************

OSRestoreStackedIntState
	; Corrupts R0,R1 (okay during APCS use)
	MOV	R1,R14					; remember r14
	SWI	OS_EnterOS				; enter SVC mode
	LDMFD	R13!,{R0}				; load the old SPSR off SVC stack
	MSR	SPSR_all,R0				; SPSR=what it did before
	MOVS	PC,R1					; CPSR=SPSR & PC=ret addr



	END