
		AREA	|ARM$$code|,CODE,READONLY

		GET	OSLib:OSLib.Hdr.OS
		GET	OSLib:OSLib.Hdr.OSReadSysInfo

		GET	hdr.cp

SWI_TABLE_ADDR	*	&FAFF35A0
SWI_RETN_ADDR	*	&FAFF3384

		ENTRY

module_base	DCD	0
		DCD	mod_init - module_base
		DCD	mod_fin - module_base
		DCD	0
		DCD	mod_title - module_base
		DCD	mod_help - module_base
		DCD	0
		DCD	0
		DCD	0
		DCD	0
		DCD	0
		DCD	0
		DCD	mod_flags - module_base
		DCD	-1

mod_flags	DCD	1

da_name
mod_title	=	"RaiseProcVec",0
mod_help	=	"RaiseProcVec",9,"0.11 (03 Sep 2005)",0
		ALIGN


mod_init	MOV	R6,R14
		SWI	XOS_ReadMemMapInfo
		MOVVS	PC,R6

		STR	R6,[R13,#-4]!
		STMFD	R13!,{R7-R11}

		MOV	R2,R0
		MOV	R5,R0
		MOV	R0,#0
		MOV	R1,#-1
		MOV	R3,#-1
		MOV	R4,#&180
		MOV	R6,#0
		MOV	R7,#0
		ADR	R8,da_name
		SWI	XOS_DynamicArea
		BVS	init_leave

		STR	R1,[R12]		;remember area number

		ADD	R1,R3,#4096
		MOV	R2,#0
zero_lp		STR	R2,[R1,#-4]!
		STR	R2,[R1,#-4]!
		STR	R2,[R1,#-4]!
		STR	R2,[R1,#-4]!
		CMP	R1,R3
		BHI	zero_lp

		;determine the physical address of our page

		STR	R3,[R3,#4]
		MOV	R0,#&2200
		MOV	R1,R3
		MOV	R2,#1
		SWI	XOS_Memory
		LDRVC	R4,[R3,#8]
		BVS	init_leave2

		ORR	R0,R4,#&550
		ORR	R0,R0,#&E
		STR	R0,[R3,#3*1024 + (&F0000/4096)*4]

		;determine the logical address of the L1PT

		MOV	R0,#6
		MOV	R1,#0
		MOV	R2,#13
		SWI	XOS_ReadSysInfo

		;map our page into memory using our page ;)

		ADD	R2,R2,#&F00*4
		ADD	R4,R4,#3*1024
		ORR	R4,R4,#&11
		STR	R4,[R2,#&FF*4]

		MOV	R0,#1
		ADD	R2,R3,#4096
		MOV	R1,R3
		SUB	R2,R2,#4
		SWI	XOS_SynchroniseCodeAreas

		;invalidate TLBs

		MCR	CP15,0,R0,C8,C7,0

		;wait until reconfigured

		MRC	CP15,0,R1,C2,C0,0
		MOV	R0,R0
		SUB	PC,PC,#4

		;build the new processor vectors

		MOV	R3,#-1
		LDR	R0,=&E59FF2B8
		MOV	R3,R3,LSL #16

		STR	R0,[R3]
		STR	R0,[R3,#4]
		STR	R0,[R3,#8]
		STR	R0,[R3,#&C]
		STR	R0,[R3,#&10]
		STR	R0,[R3,#&14]
		STR	R0,[R3,#&18]

		ADR	R0,fiq_handler
		LDMIA	R0,{R0-R2,R4-R7}
		ADD	R8,R3,#&1C
		STMIA	R8,{R0-R2,R4-R7}

		;steal the current handlers

		LDR	R8,[R3,-R3]
		MOV	R1,#&FF
		ORR	R1,R1,#&F00
		AND	R8,R8,R1
		ADD	R8,R8,#8
		LDMIA	R8,{R0-R2,R4-R7}

		ADD	R8,R8,R3
		STMIA	R8!,{R0-R2,R4-R7}

		MOV	R0,#1
		MOV	R1,R3
		SUB	R2,R8,#4
		SWI	XOS_SynchroniseCodeAreas

		MOVVC	R0,#6
		MOVVC	R1,#0
		MOVVC	R2,#18
		SWIVC	XOS_ReadSysInfo
		BVS	init_leave2

		;disable IRQs

		MRS	R0,CPSR
		ADD	R3,R2,#&69*4
		STR	R3,switab_addr
		ADR	R4,claimprocvec
		ORR	R2,R0,#&80
		MSR	CPSR_c,R2

		;move the CPU vectors

		MRC	CP15,0,R2,C1,C0,0
		LDR	R5,[R3]			;remember OS_ClaimProcessorVector
		STR	R4,[R3]			;replace OS_ClaimProcessorVector
		ORR	R2,R2,#1<<13
		MCR	CP15,0,R2,C1,C0,0

		STR	R5,prev_handler

		;wait until reconfigured

		MRC	CP15,0,R1,C2,C0,0

		;restore IRQ state

		MSR	CPSR_c,R0

		CMP	R0,R0
init_leave	LDMFD	R13!,{R7-R11,PC}


init_leave2	LDR	R1,[R12]
		MOV	R2,R0
		MOV	R0,#1
		SWI	XOS_DynamicArea
		MOV	R0,R2
		MSR	CPSR_f,#1<<28
		LDMFD	R13!,{R7-R11,PC}


fiq_handler	SUB	R14,R14,#4
		MOV	R13,#&100
		MOV	R10,#&38
		STMFD	R13!,{R0-R3,R14}
		ADD	R14,PC,#0
		LDMIA	R10,{R9,PC}
		LDMFD	R13!,{R0-R3,PC}^


;??? we don't actually unmap our page at the moment,
;    though that's hardly likely to break anything!

mod_fin		LDR	R1,[R12]
		LDR	R3,=&E59FF2B8
		MOV	R6,R14
		CMP	R1,#0
		MOVEQ	PC,R6

		;reinstate jump instructions in case
		;  they've been corrupted

		MOV	R1,#0
		MRS	R4,CPSR
		STR	R3,[R1]
		STR	R3,[R1,#4]
		STR	R3,[R1,#8]
		STR	R3,[R1,#&C]
		STR	R3,[R1,#&10]
		STR	R3,[R1,#&14]
		STR	R3,[R1,#&18]
		MOV	R0,#1
		MOV	R2,#&18
		SWI	XOS_SynchroniseCodeAreas

		;steal the latest routine addresses and
		;  ensure that they remain after we've gone

		MOV	R2,#&FF000000
		ORR	R2,R2,#&00FF0000
		MOV	R0,#&FF0
		ORR	R0,R0,#&F
		AND	R0,R0,R3
		ADD	R0,R0,#8

		;disable IRQs

		ORR	R14,R4,#&80
		MSR	CPSR_c,R14

		ADD	R2,R2,R0
		LDMIA	R2!,{R3,R5,R14}
		STMIA	R0!,{R3,R5,R14}
		LDMIA	R2,{R2,R3,R5,R14}
		STMIA	R0,{R2,R3,R5,R14}
		LDR	R3,switab_addr
		LDR	R5,prev_handler

		LDR	R1,[R12]
		MOV	R0,#0
		STR	R0,[R12]

		;move the CPU vectors

		MRC	CP15,0,R2,C1,C0,0
		STR	R5,[R3]			;restore OS_ClaimProcessorVector
		BIC	R2,R2,#1<<13
		MCR	CP15,0,R2,C1,C0,0

		;wait until reconfigured

		MRC	CP15,0,R2,C2,C0,0

		;restore IRQ state

		MSR	CPSR_c,R4

		;release our page

		MOV	R0,#1
		SWI	XOS_DynamicArea
		MOV	PC,R6


claimprocvec	;check processor vector number is valid

		MRS	R10,CPSR
		AND	R12,R0,#&FF
		CMP	R12,#7
		BHS	badvec

		STR	R9,[R13,#-4]!
		MOV	R11,#&FF000000
		ORR	R9,R10,#&80
		ORR	R11,R11,#&00FF0000
		MSR	CPSR_c,R9		;disable IRQs
		ADD	R11,R11,#&2C0

		LDR	R9,[R11,R12,LSL #2]
		TST	R0,#&100
		BEQ	release

		STR	R1,[R11,R12,LSL #2]
		MOV	R1,R9
		LDR	R9,[R13],#4
		MSR	CPSR_c,R10
		LDR	PC,=SWI_RETN_ADDR

release		TEQ	R9,R2
		LDR	R9,[R13],#4
		BNE	badrelease
		STR	R1,[R11,R12,LSL #2]
		MSR	CPSR_c,R10
		LDR	PC,=SWI_RETN_ADDR

badrelease	ADR	R0,err_badrel
		ORR	R14,R14,#V
		MSR	CPSR_c,R10
		LDR	PC,=SWI_RETN_ADDR

badvec		ADR	R0,err_badvec
		ORR	R14,R14,#V
		LDR	PC,=SWI_RETN_ADDR


err_badvec	DCD	&1A0
		=	"Bad vector number",0
		ALIGN

err_badrel	DCD	&1A1
		=	"Bad vector release",0
		ALIGN

switab_addr	DCD	0
prev_handler	DCD	0

		END
