# HG changeset patch
# User Paul Boddie <paul@boddie.org.uk>
# Date 1456777784 -3600
# Node ID 6e05b11c747d55df7494bd3b2cb4ebf075e4456c
# Parent  1ee72222a36d49836bd53e7dee9fc9c8db8c4249
Added explicit clearing of TLB mappings.

diff -r 1ee72222a36d -r 6e05b11c747d stage2/cpu.c
--- a/stage2/cpu.c	Mon Feb 29 19:09:10 2016 +0100
+++ b/stage2/cpu.c	Mon Feb 29 21:29:44 2016 +0100
@@ -155,18 +155,31 @@
 
 void init_tlb(void)
 {
-	unsigned short first_random = 0;
+	unsigned short first_random = 0, i, limit;
 
 	asm volatile(
 		"mtc0 $zero, $4\n"	/* CP0_CONTEXT */
 		"mtc0 $zero, $10\n"	/* CP0_ENTRYHI */
 		"mtc0 $zero, $2\n"	/* CP0_ENTRYLO0 */
 		"mtc0 $zero, $3\n"	/* CP0_ENTRYLO1 */
-		"mtc0 %0, $6\n"		/* CP0_WIRED */
+		"mtc0 %1, $6\n"		/* CP0_WIRED */
+		"mfc0 %0, $16\n"	/* CP0_CONFIG1 */
 		"nop"
-		:
+		: "=r" (limit)
 		: "r" (first_random)
 		);
+
+	/* Reset the mappings. The total number is bits 30..25 of Config1. */
+
+	for (i = 0; i < ((limit >> 25) & 0x3f); i++)
+	{
+		asm volatile(
+			"mtc0 %0, $0\n"	/* CP0_INDEX */
+			"nop"
+			:
+			: "r" (i)
+			);
+	}
 }
 
 void map_page_index(u32 virtual, u32 physical, u32 pagesize, u8 flags, u8 asid, u32 index)