;AutoVIDC module source - Service Call Handler
;---------------------------------------------
;
;VIDC Enhancer software control
;For software controlled VIDC Enhancers
;
;Author: Andreas Barth (1995)
;Author: Paul Vernon (Retro-Kit) (2012-2013)
;   www: http://www.retro-kit.co.uk

; We need to run this if we haven't fully initialised and after that,
; we only check when there is a Mode-change...

;R0 - general purpose - check initialisation state.
;R1 - Service entry reason code (&46)

.AutoVIDC_Service
	TEQ	R1,#Service_ModeChange		; Service_ModeChange
	MOVNE	PC,R14				; We're initialised and it's not a mode change so just return.

	LDR	R12,[R12,#0]			;R12->VARS
; If we need to initiate a re-check of the mode lists, then we can branch here to do so, bypassing the initialisation and service checks.
; R0 - General purpose
; R1 - General purpose
; R2 - Current Mode returned by OS and manipulated
; R3 - Aux IO bitmask for setting AuxIO pins
; R4 - Loop counter
; R5 - Current Mode
; R6 - MonitorType
; R7 - Have we set the clock yet?
; R8 - register used to force SVC mode and disable IRQ and FIQ bits
; R9 - register used to force SVC mode and disable IRQ and FIQ bits

;Entry:
; R1 - Service
; R2 - Mode specifier
; R3 - Monitor type
.AutoVIDC_ReInitSWI	; Re-Initialise the AutoVIDC module
.AutoVIDC_doService
	STMFD	R13!,{R0-R9,R14}
	LDRB	R0,VIDCEnable			; Are we enabled?
	TEQ	R0,#1
	LDMNEFD	R13!,{R0-R9,PC}			; No? we can exit here...

	MOV	R7,#0

;RISC OS < 3.00 does not pass in MonitorType or Mode Number so we must get them another way...
;Under RISC OS 2.xx, we don't care about the MonitorType as it doesn't map modes depending on the MonitorType.
	LDRB	R0,internal_flags
	TST	R0,#RISCOS310orNewer		;are we on RISC OS 3.1x?
	MOVEQ	R6,R3				;YES, R2/R3 are valid
	MOVEQ	R5,R2
	BEQ	_not_RISCOS31x
	BL	getMonitorTypeAndMode
	MOV	R6,R1
	MOV	R5,R0
._not_RISCOS31x					;R5=Mode R6=Monitor Type
; Check to see if we need to ignore this mode
	ADR	R0,ignoreModes
	MOV	R2,R5				;Current Mode
	BL	_checkModeBit			;check if this mode has its Bit set
	MOVS	R0,R0,LSR R5			; shift Tablebit into Carryflag

; Now the carry flag contains the Mode-bit
; and is '1' for Ignore this mode
	BCS	_cleanUp				; If the carry flag is set, we can exit.

; If we're here then we need to check the mode to set the Enhancer to Enabled or Disabled.
	MOV	R2,R5				;Current Mode

; Setup which mode list and Aux bit to set and copy the current mode into R5.

	ADR	R0,ModeListOC			; Load in the 36MHz Enhanced modes list
	MOV	R3,#3				; Set which clock speed to use
	MVN	R4,#0				; Set up a counter

._svcLoop
	ADDS	R4,R4,#1			; Increment the loop counter by one

; Is OverClocking enabled?
	LDRB	R1,OCdisable			; Get the value for OverClocking disabled.
	CMPEQ	R1,#1				; first time around, check to see if O/C is disabled
	BEQ	_set36ModeList			; If disabled, skip to the next mode list...

	BL	_checkModeBit
	MOVS	R0,R0,LSR R2			; shift Tablebit into Carryflag

;If C is set then we need to set the clock...
	MOVCS	R0,R3            		; Move the current clock speed (0-3) into R0
	BCS	_callSetClock			; Set the clock...

; Depending on which iteration of the loop we're on, we need to set the ModeList lookup tables and the correct Aux bit to set and reset the value of the MODE from long term storage in R5...

	CMP	R4,#3				; If we've been around 4 times we can finish looping now...
	BEQ	_checkVIDCclockrate

	CMP	R4,#2				; on the third loop through, prepare for the fourth loop by setting the MappedModes list as being the active list
	BEQ	_setMappedModeList

	CMP	R4,#1				; on the second loop through....
	BEQ	_set25ModeList

; After the first loop (for over-clocking) we then check the 36MHz mode list

._set36ModeList
	ADR	R0,ModeList36			; set the ModeList to the 36MHz one...
	MOV	R3,#2				; set the Clock speed to use
	MOV	R2,R5				; copy the MODE stored in R5 back to R2 for the next iteration.
	B	_svcLoop

._set25ModeList
	LDRB	R0,internal_flags		; Are we in RO2 or 3?
	TST	R0,#RISCOS3
	ADRNE	R0,ModeList25RO2		; set the ModeList to RO2 25.175MHz...
	ADREQ	R0,ModeList25			; set the ModeList to RO3 25.175MHz...
	MOV	R3,#1				; set the Clock speed to use
	MOV	R2,R5				; copy the MODE stored in R5 back to R2 for the next iteration.
	B	_svcLoop

._setMappedModeList
	ADR	R0,MappedModes			; set the ModeList to the Mapped Modes list only used for (S)VGA MonitorTypes...
	MOV	R3,#1				; set the Clock speed to use
	MOV	R2,R5				; copy the MODE stored in R5 back to R2 for the next iteration.
	CMP	R6,#3				; are we MonitorType 3
	CMPNE	R6,#4				; if not are we MonitorType 4
	BNE	_checkVIDCclockrate		; if we're here we don't need to loop over the VGA mapped modes list so go straight to checking the OS

; we've looped through all the MODE lists and not found a matching mode.

._checkVIDCclockrate
	LDRB	R1,internal_flags		; Is RISC OS control enabled?
	TST	R1,#ROinControl
	BEQ	_cleanUp			; YES

	BL	getClockSpeedInkHz		; R3=VIDC Clock speed in kHz
	TEQ	R3,#0				; If we're in RO2, we get a value of 0 back
	BEQ	_cleanUp

	MOV	R0,R3				; Let's see which clock number we need to select
	BL	AutoVIDC_ClockAvailableSWI	; Should return -1, or 0-3

._callSetClock
	CMN	R0,#1				; If it's not -1 then we need to set the VIDC clock to the matching clock rate, the value of which is in R0 as 0, 1, 2 or 3 for 24MHz, 25.175MHz, 36Mhz or overclocked.
	MOVNE	R7,#1
	BLNE	AutoVIDC_SetClockSWI

._cleanUp
	TEQ	R7,#0				; if we haven't set the clock
	MOVEQ	R0,#0				; default to the 24MHz clock
	BLEQ	AutoVIDC_SetClockSWI		; and set it!

	LDMFD	R13!,{R0-R9,PC}

._checkModeBit
	AND	R2,R2,#%1111111
	ADD	R0,R0,R2,LSR #3			; Tablebase + (Mode /32*4)
	BIC	R0,R0,#%11			; Clear lowest 2 Bits. (now necessary here)
	LDR	R0,[R0,#0]			; get part of Table
	AND	R2,R2,#%11111			; mask shift amount
	ADD	R2,R2,#1			; always shift one !!
	MOV	PC,R14
