Index: arch/ia64/include/mm/tlb.h
===================================================================
--- arch/ia64/include/mm/tlb.h	(revision 7d6ec87ecf6aee2d6e6cff8c468f25cd768c892f)
+++ arch/ia64/include/mm/tlb.h	(revision 95042fd2012cd76ab5b4872b670f82844b865b48)
@@ -33,17 +33,24 @@
 #define tlb_print()
 
-
 #include <arch/mm/page.h>
 #include <arch/mm/asid.h>
 #include <arch/register.h>
+#include <typedefs.h>
 
+extern void tc_mapping_insert(__address va, asid_t asid, vhpt_entry_t entry, bool dtc);
+extern void dtc_mapping_insert(__address va, asid_t asid, vhpt_entry_t entry);
+extern void itc_mapping_insert(__address va, asid_t asid, vhpt_entry_t entry);
 
-void tlb_fill_data(__address va,asid_t asid,vhpt_entry_t entry);
-void tlb_fill_code(__address va,asid_t asid,vhpt_entry_t entry);
+extern void tr_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, bool dtr, index_t tr);
+extern void dtr_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, index_t tr);
+extern void itr_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, index_t tr);
 
-void tlb_fill_code_tr(__u64 tr,__address va,asid_t asid,tlb_entry_t entry);
-void tlb_fill_data_tr(__u64 tr,__address va,asid_t asid,tlb_entry_t entry);
-
-
+extern void alternate_instruction_tlb_fault(void);
+extern void alternate_data_tlb_fault(void);
+extern void data_nested_tlb_fault(void);
+extern void data_dirty_bit_fault(void);
+extern void instruction_access_bit_fault(void);
+extern void data_access_bit_fault(void);
+extern void page_not_present(void);
 
 #endif
Index: arch/ia64/src/ivt.S
===================================================================
--- arch/ia64/src/ivt.S	(revision 7d6ec87ecf6aee2d6e6cff8c468f25cd768c892f)
+++ arch/ia64/src/ivt.S	(revision 95042fd2012cd76ab5b4872b670f82844b865b48)
@@ -334,12 +334,12 @@
 	HEAVYWEIGHT_HANDLER 0x0400
 	HEAVYWEIGHT_HANDLER 0x0800
-	HEAVYWEIGHT_HANDLER 0x0c00
-	HEAVYWEIGHT_HANDLER 0x1000
-	HEAVYWEIGHT_HANDLER 0x1400
+	HEAVYWEIGHT_HANDLER 0x0c00 alternate_instruction_tlb_fault
+	HEAVYWEIGHT_HANDLER 0x1000 alternate_data_tlb_fault
+	HEAVYWEIGHT_HANDLER 0x1400 data_nested_tlb_fault
 	HEAVYWEIGHT_HANDLER 0x1800
 	HEAVYWEIGHT_HANDLER 0x1c00
-	HEAVYWEIGHT_HANDLER 0x2000
-	HEAVYWEIGHT_HANDLER 0x2400
-	HEAVYWEIGHT_HANDLER 0x2800
+	HEAVYWEIGHT_HANDLER 0x2000 data_dirty_bit_fault
+	HEAVYWEIGHT_HANDLER 0x2400 instruction_access_bit_fault
+	HEAVYWEIGHT_HANDLER 0x2800 data_access_bit_fault
 	HEAVYWEIGHT_HANDLER 0x2c00 break_instruction
 	HEAVYWEIGHT_HANDLER 0x3000 external_interrupt	/* For external interrupt, heavyweight handler is used. */
@@ -352,5 +352,5 @@
 	HEAVYWEIGHT_HANDLER 0x4c00
 
-	HEAVYWEIGHT_HANDLER 0x5000
+	HEAVYWEIGHT_HANDLER 0x5000 page_not_present
 	HEAVYWEIGHT_HANDLER 0x5100
 	HEAVYWEIGHT_HANDLER 0x5200
Index: arch/ia64/src/mm/tlb.c
===================================================================
--- arch/ia64/src/mm/tlb.c	(revision 7d6ec87ecf6aee2d6e6cff8c468f25cd768c892f)
+++ arch/ia64/src/mm/tlb.c	(revision 95042fd2012cd76ab5b4872b670f82844b865b48)
@@ -34,5 +34,5 @@
 #include <arch/mm/tlb.h>
 #include <arch/barrier.h>
-
+#include <typedefs.h>
 
 /** Invalidate all TLB entries. */
@@ -51,237 +51,191 @@
 }
 
-
-
-void tlb_fill_data(__address va,asid_t asid,tlb_entry_t entry)
+/** Insert data into data translation cache.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ */
+void dtc_mapping_insert(__address va, asid_t asid, tlb_entry_t entry) {
+	tc_mapping_insert(va, asid, entry, true);
+}
+
+/** Insert data into instruction translation cache.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ */
+void itc_mapping_insert(__address va, asid_t asid, tlb_entry_t entry) {
+	tc_mapping_insert(va, asid, entry, false);
+}
+
+/** Insert data into instruction or data translation cache.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ * @param dtc If true, insert into data translation cache, use instruction translation cache otherwise.
+ */
+void tc_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, bool dtc)
 {
 	region_register rr;
-
-
-	if(!(entry.not_present.p)) return;
-
-	rr.word=rr_read(VA_REGION(va));
-
-	if(rr.map.rid==ASID2RID(asid,VA_REGION(va)))
-	{
-		asm volatile
-		(
-			"srlz.i;;\n"
-			"srlz.d;;\n"
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"srlz.i;;\n"
-			"mov cr.ifa=%1\n"        		/*va*/		 
-			"mov cr.itir=%2;;\n"				/*entry.word[1]*/ 
-			"itc.d %3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0])
-			:"r8","r9"
-		);
-	}
-	else
-	{
+	bool restore_rr = false;
+
+	if (!(entry.not_present.p))
+		return;
+
+	rr.word = rr_read(VA_REGION(va));
+	if ((restore_rr = (rr.map.rid != ASID2RID(asid, VA_REGION(va))))) {
+		/*
+		 * The selected region register does not contain required RID.
+		 * Save the old content of the register and replace the RID.
+		 */
 		region_register rr0;
-		rr0=rr;
-		rr0.map.rid=ASID2RID(asid,VA_REGION(va));
-		rr_write(VA_REGION(va),rr0.word);
-		srlz_d();
-		asm volatile
-		(
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"mov cr.ifa=%1\n"        		/*va*/		 
-			"mov cr.itir=%2;;\n"				/*entry.word[1]*/ 
-			"itc.d %3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0])
-			:"r8","r9"
-		);
+
+		rr0 = rr;
+		rr0.map.rid = ASID2RID(asid, VA_REGION(va));
+		rr_write(VA_REGION(va), rr0.word);
+		srlz_d();
+		srlz_i();
+	}
+	
+	__asm__ volatile (
+		"mov r8=psr;;\n"
+		"and r9=r8,%0;;\n"   		/* (~PSR_IC_MASK) */
+		"mov psr.l=r9;;\n"
+		"srlz.d;;\n"
+		"srlz.i;;\n"
+		"mov cr.ifa=%1\n"		/* va */
+		"mov cr.itir=%2;;\n"		/* entry.word[1] */
+		"cmp.eq p6,p7 = %4,r0;;\n"	/* decide between itc and dtc */ 
+		"(p6) itc.i %3;;\n"
+		"(p7) itc.d %3;;\n"
+		"mov psr.l=r8;;\n"
+		"srlz.d;;\n"
+		:
+		: "r" (~PSR_IC_MASK), "r" (va), "r" (entry.word[1]), "r" (entry.word[0]), "r" (dtc)
+		: "p6", "p7", "r8", "r9"
+	);
+	
+	if (restore_rr) {
 		rr_write(VA_REGION(va),rr.word);
-	}
-
-
-}
-
-void tlb_fill_code(__address va,asid_t asid,tlb_entry_t entry)
+		srlz_d();
+		srlz_i();
+	}
+}
+
+/** Insert data into instruction translation register.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ * @param tr Translation register.
+ */
+void itr_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, index_t tr)
+{
+	tr_mapping_insert(va, asid, entry, false, tr);
+}
+
+/** Insert data into data translation register.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ * @param tr Translation register.
+ */
+void dtr_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, index_t tr)
+{
+	tr_mapping_insert(va, asid, entry, true, tr);
+}
+
+/** Insert data into instruction or data translation register.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ * @param dtc If true, insert into data translation register, use instruction translation register otherwise.
+ * @param tr Translation register.
+ */
+void tr_mapping_insert(__address va, asid_t asid, tlb_entry_t entry, bool dtr, index_t tr)
 {
 	region_register rr;
-
-
-	if(!(entry.not_present.p)) return;
-
-	rr.word=rr_read(VA_REGION(va));
-
-	if(rr.map.rid==ASID2RID(asid,VA_REGION(va)))
-	{
-		asm volatile
-		(
-			"srlz.i;;\n"
-			"srlz.d;;\n"
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"srlz.i;;\n"
-			"mov cr.ifa=%1\n"        		/*va*/		 
-			"mov cr.itir=%2;;\n"				/*entry.word[1]*/ 
-			"itc.i %3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0])
-			:"r8","r9"
-		);
-	}
-	else
-	{
+	bool restore_rr = false;
+
+	if (!(entry.not_present.p))
+		return;
+
+	rr.word = rr_read(VA_REGION(va));
+	if ((restore_rr = (rr.map.rid != ASID2RID(asid, VA_REGION(va))))) {
+		/*
+		 * The selected region register does not contain required RID.
+		 * Save the old content of the register and replace the RID.
+		 */
 		region_register rr0;
-		rr0=rr;
-		rr0.map.rid=ASID2RID(asid,VA_REGION(va));
-		rr_write(VA_REGION(va),rr0.word);
-		srlz_d();
-		asm volatile
-		(
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"mov cr.ifa=%1\n"      		/*va*/		 
-			"mov cr.itir=%2;;\n"			/*entry.word[1]*/ 
-			"itc.i %3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0])
-			:"r8","r9"
-		);
+
+		rr0 = rr;
+		rr0.map.rid = ASID2RID(asid, VA_REGION(va));
+		rr_write(VA_REGION(va), rr0.word);
+		srlz_d();
+		srlz_i();
+	}
+
+	__asm__ volatile (
+		"mov r8=psr;;\n"
+		"and r9=r8,%0;;\n"		/* (~PSR_IC_MASK) */
+		"mov psr.l=r9;;\n"
+		"srlz.d;;\n"
+		"srlz.i;;\n"
+		"mov cr.ifa=%1\n"        	/* va */		 
+		"mov cr.itir=%2;;\n"		/* entry.word[1] */ 
+		"cmp.eq p6,p7=%5,r0;;\n"	/* decide between itr and dtr */
+		"(p6) itr.i itr[%4]=%3;;\n"
+		"(p7) itr.d dtr[%4]=%3;;\n"
+		"mov psr.l=r8;;\n"
+		"srlz.d;;\n"
+		:
+		:"r" (~PSR_IC_MASK), "r" (va), "r" (entry.word[1]), "r" (entry.word[0]), "r" (tr), "r" (dtr)
+		: "p6", "p7", "r8", "r9"
+	);
+	
+	if (restore_rr) {
 		rr_write(VA_REGION(va),rr.word);
-	}
-
-
-}
-
-
-void tlb_fill_data_tr(__u64 tr,__address va,asid_t asid,tlb_entry_t entry)
-{
-	region_register rr;
-
-
-	if(!(entry.not_present.p)) return;
-
-	rr.word=rr_read(VA_REGION(va));
-
-	if(rr.map.rid==ASID2RID(asid,VA_REGION(va)))
-	{
-		asm volatile
-		(
-			"srlz.i;;\n"
-			"srlz.d;;\n"
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"srlz.i;;\n"
-			"mov cr.ifa=%1\n"        		/*va*/		 
-			"mov cr.itir=%2;;\n"				/*entry.word[1]*/ 
-			"itr.d dtr[%4]=%3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0]),"r"(tr)
-			:"r8","r9"
-		);
-	}
-	else
-	{
-		region_register rr0;
-		rr0=rr;
-		rr0.map.rid=ASID2RID(asid,VA_REGION(va));
-		rr_write(VA_REGION(va),rr0.word);
-		srlz_d();
-		asm volatile
-		(
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"mov cr.ifa=%1\n"        		/*va*/		 
-			"mov cr.itir=%2;;\n"				/*entry.word[1]*/ 
-			"itr.d dtr[%4]=%3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0]),"r"(tr)
-			:"r8","r9"
-		);
-		rr_write(VA_REGION(va),rr.word);
-	}
-
-
-}
-
-void tlb_fill_code_tr(__u64 tr,__address va,asid_t asid,tlb_entry_t entry)
-{
-	region_register rr;
-
-
-	if(!(entry.not_present.p)) return;
-
-	rr.word=rr_read(VA_REGION(va));
-
-	if(rr.map.rid==ASID2RID(asid,VA_REGION(va)))
-	{
-		asm volatile
-		(
-			"srlz.i;;\n"
-			"srlz.d;;\n"
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"srlz.i;;\n"
-			"mov cr.ifa=%1\n"        		/*va*/		 
-			"mov cr.itir=%2;;\n"				/*entry.word[1]*/ 
-			"itr.i itr[%4]=%3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0]),"r"(tr)
-			:"r8","r9"
-		);
-	}
-	else
-	{
-		region_register rr0;
-		rr0=rr;
-		rr0.map.rid=ASID2RID(asid,VA_REGION(va));
-		rr_write(VA_REGION(va),rr0.word);
-		srlz_d();
-		asm volatile
-		(
-			"mov r8=psr;;\n"
-			"and r9=r8,%0;;\n"   				/*(~PSR_IC_MASK)*/
-			"mov psr.l=r9;;\n"
-			"srlz.d;;\n"
-			"mov cr.ifa=%1\n"      		/*va*/		 
-			"mov cr.itir=%2;;\n"			/*entry.word[1]*/ 
-			"itr.i itr[%4]=%3;;\n"						/*entry.word[0]*/
-			"mov psr.l=r8;;\n"
-			"srlz.d;;\n"
-			:
-			:"r"(~PSR_IC_MASK),"r"(va),"r"(entry.word[1]),"r"(entry.word[0]),"r"(tr)
-			:"r8","r9"
-		);
-		rr_write(VA_REGION(va),rr.word);
-	}
-
-
-}
-
+		srlz_d();
+		srlz_i();
+	}
+}
+
+void alternate_instruction_tlb_fault(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
+
+void alternate_data_tlb_fault(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
+
+void data_nested_tlb_fault(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
+
+void data_dirty_bit_fault(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
+
+void instruction_access_bit_fault(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
+
+void data_access_bit_fault(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
+
+void page_not_present(void)
+{
+	panic("%s\n", __FUNCTION__);
+}
Index: arch/ppc32/src/console.c
===================================================================
--- arch/ppc32/src/console.c	(revision 7d6ec87ecf6aee2d6e6cff8c468f25cd768c892f)
+++ arch/ppc32/src/console.c	(revision 95042fd2012cd76ab5b4872b670f82844b865b48)
@@ -34,5 +34,5 @@
 
 
-/** Initialize console to use ofw output */
+/** Initialize console to use frame buffer. */
 void ppc32_console_init(void)
 {