Index: kernel/arch/mips32/src/mm/tlb.c
===================================================================
--- kernel/arch/mips32/src/mm/tlb.c	(revision d5b9e8d0db718c208ae5e80f1ed56e9201df1a55)
+++ kernel/arch/mips32/src/mm/tlb.c	(revision e05b9563fd0aa49271b08b3182cfde3cef571185)
@@ -48,4 +48,13 @@
 #include <symtab.h>
 
+#define VPN_SHIFT	12	
+#define ADDR2VPN(a)	((a) >> VPN_SHIFT)
+#define ADDR2VPN2(a)	(ADDR2VPN((a)) >> 1)
+#define VPN2ADDR(vpn)	((vpn) << VPN_SHIFT)
+#define VPN22ADDR(vpn2)	(VPN2ADDR(vpn2) << 1)
+
+#define BANK_SELECT_BIT(a)	(((a) >> PAGE_WIDTH) & 1) 
+	
+
 static pte_t *find_mapping_and_check(uintptr_t, int, istate_t *);
 
@@ -77,4 +86,30 @@
 }
 
+/** Verify the TLB still contains / does not contain a given entry after the
+ * current thread possibly blocked.
+ *
+ * @param hi           Remembered contents of the EntryHi register.
+ * @param should_exist Flag saying whether the caller expect the entry to exist
+ *                     or not to exist.
+ * @returns            True if the TLB contains or does not contain the mapping
+ *                     according to the caller's expectation.
+ * @returns            False otherwise.
+ */
+static bool tlb_recheck_entry(entry_hi_t hi, bool should_exist)
+{
+	tlb_index_t index;
+
+	cp0_entry_hi_write(hi.value);
+	tlbp();
+	index.value = cp0_index_read();
+
+	if (!(should_exist ^ index.p)) {
+		/* The old entry disappeared or the new entry already exists. */
+		return false;
+	}
+
+	return true;
+}
+
 /** Process TLB Refill Exception.
  *
@@ -83,15 +118,18 @@
 void tlb_refill(istate_t *istate)
 {
+	entry_hi_t hi;
 	entry_lo_t lo;
-	entry_hi_t hi;
-	asid_t asid;
 	uintptr_t badvaddr;
 	pte_t *pte;
 	
 	badvaddr = cp0_badvaddr_read();
-	asid = AS->asid;
-	
+	hi.value = cp0_entry_hi_read();	
+
+	/* We may block in find_mapping_and_check(). */
 	pte = find_mapping_and_check(badvaddr, PF_ACCESS_READ, istate);
 	if (pte) {
+		if (!tlb_recheck_entry(hi, false))
+			return;
+
 		/*
 		 * Record access to PTE.
@@ -99,5 +137,4 @@
 		pte->a = 1;
 
-		tlb_prepare_entry_hi(&hi, asid, badvaddr);
 		tlb_prepare_entry_lo(&lo, pte->g, pte->p, pte->d,
 		    pte->cacheable, pte->pfn);
@@ -106,6 +143,5 @@
 		 * New entry is to be inserted into TLB
 		 */
-		cp0_entry_hi_write(hi.value);
-		if ((badvaddr / PAGE_SIZE) % 2 == 0) {
+		if (BANK_SELECT_BIT(badvaddr) == 0) {
 			cp0_entry_lo0_write(lo.value);
 			cp0_entry_lo1_write(0);
@@ -125,18 +161,13 @@
 void tlb_invalid(istate_t *istate)
 {
+	entry_hi_t hi;
+	entry_lo_t lo;
 	tlb_index_t index;
 	uintptr_t badvaddr;
-	entry_lo_t lo;
-	entry_hi_t hi;
 	pte_t *pte;
 
-	badvaddr = cp0_badvaddr_read();
-
 	/*
 	 * Locate the faulting entry in TLB.
 	 */
-	hi.value = cp0_entry_hi_read();
-	tlb_prepare_entry_hi(&hi, hi.asid, badvaddr);
-	cp0_entry_hi_write(hi.value);
 	tlbp();
 	index.value = cp0_index_read();
@@ -156,6 +187,13 @@
 	ASSERT(!index.p);
 
+	badvaddr = cp0_badvaddr_read();
+	hi.value = cp0_entry_hi_read();
+
+	/* We may block in find_mapping_and_check(). */
 	pte = find_mapping_and_check(badvaddr, PF_ACCESS_READ, istate);
 	if (pte) {
+		if (!tlb_recheck_entry(hi, true))
+			return;
+
 		/*
 		 * Read the faulting TLB entry.
@@ -174,9 +212,8 @@
 		 * The entry is to be updated in TLB.
 		 */
-		if ((badvaddr / PAGE_SIZE) % 2 == 0)
+		if (BANK_SELECT_BIT(badvaddr) == 0)
 			cp0_entry_lo0_write(lo.value);
 		else
 			cp0_entry_lo1_write(lo.value);
-		cp0_pagemask_write(TLB_PAGE_MASK_16K);
 		tlbwi();
 	}
@@ -189,18 +226,13 @@
 void tlb_modified(istate_t *istate)
 {
+	entry_hi_t hi;
+	entry_lo_t lo;
 	tlb_index_t index;
 	uintptr_t badvaddr;
-	entry_lo_t lo;
-	entry_hi_t hi;
 	pte_t *pte;
 
-	badvaddr = cp0_badvaddr_read();
-
 	/*
 	 * Locate the faulting entry in TLB.
 	 */
-	hi.value = cp0_entry_hi_read();
-	tlb_prepare_entry_hi(&hi, hi.asid, badvaddr);
-	cp0_entry_hi_write(hi.value);
 	tlbp();
 	index.value = cp0_index_read();
@@ -211,6 +243,13 @@
 	ASSERT(!index.p);
 
+	badvaddr = cp0_badvaddr_read();
+	hi.value = cp0_entry_hi_read();
+
+	/* We may block in find_mapping_and_check(). */
 	pte = find_mapping_and_check(badvaddr, PF_ACCESS_WRITE, istate);
 	if (pte) {
+		if (!tlb_recheck_entry(hi, true))
+			return;
+
 		/*
 		 * Read the faulting TLB entry.
@@ -230,9 +269,8 @@
 		 * The entry is to be updated in TLB.
 		 */
-		if ((badvaddr / PAGE_SIZE) % 2 == 0)
+		if (BANK_SELECT_BIT(badvaddr) == 0)
 			cp0_entry_lo0_write(lo.value);
 		else
 			cp0_entry_lo1_write(lo.value);
-		cp0_pagemask_write(TLB_PAGE_MASK_16K);
 		tlbwi();
 	}
@@ -296,5 +334,6 @@
 void tlb_prepare_entry_hi(entry_hi_t *hi, asid_t asid, uintptr_t addr)
 {
-	hi->value = ALIGN_DOWN(addr, PAGE_SIZE * 2);
+	hi->value = 0;
+	hi->vpn2 = ADDR2VPN2(ALIGN_DOWN(addr, PAGE_SIZE));
 	hi->asid = asid;
 }
@@ -303,12 +342,15 @@
 void tlb_print(void)
 {
-	page_mask_t mask;
-	entry_lo_t lo0, lo1;
+	page_mask_t mask, mask_save;
+	entry_lo_t lo0, lo0_save, lo1, lo1_save;
 	entry_hi_t hi, hi_save;
 	unsigned int i;
 
 	hi_save.value = cp0_entry_hi_read();
-	
-	printf("[nr] [asid] [vpn2] [mask] [gvdc] [pfn ]\n");
+	lo0_save.value = cp0_entry_lo0_read();
+	lo1_save.value = cp0_entry_lo1_read();
+	mask_save.value = cp0_pagemask_read();
+	
+	printf("[nr] [asid] [vpn2    ] [mask] [gvdc] [pfn     ]\n");
 	
 	for (i = 0; i < TLB_ENTRY_COUNT; i++) {
@@ -321,12 +363,15 @@
 		lo1.value = cp0_entry_lo1_read();
 		
-		printf("%-4u %-6u %#6x %#6x  %1u%1u%1u%1u  %#6x\n",
-		    i, hi.asid, hi.vpn2, mask.mask,
-		    lo0.g, lo0.v, lo0.d, lo0.c, lo0.pfn);
-		printf("                           %1u%1u%1u%1u  %#6x\n",
-		    lo1.g, lo1.v, lo1.d, lo1.c, lo1.pfn);
+		printf("%-4u %-6u %0#10x %-#6x  %1u%1u%1u%1u  %0#10x\n",
+		    i, hi.asid, VPN22ADDR(hi.vpn2), mask.mask,
+		    lo0.g, lo0.v, lo0.d, lo0.c, lo0.pfn << FRAME_WIDTH);
+		printf("                               %1u%1u%1u%1u  %0#10x\n",
+		    lo1.g, lo1.v, lo1.d, lo1.c, lo1.pfn << FRAME_WIDTH);
 	}
 	
 	cp0_entry_hi_write(hi_save.value);
+	cp0_entry_lo0_write(lo0_save.value);
+	cp0_entry_lo1_write(lo1_save.value);
+	cp0_pagemask_write(mask_save.value);
 }
 
@@ -334,11 +379,11 @@
 void tlb_invalidate_all(void)
 {
-	ipl_t ipl;
 	entry_lo_t lo0, lo1;
 	entry_hi_t hi_save;
 	int i;
 
+	ASSERT(interrupts_disabled());
+
 	hi_save.value = cp0_entry_hi_read();
-	ipl = interrupts_disable();
 
 	for (i = TLB_WIRED; i < TLB_ENTRY_COUNT; i++) {
@@ -358,5 +403,4 @@
 	}
 	
-	interrupts_restore(ipl);
 	cp0_entry_hi_write(hi_save.value);
 }
@@ -368,13 +412,12 @@
 void tlb_invalidate_asid(asid_t asid)
 {
-	ipl_t ipl;
 	entry_lo_t lo0, lo1;
 	entry_hi_t hi, hi_save;
 	int i;
 
+	ASSERT(interrupts_disabled());
 	ASSERT(asid != ASID_INVALID);
 
 	hi_save.value = cp0_entry_hi_read();
-	ipl = interrupts_disable();
 	
 	for (i = 0; i < TLB_ENTRY_COUNT; i++) {
@@ -398,5 +441,4 @@
 	}
 	
-	interrupts_restore(ipl);
 	cp0_entry_hi_write(hi_save.value);
 }
@@ -412,8 +454,9 @@
 {
 	unsigned int i;
-	ipl_t ipl;
 	entry_lo_t lo0, lo1;
 	entry_hi_t hi, hi_save;
 	tlb_index_t index;
+
+	ASSERT(interrupts_disabled());
 	
 	if (asid == ASID_INVALID)
@@ -421,8 +464,6 @@
 
 	hi_save.value = cp0_entry_hi_read();
-	ipl = interrupts_disable();
 
 	for (i = 0; i < cnt + 1; i += 2) {
-		hi.value = 0;
 		tlb_prepare_entry_hi(&hi, asid, page + i * PAGE_SIZE);
 		cp0_entry_hi_write(hi.value);
@@ -451,5 +492,4 @@
 	}
 	
-	interrupts_restore(ipl);
 	cp0_entry_hi_write(hi_save.value);
 }