MODULE Caches;
(**
	AUTHOR Timothée Martiel, 12/2015
	PURPOSE Cache maintenance operations for Zynq 7000 SoC

	Cache features:
		o L1:
			- cache line = 32 bytes
			- 4 ways
			- 256 sets
		o L2:
			- cache line = 32 bytes
			- 8 ways
*)

IMPORT SYSTEM, Trace;
CONST
	CacheLineSize *	= 32;

	PROCEDURE EnableL1Cache *;
	CODE
		; Enable Cache and TLB maintenance broadcast
		MRC	P15, 0, R0, C1, C0, 1
		ORR	R0, R0, #1H
		MCR	P15, 0, R0, C1, C0, 1
		ISB

		; Enable Caching in SCTLR
		MRC	P15, 0, R0, C1, C0, 0
		ORR	R0, R0, #4H
		MCR	P15, 0, R0, C1, C0, 0
		ISB
	END EnableL1Cache;

	(** Enable L2 cache, prefetching and other speculative execution support *)
	PROCEDURE EnableL2Cache *;
	CODE
		LDR	R0,[PC, #L2CCCrtl-$-8]			; load l2cc base address base + control register
		MOV	R1, #0								; force the disable bit
		STR	R1, [R0,#0]							; disable the l2 caches

		LDR	R0, [PC, #L2CCAuxCtrl-$-8]		; load l2cc base address base + aux control register
		LDR	R1,[R0,#0]							; read the register
		LDR	R2, [PC, #L2CCAuxControl-$-8]	; set the default bits
		ORR	R1,R1,R2
		STR	R1, [R0,#0]							; STORE THE AUX CONTROL REGISTER

		LDR	R0,[PC, #L2CCTAGLatReg-$-8]	; load l2cc base address base + tag latency address
		LDR	R1, [PC, #L2CCTAGLatency-$-8]	; set the latencies for the tag
		STR	R1, [R0,#0]							; store the tag latency register register

		LDR	R0, [PC, #L2CCDataLatReg-$-8]	; load l2cc base address base + data latency address
		LDR	R1,[PC, #L2CCDataLatency-$-8]	; set the latencies for the data
		STR	R1, [R0,#0]							; store the data latency register register

		LDR	R0,[PC, #L2CCWay-$-8]			; load l2cc base address base + way register
		LDR	R2, [PC, #H0xffff-$-8]
		STR	R2, [R0,#0]							; force invalidate

		LDR	R0, [PC, #L2CCSync-$-8]			; need to poll 0x730, pss_l2cc_cache_sync_offset
		; Load L2CC base address base + sync register
		; poll for completion

	Sync:
		LDR	R1, [R0,#0]
		CMP	R1, #0
		BNE	Sync

		LDR	R0,[PC, #L2CCIntRaw-$-8]		; clear pending interrupts
		LDR	R1,[R0,#0]
		LDR	R0,[PC, #L2CCIntClear-$-8]
		STR	R1,[R0,#0]
		LDR	R0,[PC,#L2CCCrtl-$-8]				; load l2cc base address base + control register
		LDR	R1,[R0,#0]							; read the register
		MOV	R2, #1								; set the enable bit
		ORR	R1,R1,R2
		STR	R1, [R0,#0]							; enable the l2 caches

		MRC	P15, 0, R0, C1, C0, 0				; flow prediction enable
		ORR	R0, R0, #800H						; #0x800
		MCR	P15, 0, R0, C1, C0, 0
		ISB

		MRC	P15, 0, R0, C1, C0, 1				; read auxiliary control register
		ORR	R0, R0, #4							; enable dside prefetch
		ORR	R0, R0, #2							; enable L2 prefetch hint
		MCR	P15, 0, R0, C1, C0, 1				; write auxiliary control register
		ISB
		
		B		exit

		; Data
	H0xffff: 			d32 0FFFFH
	L2CCWay:			d32 0F8F02000H + 077CH
	L2CCSync:			d32 0F8F02000H + 0730H
	L2CCCrtl:			d32 0F8F02000H + 0100H
	L2CCAuxCtrl:		d32 0F8F02000H + 0104H
	L2CCTAGLatReg: 	d32 0F8F02000H + 0108H
	L2CCDataLatReg: 	d32 0F8F02000H + 010CH
	L2CCIntClear:		d32 0F8F02000H + 0220H
	L2CCIntRaw:		d32 0F8F02000H + 021CH
	L2CCAuxControl:	d32 72360000H
	L2CCTAGLatency:	d32 0111H
	L2CCDataLatency:	d32 0121H

	exit:
	END EnableL2Cache;

	(** Clean all dirty L1 & L2 data cache entries *)
	PROCEDURE CleanDCache *;
	CONST
		L2CCBBase	= 0F8F02000H;
		L2COfs		= L2CCBBase + 7BCH;
		L2CSync	= L2CCBBase + 730H;
	CODE
		; Clean all sets of all ways of L1 cache
		MOV	R0, #0
	WayLoop:
		CMP	R0, #4
		BEQ	EndWayLoop
		LSL		R4, R0, #30
		MOV	R1, #0
	SetLoop:
		CMP	R1, #256
		BEQ	EndSetLoop

		LSL		R3, R1, #5
		ORR	R3, R4, R3
		MCR	P15, 0, R3, C7, C10, 2

		ADD	R1, R1, #1
		B		SetLoop
	EndSetLoop:
		ADD	R0, R0, #1
		B		WayLoop

	EndWayLoop:
		DSB

		; Invalidate all L2 ways
		LDR	R0, [PC, #L2COfsAdr - $ - 8]	; R0 := reg7_inv_way address
		MOV	R1, #0FH			; R1 := 0FH => invalidate all ways
		STR	R1, [R0, #0]		; reg7_inv_way <- R1

	Sync:
		DSB
		LDR	R0, [PC, #L2CSyncAdr - $ - 8]	; R0 := L2 cache sync register address
		MOV	R1, #1
		STR	R1, [R0, #0]		; [R0] := 1
	SyncLoop:						; repeat
		LDR	R1, [R0, #0]		; R1 := l2 cache syc state
		CMP	R1, #0
		BEQ	Exit					; until R1 = 0
		B		SyncLoop

	L2COfsAdr:		d32 L2COfs
	L2CSyncAdr:	d32 L2CSync
	Exit:
	END CleanDCache;

	(** Clean L1 and L2 data caches for memory region [adr, adr + size). Assumes a 1:1 memory mapping for the memory region *)
	PROCEDURE CleanDCacheRange*(adr:ADDRESS; len: SIZE);
	CONST
		cacheline = 32;
		L2CCBBase				= 0F8F02000H;
		L2CCCacheSync		= L2CCBBase + 00730H;
		L2CCCacheInvClnPAOfs= 007F0H;
		L2CCOffset				= L2CCBBase + L2CCCacheInvClnPAOfs;
	BEGIN
		IF len = 0 THEN RETURN END;
		CODE
			LDR	R0, [FP, #adr]						; R0 := adr
			LDR	R1, [FP, #len]						; R1 := len

			LDR	R2, [PC, #Cacheline - 8 - $]		; R2 := cacheline
			SUB	R3, R2, #1							; R3 := cacheline - 1
			AND	R3, R0, R3							; R3 := adr MOD cacheline

			ADD	R1, R1, R0
			SUB	R0, R0, R3							; R0 := adr - adr MOD cacheline
			;ADD	R1, R1, R3							; R1 := len + adr MOD cacheline

			MOV	R3, #0
			MCR	P15, 2, R3,  C0,  C0, 0				; Select cache level 1
			LDR	R4, [PC, #L2COfs - 8 - $]			; R4 := L2 cache flush address register address

		Loop:
			CMP	R0, R1								; while R0 < R1
			BEQ	Sync
			BHI		Sync

			MCR	P15, 0, R0,  C7, C10, 1				; Clean Cache Level 1 By MVA (R0)
			STR	R0, [R4, #0]						; Clean Cache Level 2 By PA (R0)
			DSB
			ADD	R0, R0, R2							; R0 := R0 + cacheline

			B		Loop								; end

		Sync:
			DSB
			LDR	R0, [PC, #L2CSync - 8 - $]		; R0 := L2 cache sync register address
			;MOV	R1, #1
			;STR	R1, [R0, #0]						; [R0] := 1
		SyncLoop:										; repeat
			LDR	R1, [R0, #0]						; R1 := l2 cache syc state
			CMP	R1, #0
			BEQ	Exit									; until R1 = 0
			B		SyncLoop

		Cacheline:	d32 cacheline
		L2COfs:	d32 L2CCOffset
		L2CSync:	d32 L2CCCacheSync

		Exit:
		END;
	END CleanDCacheRange;

	(** Invalidate all L1 & L2 data cache entries *)
	PROCEDURE InvalidateDCache*( dCacheBase: LONGINT );
	CODE
		LDR R1, [FP, #dCacheBase]	; R1 contains the virtual address of a region of cacheable memory
		LDR R1, [R1, #0]
		MOV R0, #1024	; R0 is the loop count
	.loop1
		MCR p15, 0, R1, c7, c2, 5	; allocate line in data cache
		ADD R1, R1, #32	; increment the address in R1 to the next cache line
		SUBS R0, R0, #1
		BNE loop1

		; clean the mini-data cache
		MOV R0, #64
	.loop2
		LDR R3, [R1], #32	; load and increment to next cache line
		SUBS R0, R0, #1
		BNE loop2

		B invalidate
		DCD dCacheBase
		
	.invalidate	
		; invalidate data cache and mini-data cache		
		MCR p15, 0, R0, c7, c6, 0
		; cpwait
		MRC p15, 0, R0, c2, c0, 0
		MOV R0, R0
		SUB PC, PC, #4
		MOV R0, R0
		MOV R0, R0
		MOV R0, R0
		MOV R0, R0
	END InvalidateDCache;

	(** Invalidate L1 and L2 data caches for the memory region [adr, adr + size). Assumes a 1:1 memory mapping for the memory region *)
	PROCEDURE InvalidateDCacheRange*(adr: ADDRESS; len: SIZE);
	CONST
		cacheline 				= 32;
		L2CCBBase				= 0F8F02000H;
		L2CCCacheSync		= L2CCBBase + 00730H;
		L2CCCacheInvPAOfs	= 00770H;
		L2CCOffset				= L2CCBBase + L2CCCacheInvPAOfs;
	BEGIN
		IF len = 0 THEN RETURN END;
		CODE
			LDR	R0, [FP, #adr]						; R0 := adr
			LDR	R1, [FP, #len]						; R1 := len

			LDR	R2, [PC, #Cacheline - 8 - $]		; R2 := cacheline
			SUB	R3, R2, #1							; R3 := cacheline - 1
			AND	R3, R0, R3							; R3 := adr MOD cacheline

			SUB	R0, R0, R3							; R0 := adr - adr MOD cacheline
			ADD	R1, R1, R3							; R1 := len + adr MOD cacheline
			MOV	R5, #0								; R5 := 0 (counter value)

			MOV	R3, #0
			MCR	P15, 2, R3,  C0,  C0, 0				; Select cache level 1
			LDR	R4, [PC, #L2COfs - 8 - $]			; R4 := L2 cache invalidate address register address

		Loop:
			CMP	R5, R1								; while R5 < R1
			BEQ	Sync
			BHI		Sync
			STR	R0, [R4, #0]						; Invalidate Cache Level 2 By PA (R0)
			DSB

			MCR	P15, 0, R0,  C7, C6, 1				; Invalidate Cache Level 1 By MVA (R0)
			ADD	R0, R0, R2							; R0 := R0 + cacheline
			ADD	R5, R5, R2

			B		Loop								; end

		Sync:
			DSB
			LDR	R0, [PC, #L2CSync - 8 - $]		; R0 := L2 cache sync register address
			MOV	R1, #1
			STR	R1, [R0, #0]						; [R0] := 1
		SyncLoop:										; repeat
			LDR	R1, [R0, #0]						; R1 := l2 cache syc state
			CMP	R1, #0
			BEQ	Exit									; until R1 = 0
			B		SyncLoop

		Cacheline:	d32 cacheline
		L2COfs:	d32 L2CCOffset
		L2CSync:	d32 L2CCCacheSync

		Exit:
		END;
	END InvalidateDCacheRange;

	PROCEDURE InvalidateICache *;
	CODE
		MCR	P15, 0, R0, C7, C5, 0	; invalidate ICache & BTB
		; cpwait
		MRC	P15, 0, R0, C2, C0, 0
		MOV	R0, R0
		SUB	PC, PC, #4
		MOV	R0, R0
		MOV	R0, R0
		MOV	R0, R0
		MOV	R0, R0
	END InvalidateICache;

	PROCEDURE InvalidateTLB *;
	CODE
		MCR	P15, 0, R0, C8, C3, 0	; invalidate I+D TLB
		; cpwait
		MRC	P15, 0, R0, C2, C0, 0
		MOV	R0, R0
		SUB	PC, PC, #4
		MOV	R0, R0
		MOV	R0, R0
		MOV	R0, R0
		MOV	R0, R0
	END InvalidateTLB;

	PROCEDURE InvalidateTLBEntry * (address: ADDRESS);
	CODE
		LDR	R0, [FP, #address]
		;ADD	SP, SP, #4				; remove parameter
		MCR	P15, 0, R0, C8, C3, 1	; invalidate address
		; cpwait
		MRC	P15, 0, R0, C2, C0, 0
		MOV	R0, R0
		SUB	PC, PC, #4
		MOV	R0, R0
		MOV	R0, R0
		MOV	R0, R0
		MOV	R0, R0
	END InvalidateTLBEntry;

	(* DrainWriteBuffer - drains the write buffer. Works only in a priviledged mode *)
	PROCEDURE DrainWriteBuffer*;
	CODE
		MCR p15, 0, R0, c7, c10, 4	; drain WB
		; cpwait
		MRC p15, 0, R0, c2, c0, 0
		MOV R0, R0
		SUB PC, PC, #4
		MOV R0, R0
		MOV R0, R0
		MOV R0, R0
		MOV R0, R0
	END DrainWriteBuffer;

	PROCEDURE Info *;
	VAR
		dword: ADDRESS;
	BEGIN
		CODE
			MOV	R0, #0
			MCR	P15, 2, R0, C0, C0, 0
			MRC	P15, 1, R0, C0, C0, 0
			STR	R0, [FP, #dword]
		END;
		Trace.String("Cache Size Reg: "); Trace.Address(dword); Trace.Ln;
		Trace.String("	Cache line size = "); Trace.Int(LSH(LONGINT(1), 2 + dword MOD 8) * 4, 0); Trace.Ln;
		Trace.String("	Associativity = "); Trace.Int(LSH(dword MOD 2000H, -3) + 1, 0); Trace.Ln;
		Trace.String("	Number of sets = "); Trace.Int(LSH(dword MOD 10000000H, -13) + 1, 0); Trace.Ln;
	END Info;
END Caches.