MODULE Initializer;
(**
	AUTHOR "Timothée Martiel";
	PURPOSE "Processor initialization code for ARM kernel";

	This module provides initialization for CPUs. It contains the initial procedure that begins the execution of
	the kernel. It also provides a mechanism for starting secondary processors: all processors should start executing
	at the beginning of the kernel image (i.e. Init): only CPU with id 0 will continue and start the kernel. The other
	processors will be sleeping in Init, waiting to be started later.
*)
IMPORT SYSTEM (* Needed to avoid Runtime importing Heaps *);
VAR
	(**
		Wakeup address for secondary processors: if set # 0, the CPU with id matching "secondaryProcId" will jump to
		that address.
	*)
	secondaryBootProc *: ADDRESS;
	(**
		Wakup id for secondary processors: when executing SEV, all CPUs wakup. Only the CPU with id matching this field
		will read and jump to "secondaryBootProc".
	*)
	secondaryProcId *: LONGINT;

	(** Configuration string base address *)
	configBase *: ADDRESS;
	(** Configuration string size *)
	configSize *: LONGINT;

	(**
		Initial kernel code: this code is placed at the beginning of the kernel image and is executed by all CPUs.
		This code:
			- sets up a stack
			- disables interrupts and MMU
			- sets the interrupt vector to 0
			- invalidates caches
			- put secondary processors into the sleeping loop
			- allow CPU0 to continue kernel initialization
	*)
	PROCEDURE {INITIAL,NOPAF} Init;
	CODE
		; set IRQ vector base register to zero
		MOV	R2, #0
		MCR	P15, 0, R2, C12, C0, 0

		; disable MMU
		MRC	P15, 0, R2, C1, C0, 0
		BIC		R2, R2, #1
		MCR	P15, 0, R2, C1, C0, 0

		LDR	FP, [pc, #InitialFP-$-8]				; set stack pointer
		LDR	SP, [pc, #InitialFP-$-8]				; set frame pointer

		B		getProcId
	InitialFP: 			d32 0001B000H				; initial frame pointer address, internal memory bank

		; Set SMP mode
		MRC p15, 0, R2, C1, C0, 1
		ORR R2, R2, #041H
		MCR p15, 0, R2, C1, C0, 1
		ISB

		; Filter CPUs: only CPU0 must initialize the kernel, other are started later on demand
		MOV	R2, #0

	getProcId:
		MRC	P15, 0, R3, C0, C0, 5
		AND	R3, R3, #0FH
		CMP	R3, #0
		BEQ	bootProc								; CPU 0 continues to booting the kernel

	secondaryProcLoop:								; Secondary CPUs (i.e. CPUs that do not start the kernel) are
		BL		InvalidateDCache
		WFE											; sleeping and waiting to be started by SEV.


		LDR	R0, [PC, #sndProcIdAdr-$-8]
		LDR	R0, [R0, #0]							; R0 := secondaryProcId

		MRC	P15, 0, R1, C0, C0, 5
		AND	R1, R1, #0FH							; R1 := CPU ID

		CMP	R0, R1									; IF R1 # R0: loop again
		BNE	secondaryProcLoop

		LDR	R0, [PC, #sndBootProcAdr-$-8]
		LDR	R1, [R0, #0]							; R1 := secondaryBootProc
		CMP	R1, #0									; IF R1 = 0: loop again
		BEQ	secondaryProcLoop
		BX		R1										; Jump to secondaryBootProc

	sndBootProcAdr:	d32 secondaryBootProc
	sndProcIdAdr:		d32 secondaryProcId

	bootProc:											; CPU0 continues executing the kernel image
		; Save configuration parameters
		LDR	R2, [PC, #CfgBase - 8 - $]
		STR	R0, [R2, #0]
		LDR	R2, [PC, #CfgSize - 8 - $]
		STR	R1, [R2, #0]

		BL		InvalidateDCache						; After invalidating DCache

		; Setup the interrupt vector: copy it from the data section below
		MOV	R0, #0									; R0 := dst
		LDR	R1, [PC, #intVecAdr - $ - 8]			; R1 := src
		MOV	R2, #fiq - RESET + 4					; R2 := LEN(src) in bytes

	copyInts:
		LDR	R4, [R1, #0]
		STR	R4, [R0, #0]
		ADD	R0, R0, #4
		ADD	R1, R1, #4
		SUBS	R2, R2, #4
		BNE	copyInts

		; Start the kernel now
		B		startKernel

	; Interrupt vector data: to install the interrupt vector, we just have to copy
	; from RESET to fiq (included) at address 0.
	RESET: 	LDR	PC, [PC, #reset-$-8]			; RESET
	UNDEF: 	LDR	PC, [PC, #undef-$-8]			; UNDEF
	SWI: 		LDR	PC, [PC, #swi-$-8]			; SWI
	PREF: 		LDR	PC, [PC, #prefetch-$-8]		; Prefetch Abort
	DATA: 		LDR	PC, [PC, #data-$-8]			; Data Abort
	INVALID: 	B		INVALID						; (not assigned)
	IRQ: 		LDR	PC, [PC, #irq-$-8]				; IRQ
	FIQ: 		LDR	PC, [PC, #fiq-$-8]				; FIQ
		; initial interrupt vector setup such that inifinte loop is triggered
	reset: 		d32 Init								; Reset starts the kernel
	undef: 		d32 04H
	swi: 		d32 08H
	prefetch: 	d32 0CH
	data: 		d32 10H
	empty:		d32 0
	irq: 		d32 14H
	fiq: 		d32 1CH

		; Address of the interrupt vector data to copy
	intVecAdr:	d32 RESET

	CfgBase:			d32 configBase
	CfgSize:			d32 configSize


	startKernel:
	END Init;

	PROCEDURE {NOPAF} InvalidateDCache *;
	CODE
		invalidate_dcache:
			mrc	p15, 1, r0, c0, c0, 1		; read CLIDR
			ands	r3, r0, #7000000H
			mov	r3, r3, lsr #23			; cache level value (naturally aligned)
			beq	finished
			mov	r10, #0					; start with level 0
		loop1:
			add	r2, r10, r10, lsr #1		; work out 3xcachelevel
			mov	r1, r0, lsr r2			; bottom 3 bits are the Cache type for this level
			and	r1, r1, #7				; get those 3 bits alone
			cmp	r1, #2
			blt		skip					; no cache or only instruction cache at this level
			mcr	p15, 2, r10, c0, c0, 0	; write the Cache Size selection register
			isb								 ; isb to sync the change to the CacheSizeID reg
			mrc	p15, 1, r1, c0, c0, 0		; reads current Cache Size ID register
			and	r2, r1, #7				; extract the line length field
			add	r2, r2, #4				; add 4 for the line length offset (log2 16 bytes)
			ldr	r4, [pc, #H0x3ff-$-8]
			ands	r4, r4, r1, lsr #3		; r4 is the max number on the way size (right aligned)
			clz	r5, r4						; r5 is the bit position of the way size increment
			ldr	r7, [pc, #H0x7fff-$-8]
			ands	r7, r7, r1, lsr #13		; r7 is the max number of the index size (right aligned)
		loop2:
			mov	r9, r4					; r9 working copy of the max way size (right aligned)
		loop3:
			orr		r11, r10, r9, lsl r5		; factor in the way number and cache number into r11
			orr		r11, r11, r7, lsl r2		; factor in the index number
			mcr	p15, 0, r11, c7, c14, 2	; clean & invalidate by set/way
			subs	r9, r9, #1				; decrement the way number
			bge		loop3
			subs	r7, r7, #1				; decrement the index
			bge		loop2
		skip:
			add	r10, r10, #2				; increment the cache number
			cmp	r3, r10
			bgt		loop1

		finished:
			mov	r10, #0					; swith back to cache level 0
			mcr	p15, 2, r10, c0, c0, 0	; select current cache level in cssr
			dsb								; dsb
			isb								; isb

			bx	lr

		H0x3ff:		d32 03FFH
		H0x7fff:	d32 07FFFH
	END InvalidateDCache;
END Initializer.

FoxARMInstructionSet.Disassemble A2.Bin -a=100120H ~