Index: arch/mips32/src/mm/tlb.c
===================================================================
--- arch/mips32/src/mm/tlb.c	(revision a98d2ecd2f9c6faa76d404cc92cbe2f4e82f6279)
+++ arch/mips32/src/mm/tlb.c	(revision dd14cced76b628787db9a8988a5bc366c95c9f92)
@@ -56,7 +56,17 @@
 void tlb_arch_init(void)
 {
+	int i;
+
 	cp0_pagemask_write(TLB_PAGE_MASK_16K);
-
-	tlb_invalidate_all();
+	cp0_entry_hi_write(0);
+	cp0_entry_lo0_write(0);
+	cp0_entry_lo1_write(0);
+
+	/* Clear and initialize TLB. */
+	
+	for (i = 0; i < TLB_ENTRY_COUNT; i++) {
+		cp0_index_write(i);
+		tlbwi();
+	}
 		
 	/*
@@ -294,37 +304,4 @@
 		symbol = s;
 	panic("%X: TLB Modified Exception at %X(%s)\n", cp0_badvaddr_read(), pstate->epc, symbol);
-}
-
-/** Invalidate TLB entries with specified ASID
- *
- * Invalidate TLB entries with specified ASID.
- *
- * @param asid ASID.
- */
-void tlb_invalidate(asid_t asid)
-{
-	entry_hi_t hi;
-	ipl_t ipl;
-	int i;	
-	
-	ASSERT(asid != ASID_INVALID);
-
-	ipl = interrupts_disable();
-	
-	for (i = 0; i < TLB_ENTRY_COUNT; i++) {
-		cp0_index_write(i);
-		tlbr();
-		
-		hi.value = cp0_entry_hi_read();
-		if (hi.asid == asid) {
-			cp0_pagemask_write(TLB_PAGE_MASK_16K);
-			cp0_entry_hi_write(0);
-			cp0_entry_lo0_write(0);
-			cp0_entry_lo1_write(0);
-			tlbwi();
-		}
-	}
-	
-	interrupts_restore(ipl);
 }
 
@@ -415,14 +392,27 @@
 void tlb_invalidate_all(void)
 {
+	ipl_t ipl;
+	entry_lo_t lo0, lo1;
 	int i;
 
-	cp0_entry_hi_write(0);
-	cp0_entry_lo0_write(0);
-	cp0_entry_lo1_write(0);
+	ipl = interrupts_disable();
 
 	for (i = 0; i < TLB_ENTRY_COUNT; i++) {
 		cp0_index_write(i);
+		tlbr();
+
+		lo0.value = cp0_entry_lo0_read();
+		lo1.value = cp0_entry_lo1_read();
+
+		lo0.v = 0;
+		lo1.v = 0;
+
+		cp0_entry_lo0_write(lo0.value);
+		cp0_entry_lo1_write(lo1.value);
+				
 		tlbwi();
 	}
+	
+	interrupts_restore(ipl);
 }
 
@@ -433,18 +423,34 @@
 void tlb_invalidate_asid(asid_t asid)
 {
+	ipl_t ipl;
+	entry_lo_t lo0, lo1;
 	entry_hi_t hi;
 	int i;
 
+	ASSERT(asid != ASID_INVALID);
+
+	ipl = interrupts_disable();
+	
 	for (i = 0; i < TLB_ENTRY_COUNT; i++) {
 		cp0_index_write(i);
 		tlbr();
 		
+		hi.value = cp0_entry_hi_read();
+		
 		if (hi.asid == asid) {
-			cp0_entry_lo0_write(0);
-			cp0_entry_lo1_write(0);
+			lo0.value = cp0_entry_lo0_read();
+			lo1.value = cp0_entry_lo1_read();
+
+			lo0.v = 0;
+			lo1.v = 0;
+
+			cp0_entry_lo0_write(lo0.value);
+			cp0_entry_lo1_write(lo1.value);
+
 			tlbwi();
 		}
 	}
-
+	
+	interrupts_restore(ipl);
 }
 
@@ -456,11 +462,17 @@
 void tlb_invalidate_page(asid_t asid, __address page)
 {
+	ipl_t ipl;
+	entry_lo_t lo0, lo1;
 	entry_hi_t hi;
 	tlb_index_t index;
-	int i;
+
+	ASSERT(asid != ASID_INVALID);
+
+	ipl = interrupts_disable();
 
 	hi.value = 0;
 	prepare_entry_hi(&hi, asid, page);
-	
+	cp0_entry_hi_write(hi.value);
+
 	tlbp();
 	index.value = cp0_index_read();
@@ -468,7 +480,18 @@
 	if (!index.p) {
 		/* Entry was found, index register contains valid index. */
-		cp0_entry_lo0_write(0);
-		cp0_entry_lo1_write(0);
+		tlbr();
+
+		lo0.value = cp0_entry_lo0_read();
+		lo1.value = cp0_entry_lo1_read();
+
+		lo0.v = 0;
+		lo1.v = 0;
+
+		cp0_entry_lo0_write(lo0.value);
+		cp0_entry_lo1_write(lo1.value);
+
 		tlbwi();
 	}
-}
+	
+	interrupts_restore(ipl);
+}