ossubr.s

ucos on sa1200 from ixp1200 EB

;*
;* File: subr.s
;*
;* uC/OS Real-time multitasking kernel for the ARM processor.
;* Low-level routines.
;*
;* Created by Marco Graziano ([e-mail]).
;*
;* Dan White - 03/31/00 added sysCpuVerGet and check for Rev-b board of ixp1200
;* Dan White - 04/18/00 added sysCpuVerMake and check for Rev-b board of ixp1200
;* Dan White - 04/19/00 export ixp1200_SysCpuRev
;*
;* Dan White - 09/06/00 add/export ixp1200_SysCpuID
;*
;* Dan M. White - 10/13/00 - added SAr_PutCPSR for bug #1112 (cpsr not saved)
;*
;*
;*
			INCLUDE platform.s

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

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

	AREA	|C$$data$$subr|, NOINIT

    EXPORT IXP1200_SysCpuRev
IXP1200_SysCpuRev
    %       4       
    EXPORT IXP1200_SysCpuID
IXP1200_SysCpuID
    %       4       
IRQStackTop
    %   0x8000  ; 32k used for IRQ stack
IRQStackBottom
	%	4

	AREA	|C$$code$$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

;	SAr_IRQStackInit - Install IRQ stack
;
;   Stack has to be word boundary.
;
	EXPORT	SAr_IRQStackInit
SAr_IRQStackInit
    STMFD   sp!, {v1-v2, lr}
    MRS     v1, cpsr
    BIC     v2, v1, #ModeMask
    ORR     v2, v2, #(NoInt | IRQ32Mode)
    MSR     cpsr, v2

    LDR     v2, =IRQStackBottom
    BIC     v2, v2, #3  ; make it long word aligned
    MOV     sp, v2
    MSR     cpsr, v1
    LDMFD   sp!, {v1-v2, pc}


;	SAir_IRQTrapWrap - IRQ trap handler
;
;	Wrapper to service the trap and call a high-level handler.
;
    IMPORT  OSSched
    IMPORT  orignal_IRQ_Vector
    IMPORT  SA_IRQ_Dispatcher
    IMPORT  SAv_OsNotUseUART
    EXPORT  SAir_IRQTrapWrap

SAir_IRQTrapWrap
    ; At this point IRQs are disabled.
    SUB     lr, lr, #4
    STMFD   sp!, {r0-r12, lr}   ; save interrupted thread's registers
    MRS     r0, SPSR            ; push SPSR to allow nested interrupts
    STMFD   sp!, {r0}

    ; Check to see if OS manages the UART
    LDR     r0, =SAv_OsNotUseUART
    LDR     r0, [r0, #0]
    MOVS    r0, r0
    BEQ     %F1					; uCOS uses UART

    ; Angel has the exclusive use of the serial port.
    ; So if it is a serial interrupt, call Angel's IRQ ISR instead, which
    ; has been saved in orignal_IRQ_Vector.
    ;
    ; Check to see if we are Rev-a board
    LDR     r0, =IXP1200_SysCpuRev
    LDR     r0, [r0]
    CMP     r0, #0
    BNE     %F4 ; Rev-b

	LDR		r0, =IRQ        ; Rev >A
    LDR     r0, [r0, #0]
    ANDS    r0, r0, #I_UART_REV_A
    BEQ     %F1					; Not UART interrupt
    B       %F5

4	LDR		r0, =IRQ
    LDR     r0, [r0, #0]
    ANDS    r0, r0, #I_UART
    BEQ     %F1					; Not UART interrupt

5   LDR     r0, =UART_SR        ; Further check UART Status Register
    LDR     r0, [r0, #0]
    ANDS    r0, r0, #(UART_TX_FIFO_EMPTY + UART_RX_READY)
    BEQ     %F1					; Not UART interrupt

    ; It is a UART interrupt for Angel, restore everything.
    LDMFD   sp!, {r0}           ; pop SPSR
    MSR     SPSR, r0
    LDMFD   sp!, {r0-r12, lr}   ; restore registers
    ADD     lr, lr, #4

	; get Angel's ISR, which is saved in orignal_IRQ_Vector
    STMFD   sp!, {r0}
    STMFD   sp!, {r0}			; Save r0
    LDR     r0, =orignal_IRQ_Vector
    LDR     r0, [r0]
    STR     r0, [sp, #0x4]      ; Save in good order for LDMFD.
    LDMFD   sp!, {r0, pc}       ; Jump to Angel's ISR.

1
    BL      SA_IRQ_Dispatcher

    LDMFD   sp!, {r12}          ; recover SPSR value from stack

    ; 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

    ; if EQ then we can return immediately, non-preemption
    MSREQ   SPSR, r12           ; restore the SPSR
    LDMEQFD sp!, {r0-r12, pc}^  ; and return to the interrupted thread

	; if NE then we need to perform a context switch
    ; We have recovered the SPSR value, so only r0-r12,lr are on the stack
    LDR     r4, [sp,#(13 * 4)]  ; r4 = return address to interrupted thread

    ; change processor mode
	AND     r11, r12, #0x1F     ; r11 = interrupted thread processor mode
	MRS     r0, CPSR            ; get current mode info.
	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.
	STMFD   sp!, {r4}           ; and construct return stack
	MSR     CPSR, r0            ; return back 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
    STMFD   sp!, {r0-r12, lr}   ; save registers
    BL      OSSched             ; perform the context switch
	LDMFD   sp!, {r0-r12, lr, pc}   ; return to the interrupted thread


;	SAr_FIQStackInit - Install FIQ stack
;
;   Stack has to be word boundary.
;
	EXPORT	SAr_FIQStackInit
SAr_FIQStackInit
    STMFD   sp!, {v1-v2, lr}
    MRS     v1, cpsr
    BIC     v2, v1, #ModeMask
    ORR     v2, v2, #(NoInt | FIQ32Mode)
    MSR     cpsr, v2

    BIC     a1, a1, #3  ; make it long word aligned
    MOV     sp, a1
    MSR     cpsr, v1
    LDMFD   sp!, {v1-v2, pc}


;	SAir_FIQTrapWrap - FIQ trap handler
;
;	Wrapper to service the trap and call a high-level handler.
;
    IMPORT  SA_FIQ_Dispatcher
    EXPORT	SAir_FIQTrapWrap
SAir_FIQTrapWrap
    STMFD   sp!, {r0-r12, lr}   ; save registers
    MRS     v1, SPSR            ; push SPSR to allow nested interrupts
    STMFD   sp!, {v1}

    BL      SA_FIQ_Dispatcher

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

;	unsigned int SAr_DisableInt(void)
;	void SAr_EnableInt(unsigned int)
;
;	Disable and enable IRQ and FIQ preserving current CPU mode.
;
	EXPORT	SAr_DisableInt
SAr_DisableInt
    STMFD   sp!, {v1,lr}
    MRS     v1, cpsr
    AND     a1, v1, #NoInt
    ORR     v1, v1, #NoInt
    MSR     cpsr, v1
    LDMFD   sp!, {v1,pc}


	EXPORT	SAr_EnableInt
SAr_EnableInt
    STMFD   sp!, {v1, lr}
    MRS     v1, cpsr
    BIC     v1, v1, #NoInt
    ORR     v1, v1, a1
    MSR     cpsr, v1
    LDMFD   sp!, {v1, pc}


	EXPORT	SAr_GetCPSR
SAr_GetCPSR
    MRS     a1, cpsr
    MOV     pc, lr

	EXPORT	SAr_PutCPSR
SAr_PutCPSR
    MSR     cpsr, a1
    MOV     pc, lr

;	void OSCtxSw(void)
;	
;	Perform a context switch.
;

	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

;/******************************************************************************
;*
;* sysCpuVerMake - get the CPU version from CoProcessor to global
;*
;* RETURNS: nothing
;*
;*/

    EXPORT  sysCpuVerMake           ;/* Put CPU version revision */
sysCpuVerMake
    LDR     r0, =IXP1200_SysCpuRev     ; IXP1200_cpu rev global
	MRC	    p15, 0, r3, c0, c0, 0
    AND	    r3, r3, #0xF
    STR     r3, [r0]               ; set it
	MOV	pc, lr

;/******************************************************************************
;*
;* sysCpuVerGet - get the CPU version for user mode code
;*
;* RETURNS: the CPU version in the range 0..15
;*  can't do coprocessor read from user mode...
;*/

    EXPORT  sysCpuVerGet           ;/* Get CPU version revision */
sysCpuVerGet
    LDR r0, =IXP1200_SysCpuRev
    LDR     r0, [r0, #0]
;    MOVS    r0, r0
	MOV	pc, lr

;/******************************************************************************
;*
;* sysCpuIDMake - get the CPU ID info from CoProcessor to global
;*
;* RETURNS: nothing
;*
;*/

    EXPORT  sysCpuIDMake           ;/* Put CPU version revision */
sysCpuIDMake
    LDR     r0, =IXP1200_SysCpuID     ; IXP1200_cpu rev global
	MRC	    p15, 0, r3, c0, c0, 0
    STR     r3, [r0]               ; set it
	MOV	pc, lr

;/******************************************************************************
;*
;* sysCpuIDGet - get the CPU identifier for user mode code
;*
;* RETURNS: the CPU ID
;*  can't do coprocessor read from user mode...
;*/

    EXPORT  sysCpuIDGet           ;/* Get CPU version revision */
sysCpuIDGet
    LDR r0, =IXP1200_SysCpuID
    LDR     r0, [r0, #0]
	MOV	pc, lr
;/*******************************************************************************
;*
;* pciCsrWr - Write PCI CSR register safely under C0 / Hyannis bug
;*
;* This routine writes a PCI CSR and then reads it back. This is to avoid a
;* hardware bug in C0/Hyannis where if a write to a PCI CSR is followed by
;* a PCI Mem/IO/Config write, a random CSR can get trashed.
;*
;* For safety, interrupts are disabled during this so that we can't be
;* swapped out
;*
;* NOTE
;* For reasons of efficiency, this routine does not establish a stack frame.
;*
;* RETURNS: N/A
;*
;* void pciCsrWr
;*	(
;*	void*			addr		/@ Address to be written @/
;*	UINT32			data		/@ data to be written @/
;*	)
;*/
    EXPORT  pciCsrWr
pciCsrWr
;	/* Disable insterrupts */
	MRS	r2, cpsr
;	ORR	r3, r2, #I_BIT | F_BIT
	ORR	r3, r2, #0xC0
	MSR	cpsr, r3

;	/* Do write */
	STR	r1, [r0]
	LDR	r1, [r0]

;	/* Restore interrupts */
	MSR	cpsr, r2

	MOV	pc, lr		;/* Return */

	END