subr.s

uHAL for SA1110,INTEL公司出品

;*
;* File: subr.s
;*
;* uC/OS Real-time multitasking kernel for the ARM processor.
;* Low-level routines.
;*
;* Created by Marco Graziano ([e-mail]).
;*

;	Functions defined in this module:
;
;	void NewFIQ(PFV)		/* used in PIDInit() */
;	void NewSWI(PFV)		/* used in PIDInit() */
;	void SWITrap(void) 		/* SWI trap wrapper */
;	void IRQTrap(void)		/* IRQ trap wrapper */
;	void FIQTrap(void)		/* FIQ trap wrapper */
;	void OSDisableInt(void)		/* disable interrupts when in SVC */
;	void OSEnableInt(void)		/* enable interrupts when in SVC */
;	void OSCtxSw(void)		/* context switch */
;	void OSStartHighRdy(void)	/* start highest priority task */

SwiV		EQU	0x08
IrqV		EQU	0x18
FiqV		EQU	0x1C
NoInt		EQU	0x80

SVC32Mode	EQU	0x13
IRQ32Mode	EQU	0x12
FIQ32Mode	EQU	0x11

OSEnterSWI	EQU	0x00

	AREA	|subr|, CODE, READONLY

	; Improper use of locations within a READONLY area
SavedIRQ	DCD	0x0
SavedFIQ	DCD	0x0
SavedSWI	DCD	0x0

	; External symbols we need the addresses of
			IMPORT	OSTCBCur
addr_OSTCBCur		DCD	OSTCBCur
			IMPORT	OSTCBHighRdy
addr_OSTCBHighRdy	DCD	OSTCBHighRdy

;	void NewFIQ(PFV NewHandler)
;	PFV NewHandler - Address of a new high level handler.
;
;       Install a new FIQ trap wrapper in the ARM execptions vector.
;	*** Call with interrupts disabled.
;
	EXPORT	NewFIQ
NewFIQ
	STMFD	sp!, {v1-v2}			; save working registers
	STR	a1, SavedFIQ			; save new high-level handler
        ADR     v1, FIQTrap - 8 - FiqV          ; offset from branch location
        MOV     v1, v1, LSR #2                  ; turn into a word offset
        ORR     v1, v1, #0xEA000000             ; assemble a branch to the
        MOV     v2, #FiqV			; wrapper instruction
        STR     v1, [v2]                        ; store branch in vector
	LDMFD	sp!, {v1-v2}			; restore registers
	MOV	pc, lr				; return


;	void NewSWI(PFV NewHandler)
;	PFV NewHandler - Address of a new high level handler.
;
;       Install a new SWI trap wrapper in the ARM execptions vector.
;	*** Call with interrupts disabled.
;
	EXPORT	NewSWI
NewSWI
	STMFD	sp!, {v1-v2}			; save working registers
	STR	a1, SavedSWI			; save high-level handler
	ADR	v1, SWITrap - 8 - SwiV		; offset from branch
	MOV	v1, v1, LSR #2			; turn into words
	ORR	v1, v1, #0xEA000000		; build a branch to it
	MOV	v2, #SwiV			; replace vector
	STR	v1, [v2]			; store it
	LDMFD	sp!, {v1-v2}			; restore registers
	MOV	pc, lr				; return


;	SWITrap - SWI trap handler
;
;	Wrapper to service the trap and call a high-level handler.
;	We assume that the demon allocated a stack for SVC mode.
;	The trap is started with interrupts disabled.
;
SWITrap
        STMFD   sp!, {r0-r12, lr}       ; save registers
	LDR	a1, [lr, #-4]		; get swi instruction
	BIC	a1, a1, #0xFF000000	; extract swi number from instruction

	; A special SWI is used to enter uC/OS and switch to SVC mode
	TST	a1, #OSEnterSWI		; check if enter OS swi
	BNE	%F01			

	; The data-sheet states that no attempt will be made to write
	; an 8bit constants into the whole PSR.
	MRS	r0,CPSR
	BIC	r0,r0,#0xDF
	ORR	r0,r0,#(SVC32Mode | NoInt)
	MSR	CPSR,r0
	LDMFD	sp!, {r0-r12, pc}	; return staying in SVC mode

01					; re-enable interrupts if needed
	MRS	v1, SPSR		; get saved psr
	MRS	v2, CPSR		; get current psr
	AND	v1, v1, #NoInt		; extract interrupt level from old psr
	BIC	v2, v2, #NoInt		; clear I bit in current psr
	ORR	v2, v2, v1		; set current interrupt from saved psr
	MSR	CPSR, v2		; change current psr

	ADR	v1, SavedSWI
	MOV	lr, pc			; arrange for a safe return here	
	LDR	pc, [v1]		; call high-level handler passing swi#

        LDMFD   sp!, {r0-r12, pc}^      ; restore registers and return


;	IRQTrap - IRQ trap handler
;
;	Wrapper to service the trap and call a high-level handler.
;
	EXPORT IRQContext
IRQTrap
	; We take the cycle hit of modifying the return address here,
	; to simplify the code needed to return to the interrupted
	; thread, possibly performing a context switch on the way.
	SUB	lr,lr,#4

	; At this points IRQs are disabled, so we know that the SPSR
	; register will not be trashed by another interrupt.
        STMFD   sp!, {r0-r12, lr}       ; save registers
        MRS     v1, SPSR		; push SPSR to allow nested interrupts
        STMFD   sp!, {v1}

	ADR	v1, SavedIRQ
        MOV     lr, pc                  ; arrange for a safe return here
        LDR     pc, [v1]         	; call high-level handler
					; it will re-enable interrupts

	; The high-level handler dispatches the IRQ to the proper
	; user defined ISR and returns 1 if pre-emption is needed
	CMP	a1, #0			; a1 is TRUE if preemption

IRQContext
	; NOTE: The following code assumes that all threads use r13 as
	; the stack-pointer, and that it is a APCS conformant stack.
	; i.e. there is never any data stored beneath the current
	; stack-pointer.
	; The above needs to be true to enable the context switches
	; started as a return from an interrupt to use the current
	; threads stack as the state save area.
	;
	LDMFD	sp!,{r12}		; recover SPSR value from stack
	; if NE then we need to check if we were a nested interrupt
	AND	r11,r12,#0x1F		; mask out all but the mode bits
	TEQNE	r11,#IRQ32Mode		; check for interrupted IRQ thread
	; if EQ then we can return immediately
	MSREQ	SPSR,r12		; restore the SPSR
	LDMEQFD	sp!,{r0-r12,pc}^	; and return to the interrupted thread

	; We now need to perform a context switch.
	; r12 = SPSR describing the interrupted thread.
	; r11 = interrupted thread processor mode

	; We need to protect the SPSR before we actually perform the
	; return to the interrupted thread, since we don't want to
	; lose the value by another interrupt occuring between the
	; SPSR load and the PC+CPSR load. Similarly we need to protect
	; the IRQ stack and threading code while we setup the state
	; required to enter the context switch code from an interrupt
	; routine. We rely on the interrupted thread having IRQs
	; enabled (since we would never have reached this point
	; otherwise).
	; We have recovered the SPSR value, so only r0-r12,lr are on the stack.

	LDR	r4,[sp,#(13 * 4)]	; load return address: nasty use of
					; a constant

	; r11 contains the mode bits describing the interrupted thread
	MRS	r0,CPSR			; get current mode info.
	ORR	r0,r0,#0x80		; and set IRQ disable flag
	BIC	r1,r0,#0x1F		; clear mode bits
	ORR	r1,r1,r11		; insert interrupted thread mode bits
	MSR	CPSR,r1		; and change to that mode

	; We are now in the interrupted thread mode with IRQs
	; disabled.
	MOV	r3,lr			; copy the current lr
	MRS	r1,SPSR			; copy current SPSR
	MRS	r2,CPSR			; copy current CPSR
	STMFD	sp!,{r1,r2,r3,r4}	; and construct return stack
	MSR	CPSR,r0		; return to IRQ mode
	; IRQ mode; IRQs disabled
	; r12 = SPSR describing interrupted thread
	MSR	SPSR,r12		; restore SPSR_irq ready for return
	LDMFD	sp!,{r0-r12,lr}		; restore all the registers
	SUBS	pc,pc,#0		; and return to the interrupted mode
	NOP				; flush the pipeline
	NOP
	NOP

	; we are now executing in the interrupted mode with IRQs enabled
	BL	OSCtxSw			; perform the context switch
	LDMFD	sp!,{lr}
	MSR	SPSR,lr		; recover the SPSR when the thread 
					; was interrupted
	LDMFD	sp!,{lr}
	MSR	CPSR,lr		; recover the CPSR when the thread 
					; was interrupted
	LDMFD	sp!,{lr,pc}		; return to the interrupted thread


;	FIQTrap - FIQ trap handler
;
;	Wrapper to service the trap and call a high-level handler.
;	WARNING: NEEDS CHANGES TO ALLOW CONTEXT SWITCH AFTER THE INTERRUPT
;		 LOOK AT IRQTrap() to figure out what to do. (MEG)
;
FIQTrap
        STMFD   sp!, {r0-r7, lr}        ; save registers
        MRS     v1, SPSR		; push SPSR to allow nested interrupts
        STMFD   sp!, {v1}

	ADR	v1, SavedFIQ
        MOV     lr, pc                  ; arrange for a safe return here
        LDR     pc, [v1]         	; call high-level handler

        LDMFD   sp!, {v1}		; pop SPSR
        MSR     SPSR, v1
        LDMFD   sp!, {r0-r7, lr}        ; restore registers
        SUBS    pc, lr, #4              ; return from trap
 

;	void OSDisableInt(void)
;	void OSEnableInt(void)
;
;	Disable and enable IRQ and FIQ preserving current CPU mode.
;
	EXPORT	OSDisableInt
OSDisableInt
	mrs	r12, CPSR
	orr	r12, r12, #NoInt
	msr	CPSR_c, r12
	mov	pc, lr

	EXPORT	OSEnableInt
OSEnableInt
	mrs	r12, CPSR
	bic	r12, r12, #NoInt
	msr	CPSR_c, r12
	mov	pc, lr



;	void OSCtxSw(void)
;	
;	Perform a context switch.
;
	; The following code assumes that the virtual memory is directly
	; mapped into  physical memory. If this is not true, the cache must 
	; be flushed at context switch to avoid address aliasing.

	EXPORT	OSCtxSw
OSCtxSw
	STMFD	sp!, {r0-r12, lr}	; save register file and ret address
	MRS	v1, CPSR
	STMFD	sp!, {v1}		; save current PSR
	MRS	v1, SPSR		; YYY+
	STMFD	sp!, {v1}		; YYY+ save SPSR

	; Get current task TCB address
	LDR	v1, addr_OSTCBCur
	LDR	v2, [v1]
	STR	sp, [v2]		; store sp in preempted tasks's TCB

	; Get highest priority task TCB address
	LDR	v3, addr_OSTCBHighRdy
	LDR	v3, [v3]
	LDR	sp, [v3]		; get new task's stack pointer

	STR	v3, [v1]		; set new current task TCB address

	LDMFD	sp!, {v1}		; YYY+
	MSR	SPSR, v1		; YYY+
	LDMFD	sp!, {v1}		; YYY+
	MSR	CPSR, v1		; YYY+
	LDMFD	sp!, {r0-r12, pc}	; YYY+


;	void OSStartHighRdy(void)
;	
;	Start the task with the highest priority;
;
	EXPORT	OSStartHighRdy
OSStartHighRdy
	; Get current task TCB address
	LDR	v1, addr_OSTCBCur
	; Get highest priority task TCB address
	LDR	v2, addr_OSTCBHighRdy
	LDR	v2, [v2]		; get stack pointer
	LDR	sp, [v2]		; switch to the new stack

	STR	v2, [v1]		; set new current task TCB address

	LDMFD	sp!, {v1}		; YYY
	LDMFD	sp!, {v1}		; get new state from top of the stack
	MSR	CPSR, v1		; CPSR should be SVC32Mode
	LDMFD	sp!, {r0-r12, pc}	; start the new task

	END