Index: arch/sparc64/src/mm/tlb.c
===================================================================
--- arch/sparc64/src/mm/tlb.c	(revision dbb688616f86482bdf0241f217eaf220fbbe49d7)
+++ arch/sparc64/src/mm/tlb.c	(revision 3fc03fd55a97ee8c78eecc373fec290d6bd547d9)
@@ -29,10 +29,61 @@
 #include <arch/mm/tlb.h>
 #include <mm/tlb.h>
+#include <arch/mm/frame.h>
+#include <arch/mm/page.h>
+#include <arch/mm/mmu.h>
 #include <print.h>
 #include <arch/types.h>
 #include <typedefs.h>
+#include <config.h>
 
+/** Initialize ITLB and DTLB.
+ *
+ * The goal of this function is to disable MMU
+ * so that both TLBs can be purged and new
+ * kernel 4M locked entry can be installed.
+ * After TLB is initialized, MMU is enabled
+ * again.
+ */
 void tlb_arch_init(void)
 {
+	tlb_tag_access_reg_t tag;
+	tlb_data_t data;
+	frame_address_t fr;
+	page_address_t pg;
+
+	fr.address = config.base;
+	pg.address = config.base;
+	
+	immu_disable();
+	dmmu_disable();
+	
+	/*
+	 * For simplicity, we do identity mapping of first 4M of memory.
+	 * The very next change should be leaving the first 4M unmapped.
+	 */
+	tag.value = 0;
+	tag.vpn = pg.vpn;
+
+	itlb_tag_access_write(tag.value);
+	dtlb_tag_access_write(tag.value);
+
+	data.value = 0;
+	data.v = true;
+	data.size = PAGESIZE_4M;
+	data.pfn = fr.pfn;
+	data.l = true;
+	data.cp = 1;
+	data.cv = 1;
+	data.p = true;
+	data.w = true;
+	data.g = true;
+
+	itlb_data_in_write(data.value);
+	dtlb_data_in_write(data.value);
+
+	tlb_invalidate_all();
+
+	dmmu_enable();
+	immu_enable();
 }
 
@@ -74,5 +125,4 @@
 		d.value = itlb_data_access_read(i);
 		if (!d.l) {
-			printf("invalidating ");
 			t.value = itlb_tag_read_read(i);
 			d.v = false;
Index: arch/sparc64/src/sparc64.c
===================================================================
--- arch/sparc64/src/sparc64.c	(revision dbb688616f86482bdf0241f217eaf220fbbe49d7)
+++ arch/sparc64/src/sparc64.c	(revision 3fc03fd55a97ee8c78eecc373fec290d6bd547d9)
@@ -29,6 +29,5 @@
 #include <arch.h>
 #include <print.h>
-#include <arch/asm.h>
-#include <memstr.h>
+#include <arch/trap.h>
 #include <arch/trap_table.h>
 #include <arch/console.h>
@@ -45,12 +44,5 @@
 void arch_pre_smp_init(void)
 {
-	/*
-	 * Copy OFW's trap table into kernel and point TBA there.
-	 */
-	memcpy((void *) trap_table, (void *) tba_read(), TRAP_TABLE_SIZE);
-/*
- *	TBA cannot be changed until there are means of getting it into TLB.
- *	tba_write((__u64) trap_table);
- */
+	trap_init();
 }
 
Index: arch/sparc64/src/trap.c
===================================================================
--- arch/sparc64/src/trap.c	(revision 3fc03fd55a97ee8c78eecc373fec290d6bd547d9)
+++ arch/sparc64/src/trap.c	(revision 3fc03fd55a97ee8c78eecc373fec290d6bd547d9)
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2005 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 <arch/trap.h>
+#include <arch/trap_table.h>
+#include <arch/asm.h>
+#include <memstr.h>
+
+void trap_init(void)
+{
+	/*
+	 * Copy OFW's trap table into kernel.
+	 */
+	memcpy((void *) trap_table, (void *) tba_read(), TRAP_TABLE_SIZE);
+
+	/* Point TBA to kernel copy of OFW's trap table. */
+	tba_write((__u64) trap_table);
+}