;
;    DivaPC ARM Assembler source
;
;    SYS.S.SYSS - Assembly-language system functions
;
;
;        15-04-92 INH  Original
;        07-12-93      Service ID removed
;        19-05-95 DAF  Corrected SYSs_CallViaR12 - had bug due to me giving
;                      INH the wrong version of code.
; 2.15 1997.10.14 RJW  Callback fns ARMcoded from SYS.c
; 2.16 1997.10.11 RJW  SYS DoCallbacks revamped
;

	GET SYS.S.STDDEFS

        GET Module.mh.PCSUP  ; for Pcsupport SWI numbers

	AREA |C$$Code|, CODE, READONLY
;Imports
	IMPORT	CPUS_Run

	IMPORT	CPU_pTimerWord
;Exports
	EXPORT  SYSs_CallViaR12
	EXPORT	SYS_DoCallbacks
	EXPORT	SYS_InitDoCallbacks
	EXPORT	SYS_SetCallback



; CallViaR12 ---------------------------------

; This is Borris' CPU.S.R12SUPP, moved here 16/5/95

; Assembler support routine for performing 'far' call into a module.
; Used in contexts such as transfer and notify functions of DMA
; handler when R12_value is not zero.
;
; uint SYSs_CallViaR12(_kernel_swi_regs *inout, int fnaddr, uint R12_value)
;
; Can be called from USR, SVC or IRQ mode.
; Must not be called from FIQ mode.
;
; Not truly APCS, but you shouldn't be doing an APCS stack trace through
; this point anyway!
;
; Interrupt mode preserved across call.
; USR mode (probably) calls with IRQs enabled.
; SVC mode preserves entry IRQ state.
; IRQ mode disables IRQs for entry to user's routine
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


USR_mode	*	2_00
IRQ_mode	*	2_10
SVC_mode	*	2_11
I_bit		*	1 :SHL: 27


SYSs_CallViaR12  ROUT

            STMFD   SP!, {R4, LR}    ; onto whichever stack is current

            MOV     R3, PC
            ANDS    R3, R3, #3
            ASSERT  USR_mode = 0
            BEQ     %f10            ; call from USR mode

            TEQ     R3, #SVC_mode   ; aLReady in SVC mode?
            BNE     %f20            ; must be IRQ mode (FIQ not allowed)

            MOV     R4, R1          ; fnptr
            MOV     R12, R2         ; R12_value
            LDMFD   R0, {R0-R3}     ; inout
            MOV     LR, PC          ; return straight after
            MOV     PC, R4          ; fnptr

            LDMFD   SP!, {R4, PC}^  ; return (maybe to wrapper)

; Handle USR => SVC call - fairly basic wrapper

10
            EnterSVCmode
            BL      SYSs_CallViaR12
            ExitSVCmode
            LDMFD   SP!, {R4, PC}^  ; exit through USR stack

; Handle IRQ => SVC call - little more complicated

20          MOV     R3, PC
            ORR     R3, R3, # SVC_mode + I_bit
            TEQP    R3, # 0
            MOV     R0, R0          ; SVC mode, IRQs disabled

            STMFD   SP!, {LR}       ; preserve SVC's R14
            BL      SYSs_CallViaR12 ; Call now in SVC mode
            LDMFD   SP!, {LR}       ; restore SVC's R14

            TEQ     PC, # IRQ_mode + I_bit
            MOV     R0, R0          ; snore

            LDMFD   SP!, {R4, PC}^  ; restores IRQ entry state

; Everything below here from RJW
SYS_DoCallbacks
	; Only ever called in USR mode...
	STMFD	r13!,{r14}

sdc_lp
; IMPORT about_to_call_swi
;	BL about_to_call_swi
	SWI PCSupport_GetCallback

	CMP R0,#0
	LDMEQFD	r13!,{PC}^

	LDMIB	r0,{r1,r2,r12}		; r1 = fn
					; r2 = R0val
					; r12= pCB->R12val
	MOV	r0,r2
	MOV	R3,R2
; IMPORT log_callback
;	BL	log_callback
	; If its a user mode one, then thats easy!
	CMP	r12,#0
	ADREQ	LR,sdc_lp
	MOVEQ	PC,r1
	; If its an SVC one, then thats harder...
	EnterSVCmode
	STMFD	r13!,{r14}
	MOV	LR,PC
	ORR	PC,R1,#3	; to ensure we arrive in SVC mode
	LDMFD	r13!,{r14}
	ExitSVCmode

	B sdc_lp

CB_SetAddr
	DCD 0
CB_GetAddr
	DCD 0
CB_R12
	DCD 0
SYS_InitDoCallbacks
	STMFD	r13!,{r0-r8,r14}

	SWI	PCSupport_GetInternalAddr
	ORR	R3,R3,#3	; Cos we only want to call it in SVC mode
	ORR	R4,R4,#3	; Cos we only want to call it in SVC mode
	STR	R3,CB_SetAddr	; R3 & R4 return CB get & set fn addresses
	STR	R4,CB_GetAddr
	STR	R2,CB_R12       ; R2 has private word

	LDMFD	r13!,{r0-r8,pc}^
SYS_SetCallback
	; r0 = Callback *pCB
	; r1 = OneIntFnPtr fnptr
	; r2 = int param
	MOV	r3,#0
	STMIB	r0,{r1,r2,r3}	; pCb->fn     = fnptr
				; pCb->R0val  = param
				; pCb->R12val = 0

	MOV     R3, PC
	ANDS    R3, R3, #3
	LDRNE	PC,CB_SetAddr	; If we are already in a priveledged mode then
				; just call the SWI.
	; Otherwise do it the hard way
	EnterSVCmode

	STMFD	R13!,{R14}
	MOV	r14,PC
	LDR	PC,CB_SetAddr
	LDMFD	R13!,{R14}

	ExitSVCmode
	MOVS	PC,R14

	END

    END


