Index: kernel/arch/sparc64/src/mm/tlb.c
===================================================================
--- kernel/arch/sparc64/src/mm/tlb.c	(revision 454f1da77de2349b52f4bfe6ec9378202f3cdbda)
+++ kernel/arch/sparc64/src/mm/tlb.c	(revision 2057572a989f56acc5c906946caebf6b2e039bda)
@@ -55,12 +55,12 @@
 #endif
 
-static void dtlb_pte_copy(pte_t *t, bool ro);
-static void itlb_pte_copy(pte_t *t);
-static void do_fast_instruction_access_mmu_miss_fault(istate_t *istate, const
-	char *str);
+static void dtlb_pte_copy(pte_t *t, index_t index, bool ro);
+static void itlb_pte_copy(pte_t *t, index_t index);
+static void do_fast_instruction_access_mmu_miss_fault(istate_t *istate,
+    const char *str);
 static void do_fast_data_access_mmu_miss_fault(istate_t *istate,
-	 tlb_tag_access_reg_t tag, const char *str);
+    tlb_tag_access_reg_t tag, const char *str);
 static void do_fast_data_access_protection_fault(istate_t *istate,
-	tlb_tag_access_reg_t tag, const char *str);
+    tlb_tag_access_reg_t tag, const char *str);
 
 char *context_encoding[] = {
@@ -93,6 +93,6 @@
  * @param cacheable True if the mapping is cacheable, false otherwise.
  */
-void dtlb_insert_mapping(uintptr_t page, uintptr_t frame, int pagesize, bool
-	locked, bool cacheable)
+void dtlb_insert_mapping(uintptr_t page, uintptr_t frame, int pagesize,
+    bool locked, bool cacheable)
 {
 	tlb_tag_access_reg_t tag;
@@ -127,9 +127,10 @@
 /** Copy PTE to TLB.
  *
- * @param t Page Table Entry to be copied.
- * @param ro If true, the entry will be created read-only, regardless of its w
- * 	field.
- */
-void dtlb_pte_copy(pte_t *t, bool ro)
+ * @param t 	Page Table Entry to be copied.
+ * @param index	Zero if lower 8K-subpage, one if higher 8K-subpage.
+ * @param ro 	If true, the entry will be created read-only, regardless of its
+ * 		w field.
+ */
+void dtlb_pte_copy(pte_t *t, index_t index, bool ro)
 {
 	tlb_tag_access_reg_t tag;
@@ -138,13 +139,13 @@
 	frame_address_t fr;
 
-	pg.address = t->page;
-	fr.address = t->frame;
+	pg.address = t->page + (index << MMU_PAGE_WIDTH);
+	fr.address = t->frame + (index << MMU_PAGE_WIDTH);
 
 	tag.value = 0;
 	tag.context = t->as->asid;
 	tag.vpn = pg.vpn;
-	
+
 	dtlb_tag_access_write(tag.value);
-	
+
 	data.value = 0;
 	data.v = true;
@@ -159,5 +160,5 @@
 	data.w = ro ? false : t->w;
 	data.g = t->g;
-	
+
 	dtlb_data_in_write(data.value);
 }
@@ -165,7 +166,8 @@
 /** Copy PTE to ITLB.
  *
- * @param t Page Table Entry to be copied.
- */
-void itlb_pte_copy(pte_t *t)
+ * @param t 	Page Table Entry to be copied.
+ * @param index	Zero if lower 8K-subpage, one if higher 8K-subpage.
+ */
+void itlb_pte_copy(pte_t *t, index_t index)
 {
 	tlb_tag_access_reg_t tag;
@@ -174,6 +176,6 @@
 	frame_address_t fr;
 
-	pg.address = t->page;
-	fr.address = t->frame;
+	pg.address = t->page + (index << MMU_PAGE_WIDTH);
+	fr.address = t->frame + (index << MMU_PAGE_WIDTH);
 
 	tag.value = 0;
@@ -200,4 +202,5 @@
 {
 	uintptr_t va = ALIGN_DOWN(istate->tpc, PAGE_SIZE);
+	index_t index = (istate->tpc >> MMU_PAGE_WIDTH) % MMU_PAGES_PER_PAGE;
 	pte_t *t;
 
@@ -210,7 +213,7 @@
 		 */
 		t->a = true;
-		itlb_pte_copy(t);
+		itlb_pte_copy(t, index);
 #ifdef CONFIG_TSB
-		itsb_pte_copy(t);
+		itsb_pte_copy(t, index);
 #endif
 		page_table_unlock(AS, true);
@@ -223,5 +226,5 @@
 		if (as_page_fault(va, PF_ACCESS_EXEC, istate) == AS_PF_FAULT) {
 			do_fast_instruction_access_mmu_miss_fault(istate,
-				__FUNCTION__);
+			    __FUNCTION__);
 		}
 	}
@@ -237,8 +240,10 @@
 	tlb_tag_access_reg_t tag;
 	uintptr_t va;
+	index_t index;
 	pte_t *t;
 
 	tag.value = dtlb_tag_access_read();
-	va = tag.vpn << PAGE_WIDTH;
+	va = ALIGN_DOWN((uint64_t) tag.vpn << MMU_PAGE_WIDTH, PAGE_SIZE);
+	index = tag.vpn % MMU_PAGES_PER_PAGE;
 
 	if (tag.context == ASID_KERNEL) {
@@ -246,8 +251,8 @@
 			/* NULL access in kernel */
 			do_fast_data_access_mmu_miss_fault(istate, tag,
-				__FUNCTION__);
+			    __FUNCTION__);
 		}
 		do_fast_data_access_mmu_miss_fault(istate, tag, "Unexpected "
-			"kernel page fault.");
+		    "kernel page fault.");
 	}
 
@@ -260,17 +265,18 @@
 		 */
 		t->a = true;
-		dtlb_pte_copy(t, true);
+		dtlb_pte_copy(t, index, true);
 #ifdef CONFIG_TSB
-		dtsb_pte_copy(t, true);
+		dtsb_pte_copy(t, index, true);
 #endif
 		page_table_unlock(AS, true);
 	} else {
 		/*
-		 * Forward the page fault to the address space page fault handler.
+		 * Forward the page fault to the address space page fault
+		 * handler.
 		 */		
 		page_table_unlock(AS, true);
 		if (as_page_fault(va, PF_ACCESS_READ, istate) == AS_PF_FAULT) {
 			do_fast_data_access_mmu_miss_fault(istate, tag,
-				__FUNCTION__);
+			    __FUNCTION__);
 		}
 	}
@@ -282,8 +288,10 @@
 	tlb_tag_access_reg_t tag;
 	uintptr_t va;
+	index_t index;
 	pte_t *t;
 
 	tag.value = dtlb_tag_access_read();
-	va = tag.vpn << PAGE_WIDTH;
+	va = ALIGN_DOWN((uint64_t) tag.vpn << MMU_PAGE_WIDTH, PAGE_SIZE);
+	index = tag.vpn % MMU_PAGES_PER_PAGE;	/* 16K-page emulation */
 
 	page_table_lock(AS, true);
@@ -297,8 +305,9 @@
 		t->a = true;
 		t->d = true;
-		dtlb_demap(TLB_DEMAP_PAGE, TLB_DEMAP_SECONDARY, va);
-		dtlb_pte_copy(t, false);
+		dtlb_demap(TLB_DEMAP_PAGE, TLB_DEMAP_SECONDARY,
+		    va + index * MMU_PAGE_SIZE);
+		dtlb_pte_copy(t, index, false);
 #ifdef CONFIG_TSB
-		dtsb_pte_copy(t, false);
+		dtsb_pte_copy(t, index, false);
 #endif
 		page_table_unlock(AS, true);
@@ -311,5 +320,5 @@
 		if (as_page_fault(va, PF_ACCESS_WRITE, istate) == AS_PF_FAULT) {
 			do_fast_data_access_protection_fault(istate, tag,
-				__FUNCTION__);
+			    __FUNCTION__);
 		}
 	}
@@ -329,8 +338,8 @@
 
 		printf("%d: vpn=%#llx, context=%d, v=%d, size=%d, nfo=%d, "
-			"ie=%d, soft2=%#x, diag=%#x, pfn=%#x, soft=%#x, l=%d, "
-			"cp=%d, cv=%d, e=%d, p=%d, w=%d, g=%d\n", i, t.vpn,
-			t.context, d.v, d.size, d.nfo, d.ie, d.soft2, d.diag,
-			d.pfn, d.soft, d.l, d.cp, d.cv, d.e, d.p, d.w, d.g);
+		    "ie=%d, soft2=%#x, diag=%#x, pfn=%#x, soft=%#x, l=%d, "
+		    "cp=%d, cv=%d, e=%d, p=%d, w=%d, g=%d\n", i, t.vpn,
+		    t.context, d.v, d.size, d.nfo, d.ie, d.soft2, d.diag,
+		    d.pfn, d.soft, d.l, d.cp, d.cv, d.e, d.p, d.w, d.g);
 	}
 
@@ -341,14 +350,14 @@
 		
 		printf("%d: vpn=%#llx, context=%d, v=%d, size=%d, nfo=%d, "
-			"ie=%d, soft2=%#x, diag=%#x, pfn=%#x, soft=%#x, l=%d, "
-			"cp=%d, cv=%d, e=%d, p=%d, w=%d, g=%d\n", i, t.vpn,
-			t.context, d.v, d.size, d.nfo, d.ie, d.soft2, d.diag,
-			d.pfn, d.soft, d.l, d.cp, d.cv, d.e, d.p, d.w, d.g);
-	}
-
-}
-
-void do_fast_instruction_access_mmu_miss_fault(istate_t *istate, const char
-	*str)
+		    "ie=%d, soft2=%#x, diag=%#x, pfn=%#x, soft=%#x, l=%d, "
+		    "cp=%d, cv=%d, e=%d, p=%d, w=%d, g=%d\n", i, t.vpn,
+		    t.context, d.v, d.size, d.nfo, d.ie, d.soft2, d.diag,
+		    d.pfn, d.soft, d.l, d.cp, d.cv, d.e, d.p, d.w, d.g);
+	}
+
+}
+
+void do_fast_instruction_access_mmu_miss_fault(istate_t *istate,
+    const char *str)
 {
 	fault_if_from_uspace(istate, "%s\n", str);
@@ -357,13 +366,13 @@
 }
 
-void do_fast_data_access_mmu_miss_fault(istate_t *istate, tlb_tag_access_reg_t
-	tag, const char *str)
+void do_fast_data_access_mmu_miss_fault(istate_t *istate,
+    tlb_tag_access_reg_t tag, const char *str)
 {
 	uintptr_t va;
 
-	va = tag.vpn << PAGE_WIDTH;
+	va = tag.vpn << MMU_PAGE_WIDTH;
 
 	fault_if_from_uspace(istate, "%s, Page=%p (ASID=%d)\n", str, va,
-		tag.context);
+	    tag.context);
 	dump_istate(istate);
 	printf("Faulting page: %p, ASID=%d\n", va, tag.context);
@@ -371,13 +380,13 @@
 }
 
-void do_fast_data_access_protection_fault(istate_t *istate, tlb_tag_access_reg_t
-	tag, const char *str)
+void do_fast_data_access_protection_fault(istate_t *istate,
+    tlb_tag_access_reg_t tag, const char *str)
 {
 	uintptr_t va;
 
-	va = tag.vpn << PAGE_WIDTH;
+	va = tag.vpn << MMU_PAGE_WIDTH;
 
 	fault_if_from_uspace(istate, "%s, Page=%p (ASID=%d)\n", str, va,
-		tag.context);
+	    tag.context);
 	printf("Faulting page: %p, ASID=%d\n", va, tag.context);
 	dump_istate(istate);
@@ -394,6 +403,6 @@
 	
 	printf("DTLB SFSR: asi=%#x, ft=%#x, e=%d, ct=%d, pr=%d, w=%d, ow=%d, "
-		"fv=%d\n", sfsr.asi, sfsr.ft, sfsr.e, sfsr.ct, sfsr.pr, sfsr.w,
-		sfsr.ow, sfsr.fv);
+	    "fv=%d\n", sfsr.asi, sfsr.ft, sfsr.e, sfsr.ct, sfsr.pr, sfsr.w,
+	    sfsr.ow, sfsr.fv);
 	printf("DTLB SFAR: address=%p\n", sfar);
 	
@@ -482,9 +491,9 @@
 	mmu_primary_context_write(ctx.v);
 	
-	for (i = 0; i < cnt; i++) {
+	for (i = 0; i < cnt * MMU_PAGES_PER_PAGE; i++) {
 		itlb_demap(TLB_DEMAP_PAGE, TLB_DEMAP_PRIMARY,
-		    page + i * PAGE_SIZE);
+		    page + i * MMU_PAGE_SIZE);
 		dtlb_demap(TLB_DEMAP_PAGE, TLB_DEMAP_PRIMARY,
-		    page + i * PAGE_SIZE);
+		    page + i * MMU_PAGE_SIZE);
 	}
 	
