Index: kernel/arch/sparc64/src/mm/tlb.c
===================================================================
--- kernel/arch/sparc64/src/mm/tlb.c	(revision f47fd19f9885f194c0b816b7814357789c9565e2)
+++ kernel/arch/sparc64/src/mm/tlb.c	(revision a796127170a8ce9b89fe78fa9fb1b13f5b742b74)
@@ -51,6 +51,8 @@
 #include <symtab.h>
 
-static void dtlb_pte_copy(pte_t *t);
+static void dtlb_pte_copy(pte_t *t, bool ro);
+static void itlb_pte_copy(pte_t *t);
 static void do_fast_data_access_mmu_miss_fault(istate_t *istate, const char *str);
+static void do_fast_instruction_access_mmu_miss_fault(istate_t *istate, const char *str);
 
 char *context_encoding[] = {
@@ -106,6 +108,67 @@
 }
 
-void dtlb_pte_copy(pte_t *t)
-{
+/** 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)
+{
+	tlb_tag_access_reg_t tag;
+	tlb_data_t data;
+	page_address_t pg;
+	frame_address_t fr;
+
+	pg.address = t->page;
+	fr.address = t->frame;
+
+	tag.value = 0;
+	tag.context = t->as->asid;
+	tag.vpn = pg.vpn;
+	
+	dtlb_tag_access_write(tag.value);
+	
+	data.value = 0;
+	data.v = true;
+	data.size = PAGESIZE_8K;
+	data.pfn = fr.pfn;
+	data.l = false;
+	data.cp = t->c;
+	data.cv = t->c;
+	data.p = t->p;
+	data.w = ro ? false : t->w;
+	data.g = t->g;
+	
+	dtlb_data_in_write(data.value);
+}
+
+void itlb_pte_copy(pte_t *t)
+{
+	tlb_tag_access_reg_t tag;
+	tlb_data_t data;
+	page_address_t pg;
+	frame_address_t fr;
+
+	pg.address = t->page;
+	fr.address = t->frame;
+
+	tag.value = 0;
+	tag.context = t->as->asid;
+	tag.vpn = pg.vpn;
+	
+	itlb_tag_access_write(tag.value);
+	
+	data.value = 0;
+	data.v = true;
+	data.size = PAGESIZE_8K;
+	data.pfn = fr.pfn;
+	data.l = false;
+	data.cp = t->c;
+	data.cv = t->c;
+	data.p = t->p;
+	data.w = false;
+	data.g = t->g;
+	
+	itlb_data_in_write(data.value);
 }
 
@@ -113,5 +176,26 @@
 void fast_instruction_access_mmu_miss(int n, istate_t *istate)
 {
-	panic("%s\n", __FUNCTION__);
+	uintptr_t va = ALIGN_DOWN(istate->tpc, PAGE_SIZE);
+	pte_t *t;
+
+	page_table_lock(AS, true);
+	t = page_mapping_find(AS, va);
+	if (t && PTE_EXECUTABLE(t)) {
+		/*
+		 * The mapping was found in the software page hash table.
+		 * Insert it into ITLB.
+		 */
+		t->a = true;
+		itlb_pte_copy(t);
+		page_table_unlock(AS, true);
+	} else {
+		/*
+		 * Forward the page fault to the address space page fault handler.
+		 */		
+		page_table_unlock(AS, true);
+		if (as_page_fault(va, PF_ACCESS_EXEC, istate) == AS_PF_FAULT) {
+			do_fast_instruction_access_mmu_miss_fault(istate, __FUNCTION__);
+		}
+	}
 }
 
@@ -145,5 +229,6 @@
 		 * Insert it into DTLB.
 		 */
-		dtlb_pte_copy(t);
+		t->a = true;
+		dtlb_pte_copy(t, true);
 		page_table_unlock(AS, true);
 	} else {
@@ -191,4 +276,12 @@
 }
 
+void do_fast_instruction_access_mmu_miss_fault(istate_t *istate, const char *str)
+{
+	char *tpc_str = get_symtab_entry(istate->tpc);
+
+	printf("TPC=%p, (%s)\n", istate->tpc, tpc_str);
+	panic("%s\n", str);
+}
+
 void do_fast_data_access_mmu_miss_fault(istate_t *istate, const char *str)
 {
Index: kernel/arch/sparc64/src/start.S
===================================================================
--- kernel/arch/sparc64/src/start.S	(revision f47fd19f9885f194c0b816b7814357789c9565e2)
+++ kernel/arch/sparc64/src/start.S	(revision a796127170a8ce9b89fe78fa9fb1b13f5b742b74)
@@ -36,6 +36,4 @@
 .register %g2, #scratch
 .register %g3, #scratch
-.register %g6, #scratch
-.register %g7, #scratch
 
 .section K_TEXT_START, "ax"
@@ -154,5 +152,5 @@
 	 */
 	
-	set kernel_image_start, %g7
+	set kernel_image_start, %g5
 	
 	! write ITLB tag of context 1
@@ -160,20 +158,20 @@
 	set VA_DMMU_TAG_ACCESS, %g2
 	stxa %g1, [%g2] ASI_IMMU
-	flush %g7
+	flush %g5
 
 	! write ITLB data and install the temporary mapping in context 1
 	SET_TLB_DATA(g1, g2, 0)			! use non-global mapping
 	stxa %g1, [%g0] ASI_ITLB_DATA_IN_REG		
-	flush %g7
+	flush %g5
 	
 	! switch to context 1
 	set MEM_CONTEXT_TEMP, %g1
 	stxa %g1, [VA_PRIMARY_CONTEXT_REG] %asi	! ASI_DMMU is correct here !!!
-	flush %g7
+	flush %g5
 	
 	! demap context 0
 	SET_TLB_DEMAP_CMD(g1, TLB_DEMAP_NUCLEUS)
 	stxa %g0, [%g1] ASI_IMMU_DEMAP			
-	flush %g7
+	flush %g5
 	
 	! write ITLB tag of context 0
@@ -181,14 +179,14 @@
 	set VA_DMMU_TAG_ACCESS, %g2
 	stxa %g1, [%g2] ASI_IMMU
-	flush %g7
+	flush %g5
 
 	! write ITLB data and install the permanent kernel mapping in context 0
 	SET_TLB_DATA(g1, g2, 0)			! use non-global mapping
 	stxa %g1, [%g0] ASI_ITLB_DATA_IN_REG		
-	flush %g7
+	flush %g5
 
 	! switch to context 0
 	stxa %g0, [VA_PRIMARY_CONTEXT_REG] %asi	! ASI_DMMU is correct here !!!
-	flush %g7
+	flush %g5
 
 	! ensure nucleus mapping
@@ -198,14 +196,14 @@
 	set MEM_CONTEXT_TEMP, %g1
 	stxa %g1, [VA_PRIMARY_CONTEXT_REG] %asi	! ASI_DMMU is correct here !!!
-	flush %g7
+	flush %g5
 
 	! demap context 1
 	SET_TLB_DEMAP_CMD(g1, TLB_DEMAP_PRIMARY)
 	stxa %g0, [%g1] ASI_IMMU_DEMAP			
-	flush %g7
+	flush %g5
 	
 	! set context 0 in the primary context register
 	stxa %g0, [VA_PRIMARY_CONTEXT_REG] %asi	! ASI_DMMU is correct here !!!
-	flush %g7
+	flush %g5
 	
 	! set TL back to 0
Index: kernel/arch/sparc64/src/trap/trap_table.S
===================================================================
--- kernel/arch/sparc64/src/trap/trap_table.S	(revision f47fd19f9885f194c0b816b7814357789c9565e2)
+++ kernel/arch/sparc64/src/trap/trap_table.S	(revision a796127170a8ce9b89fe78fa9fb1b13f5b742b74)
@@ -33,6 +33,4 @@
 .register %g2, #scratch
 .register %g3, #scratch
-.register %g6, #scratch
-.register %g7, #scratch 
 
 .text
@@ -204,5 +202,5 @@
 .global spill_0_normal
 spill_0_normal:
-	SPILL_NORMAL_HANDLER
+	SPILL_NORMAL_HANDLER_KERNEL
 
 /* TT = 0xc0, TL = 0, fill_0_normal handler */
@@ -210,5 +208,5 @@
 .global fill_0_normal
 fill_0_normal:
-	FILL_NORMAL_HANDLER
+	FILL_NORMAL_HANDLER_KERNEL
 
 /*
@@ -268,5 +266,5 @@
 .global spill_0_normal_high
 spill_0_normal_high:
-	SPILL_NORMAL_HANDLER
+	SPILL_NORMAL_HANDLER_KERNEL
 
 /* TT = 0xc0, TL > 0, fill_0_normal handler */
@@ -274,5 +272,5 @@
 .global fill_0_normal_high
 fill_0_normal_high:
-	FILL_NORMAL_HANDLER
+	FILL_NORMAL_HANDLER_KERNEL
 
 
@@ -280,34 +278,73 @@
  *
  * This trap handler makes arrangements to make calling of scheduler() from
- * within a trap context possible. It is guaranteed to function only when traps
- * are not nested (i.e. for TL=1).
+ * within a trap context possible. It is called from several other trap
+ * handlers.
  *
- * Every trap handler on TL=1 that makes a call to the scheduler needs to
- * be based on this function. The reason behind it is that the nested
- * trap levels and the automatic saving of the interrupted context by hardware
- * does not work well together with scheduling (i.e. a thread cannot be rescheduled
- * with TL>0). Therefore it is necessary to eliminate the effect of trap levels
- * by software and save the necessary state on the kernel stack.
- *
- * Note that for traps with TL>1, more state needs to be saved. This function
- * is therefore not going to work when TL>1.
- *
- * The caller is responsible for doing SAVE and allocating
- * PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE bytes on the stack.
+ * This function can be entered either with interrupt globals or alternate globals.
+ * Memory management trap handlers are obliged to switch to one of those global sets
+ * prior to calling this function. Register window management functions are not
+ * allowed to modify the alternate global registers.
  *
  * Input registers:
- * 	%l0	 	Address of function to call.
- * Output registers:
- *	 %l1 - %l7 	Copy of %g1 - %g7
+ *	%g1		Address of function to call.
+ * 	%g2	 	Argument for the function.
+ *	%g6		Pre-set as kernel stack base if trap from userspace.
+ *	%g7		Reserved.
  */
 .global preemptible_handler
 preemptible_handler:
-	/*
-	 * Save TSTATE, TPC, TNPC and PSTATE aside.
+	rdpr %tstate, %g3
+	andcc %g3, TSTATE_PRIV_BIT, %g0		! if this trap came from the privileged mode...
+	bnz 0f					! ...skip setting of kernel stack and primary context
+	nop
+
+	/*
+	 * Switch to kernel stack. The old stack is
+	 * automatically saved in the old window's %sp
+	 * and the new window's %fp.
+	 */
+	save %g6, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
+
+	/*
+	 * Mark the CANSAVE windows as OTHER windows.
+	 * Set CLEANWIN to NWINDOW-1 so that clean_window traps do not occur.
+	 */
+	rdpr %cansave, %l0
+	wrpr %l0, %otherwin
+	wrpr %g0, %cansave
+	wrpr %g0, NWINDOW-1, %cleanwin
+
+	/*
+	 * Switch to primary context 0.
+	 */
+	mov VA_PRIMARY_CONTEXT_REG, %l0
+        stxa %g0, [%l0] ASI_DMMU
+	set kernel_image_start, %l0
+        flush %l0
+
+	ba 1f
+	nop
+
+0:
+	save %sp, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
+
+	/*
+	 * At this moment, we are using the kernel stack 
+	 * and have successfully allocated a register window.
+	 */
+1:
+
+	/*
+	 * Copy arguments.
+	 */
+	mov %g1, %l0
+	mov %g2, %o0
+
+	/*
+	 * Save TSTATE, TPC and TNPC aside.
 	 */
 	rdpr %tstate, %g1
 	rdpr %tpc, %g2
 	rdpr %tnpc, %g3
-	rdpr %pstate, %g4
 
 	/*
@@ -316,48 +353,20 @@
 	 * the kernel stack of THREAD locked in DTLB.
 	 */
-	stx %g1, [%fp + STACK_BIAS + SAVED_TSTATE]
-	stx %g2, [%fp + STACK_BIAS + SAVED_TPC]
-	stx %g3, [%fp + STACK_BIAS + SAVED_TNPC]
-	stx %g4, [%fp + STACK_BIAS + SAVED_PSTATE]
+	stx %g1, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TSTATE]
+	stx %g2, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TPC]
+	stx %g3, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC]
 	
-	/*
-	 * Write 0 to TL.
-	 */
 	wrpr %g0, 0, %tl
-	
-	/*
-	 * Alter PSTATE.
-	 * - switch to normal globals.
-	 */
-	and %g4, ~(PSTATE_AG_BIT|PSTATE_IG_BIT|PSTATE_MG_BIT), %g4
-	wrpr %g4, 0, %pstate
-	 
-	/*
-	 * Save the normal globals.
-	 */
+	wrpr %g0, PSTATE_PRIV_BIT, %pstate
 	SAVE_GLOBALS
 	
 	/*
-	 * Call the higher-level handler.
-	 */
-	mov %fp, %o1				! calculate istate address
+	 * Call the higher-level handler and pass istate as second parameter.
+	 */
 	call %l0
-	add %o1, STACK_BIAS + SAVED_PSTATE, %o1	! calculate istate address
-	
-	/*
-	 * Restore	 the normal global register set.
-	 */
+	add %sp, PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC, %o1
+
 	RESTORE_GLOBALS
-	
-	/*
-	 * Restore PSTATE from saved copy.
-	 * Alternate/Interrupt/MM globals become active.
-	 */
-	ldx [%fp + STACK_BIAS + SAVED_PSTATE], %l4
-	wrpr %l4, 0, %pstate
-	
-	/*
-	 * Write 1 to TL.
-	 */
+	wrpr %g0, PSTATE_AG_BIT | PSTATE_PRIV_BIT, %pstate
 	wrpr %g0, 1, %tl
 	
@@ -365,23 +374,8 @@
 	 * Read TSTATE, TPC and TNPC from saved copy.
 	 */
-	ldx [%fp + STACK_BIAS + SAVED_TSTATE], %g1
-	ldx [%fp + STACK_BIAS + SAVED_TPC], %g2
-	ldx [%fp + STACK_BIAS + SAVED_TNPC], %g3
-
-	/*
-	 * Do restore to match the save instruction from the top-level handler.
-	 */
-	restore
-
-	/*
-	 * On execution of the RETRY instruction, CWP will be restored from the TSTATE
-	 * register. However, because of scheduling, it is possible that CWP in the saved
-	 * TSTATE is different from the current CWP. The following chunk of code fixes
-	 * CWP in the saved copy of TSTATE.
-	 */
-	rdpr %cwp, %g4		! read current CWP
-	and %g1, ~0x1f, %g1	! clear CWP field in saved TSTATE
-	or %g1, %g4, %g1	! write current CWP to TSTATE
-	
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TSTATE], %g1
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TPC], %g2
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC], %g3
+
 	/*
 	 * Restore TSTATE, TPC and TNPC from saved copies.
@@ -390,7 +384,45 @@
 	wrpr %g2, 0, %tpc
 	wrpr %g3, 0, %tnpc
-	 
-	/*
-	 * Return from interrupt.
-	 */
+
+	/*
+	 * If OTHERWIN is zero, then all the userspace windows have been
+	 * spilled to kernel memory (i.e. register window buffer). If
+	 * OTHERWIN is non-zero, then some userspace windows are still
+	 * valid. Others might have been spilled. However, the CWP pointer
+	 * needs no fixing because the scheduler had not been called.
+	 */
+	rdpr %otherwin, %l0
+	brnz %l0, 0f
+	nop
+
+	/*
+	 * OTHERWIN == 0
+	 */
+
+	/*
+	 * If TSTATE.CWP + 1 == CWP, then we still do not have to fix CWP.
+	 */
+	and %g1, TSTATE_CWP_MASK, %l0
+	inc %l0
+	and %l0, TSTATE_CWP_MASK, %l0	! %l0 mod NWINDOW
+	rdpr %cwp, %l1
+	cmp %l0, %l1
+	bz 0f				! CWP is ok
+	nop
+
+	/*
+	 * Fix CWP.
+	 */
+	mov %fp, %g1
+	flushw
+	wrpr %l0, 0, %cwp
+	mov %g1, %fp
+	
+	/*
+	 * OTHERWIN != 0 or fall-through from the OTHERWIN == 0 case.
+	 */
+0:
+	! TODO: restore register windows from register window memory buffer
+
+	restore
 	retry
