Index: arch/ia64/src/dummy.s
===================================================================
--- arch/ia64/src/dummy.s	(revision 2c49fbbe94d5783a6ef513db167c10a5899f7d07)
+++ arch/ia64/src/dummy.s	(revision a0d74fd54b1285e717ed7f3095dbcf46662e74e9)
@@ -32,6 +32,4 @@
 .global asm_delay_loop
 .global userspace
-.global before_thread_runs_arch
-.global after_thread_ran_arch
 .global cpu_sleep
 .global dummy
@@ -40,6 +38,4 @@
 .global fpu_init
 
-before_thread_runs_arch:
-after_thread_ran_arch:
 userspace:
 calibrate_delay_loop:
Index: arch/ia64/src/mm/page.c
===================================================================
--- arch/ia64/src/mm/page.c	(revision 2c49fbbe94d5783a6ef513db167c10a5899f7d07)
+++ arch/ia64/src/mm/page.c	(revision a0d74fd54b1285e717ed7f3095dbcf46662e74e9)
@@ -56,5 +56,4 @@
 void set_environment(void)
 {
-
 	region_register rr;
 	pta_register pta;	
@@ -63,15 +62,14 @@
 	/*
 	 * First set up kernel region register.
-	 * This action is redundand (see start.S) but I would to keep it to make sure that 
-	 *no unexpected changes will be made. 
+	 * This is redundant (see start.S) but we keep it here just for sure.
 	 */
 	rr.word = rr_read(VRN_KERNEL);
 	rr.map.ve = 0;                  /* disable VHPT walker */
 	rr.map.ps = PAGE_WIDTH;
-	rr.map.rid = ASID2RID(ASID_KERNEL,VRN_KERNEL);  
+	rr.map.rid = ASID2RID(ASID_KERNEL, VRN_KERNEL);
 	rr_write(VRN_KERNEL, rr.word);
 	srlz_i();
 	srlz_d();
-	
+
 	/*
 	 * And invalidate the rest of region register.
@@ -84,5 +82,5 @@
 		rr.word == rr_read(i);
 		rr.map.ve = 0;		/* disable VHPT walker */
-		rr.map.rid = ASID2RID(ASID_INVALID,i);
+		rr.map.rid = RID_INVALID;
 		rr_write(i, rr.word);
 		srlz_i();
@@ -101,8 +99,4 @@
 	srlz_i();
 	srlz_d();
-	
-
-	return ;	
-	
 }
 
Index: arch/ia64/src/mm/tlb.c
===================================================================
--- arch/ia64/src/mm/tlb.c	(revision 2c49fbbe94d5783a6ef513db167c10a5899f7d07)
+++ arch/ia64/src/mm/tlb.c	(revision a0d74fd54b1285e717ed7f3095dbcf46662e74e9)
@@ -32,9 +32,12 @@
 
 #include <mm/tlb.h>
+#include <mm/asid.h>
 #include <arch/mm/tlb.h>
+#include <arch/mm/page.h>
 #include <arch/barrier.h>
 #include <arch/interrupt.h>
 #include <typedefs.h>
 #include <panic.h>
+#include <print.h>
 
 /** Invalidate all TLB entries. */
@@ -85,9 +88,9 @@
 	bool restore_rr = false;
 
-	if (!(entry.not_present.p))
+	if (!(entry.p))
 		return;
 
-	rr.word = rr_read(VA_REGION(va));
-	if ((restore_rr = (rr.map.rid != ASID2RID(asid, VA_REGION(va))))) {
+	rr.word = rr_read(VA2VRN(va));
+	if ((restore_rr = (rr.map.rid != ASID2RID(asid, VA2VRN(va))))) {
 		/*
 		 * The selected region register does not contain required RID.
@@ -97,6 +100,6 @@
 
 		rr0 = rr;
-		rr0.map.rid = ASID2RID(asid, VA_REGION(va));
-		rr_write(VA_REGION(va), rr0.word);
+		rr0.map.rid = ASID2RID(asid, VA2VRN(va));
+		rr_write(VA2VRN(va), rr0.word);
 		srlz_d();
 		srlz_i();
@@ -121,5 +124,5 @@
 	
 	if (restore_rr) {
-		rr_write(VA_REGION(va),rr.word);
+		rr_write(VA2VRN(va), rr.word);
 		srlz_d();
 		srlz_i();
@@ -164,9 +167,9 @@
 	bool restore_rr = false;
 
-	if (!(entry.not_present.p))
+	if (!(entry.p))
 		return;
 
-	rr.word = rr_read(VA_REGION(va));
-	if ((restore_rr = (rr.map.rid != ASID2RID(asid, VA_REGION(va))))) {
+	rr.word = rr_read(VA2VRN(va));
+	if ((restore_rr = (rr.map.rid != ASID2RID(asid, VA2VRN(va))))) {
 		/*
 		 * The selected region register does not contain required RID.
@@ -176,6 +179,6 @@
 
 		rr0 = rr;
-		rr0.map.rid = ASID2RID(asid, VA_REGION(va));
-		rr_write(VA_REGION(va), rr0.word);
+		rr0.map.rid = ASID2RID(asid, VA2VRN(va));
+		rr_write(VA2VRN(va), rr0.word);
 		srlz_d();
 		srlz_i();
@@ -200,8 +203,38 @@
 	
 	if (restore_rr) {
-		rr_write(VA_REGION(va),rr.word);
-		srlz_d();
-		srlz_i();
-	}
+		rr_write(VA2VRN(va), rr.word);
+		srlz_d();
+		srlz_i();
+	}
+}
+
+/** Insert data into DTLB.
+ *
+ * @param va Virtual page address.
+ * @param asid Address space identifier.
+ * @param entry The rest of TLB entry as required by TLB insertion format.
+ * @param dtr If true, insert into data translation register, use data translation cache otherwise.
+ * @param tr Translation register if dtr is true, ignored otherwise.
+ */
+void dtlb_mapping_insert(__address page, __address frame, bool dtr, index_t tr)
+{
+	tlb_entry_t entry;
+	
+	entry.word[0] = 0;
+	entry.word[1] = 0;
+	
+	entry.p = true;			/* present */
+	entry.ma = MA_WRITEBACK;
+	entry.a = true;			/* already accessed */
+	entry.d = true;			/* already dirty */
+	entry.pl = PL_KERNEL;
+	entry.ar = AR_READ | AR_WRITE;
+	entry.ppn = frame >> PPN_SHIFT;
+	entry.ps = PAGE_WIDTH;
+	
+	if (dtr)
+		dtr_mapping_insert(page, ASID_KERNEL, entry, tr);
+	else
+		dtc_mapping_insert(page, ASID_KERNEL, entry);
 }
 
@@ -211,7 +244,29 @@
 }
 
+/** Data TLB fault with VHPT turned off.
+ *
+ * @param vector Interruption vector.
+ * @param pstate Structure with saved interruption state.
+ */
 void alternate_data_tlb_fault(__u64 vector, struct exception_regdump *pstate)
 {
-	panic("%s: %P\n", __FUNCTION__, pstate->cr_ifa);
+	region_register rr;
+	rid_t rid;
+	__address va;
+	
+	va = pstate->cr_ifa;	/* faulting address */
+	rr.word = rr_read(VA2VRN(va));
+	rid = rr.map.rid;
+	if (RID2ASID(rid) == ASID_KERNEL) {
+		if (VA2VRN(va) == VRN_KERNEL) {
+			/*
+			 * Provide KA2PA(identity) mapping for faulting piece of
+			 * kernel address space.
+			 */
+			dtlb_mapping_insert(va, KA2PA(va), false, 0);
+			return;
+		}
+	}
+	panic("%s: va=%P, rid=%d\n", __FUNCTION__, pstate->cr_ifa, rr.map.rid);
 }
 
Index: arch/ia64/src/proc/scheduler.c
===================================================================
--- arch/ia64/src/proc/scheduler.c	(revision a0d74fd54b1285e717ed7f3095dbcf46662e74e9)
+++ arch/ia64/src/proc/scheduler.c	(revision a0d74fd54b1285e717ed7f3095dbcf46662e74e9)
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2006 Jakub Jermar
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <proc/scheduler.h>
+#include <proc/thread.h>
+#include <arch.h>
+#include <arch/mm/tlb.h>
+#include <config.h>
+#include <align.h>
+
+/** Ensure that thread's kernel stack is locked in TLB. */
+void before_thread_runs_arch(void)
+{
+	__address base;
+	
+	base = ALIGN_DOWN(config.base, 1<<KERNEL_PAGE_WIDTH);
+
+	if ((__address) THREAD->kstack < base || (__address) THREAD->kstack > base + (1<<KERNEL_PAGE_WIDTH)) {
+		/*
+		 * Kernel stack of this thread is not locked in DTLB.
+		 * First, make sure it is not mapped already.
+		 * If not, fill respective tranlsation register.
+		 */
+		 dtlb_mapping_insert((__address) THREAD->kstack, KA2PA(THREAD->kstack), true, DTR_KSTACK);
+	}
+}
+
+void after_thread_ran_arch(void)
+{
+}
Index: arch/ia64/src/start.S
===================================================================
--- arch/ia64/src/start.S	(revision 2c49fbbe94d5783a6ef513db167c10a5899f7d07)
+++ arch/ia64/src/start.S	(revision a0d74fd54b1285e717ed7f3095dbcf46662e74e9)
@@ -53,5 +53,5 @@
 	movl r10=(RR_MASK)
 	and r9=r10,r9
-	movl r10=((ASID2RID(ASID_KERNEL,VRN_KERNEL)<<RID_SHIFT)|(KERNEL_PAGE_WIDTH<<PS_SHIFT))
+	movl r10=((RID_KERNEL<<RID_SHIFT)|(KERNEL_PAGE_WIDTH<<PS_SHIFT))
 	or  r9=r10,r9
 	mov rr[r8]=r9
