Index: boot/arch/sparc64/loader/asm.S
===================================================================
--- boot/arch/sparc64/loader/asm.S	(revision 0af4f9e279622de1a47b4ab61926052c53c7d4c6)
+++ boot/arch/sparc64/loader/asm.S	(revision 1eb154f4c7cdc356476ee62eed5380d8b15255d7)
@@ -100,10 +100,12 @@
 jump_to_kernel:
 	/*
-	 * Make sure that the code we have moved has drained to main memory.
-	 * We have a reason to assume that the I-cache does not include the
-	 * code to which we are going to pass control. Hence no I-cache
-	 * invalidation.
+	 * We have copied code and now we need to guarantee cache coherence.
+	 * 1. Make sure that the code we have moved has drained to main memory.
+	 * 2. Invalidate I-cache.
+	 * 3. Flush instruction pipeline.
 	 */ 
-	membar #StoreStore
+	call	icache_flush
+	membar	#StoreStore
+	flush	%i7
 
 	mov %o0, %l1
@@ -112,4 +114,22 @@
 	mov %o3, %o2
 	jmp %l1				! jump to kernel
+	nop
+
+#define ICACHE_SIZE		8192
+#define ICACHE_LINE_SIZE	32
+#define ICACHE_SET_BIT		(1 << 13)
+#define ASI_ICACHE_TAG		0x67
+
+# Flush I-cache
+icache_flush:
+	set	((ICACHE_SIZE - ICACHE_LINE_SIZE) | ICACHE_SET_BIT), %g1
+	stxa	%g0, [%g1] ASI_ICACHE_TAG
+0:	membar	#Sync
+	subcc	%g1, ICACHE_LINE_SIZE, %g1
+	bnz,pt	%xcc, 0b
+	stxa	%g0, [%g1] ASI_ICACHE_TAG
+	membar	#Sync
+	retl
+	! SF Erratum #51
 	nop
 
Index: boot/arch/sparc64/loader/ofwarch.c
===================================================================
--- boot/arch/sparc64/loader/ofwarch.c	(revision 0af4f9e279622de1a47b4ab61926052c53c7d4c6)
+++ boot/arch/sparc64/loader/ofwarch.c	(revision 1eb154f4c7cdc356476ee62eed5380d8b15255d7)
@@ -70,5 +70,7 @@
 	uint64_t current_mid;
 	
-	asm volatile ("ldxa [%1] %2, %0\n" : "=r" (current_mid) : "r" (0), "i" (ASI_UPA_CONFIG));
+	asm volatile ("ldxa [%1] %2, %0\n"
+	    : "=r" (current_mid)
+	    : "r" (0), "i" (ASI_UPA_CONFIG));
 	current_mid >>= UPA_CONFIG_MID_SHIFT;
 	current_mid &= UPA_CONFIG_MID_MASK;
@@ -76,10 +78,13 @@
 	int cpus;
 	
-	for (cpus = 0; node != 0 && node != -1; node = ofw_get_peer_node(node), cpus++) {
-		if (ofw_get_property(node, "device_type", type_name, sizeof(type_name)) > 0) {
+	for (cpus = 0; node != 0 && node != -1; node = ofw_get_peer_node(node),
+	    cpus++) {
+		if (ofw_get_property(node, "device_type", type_name,
+		    sizeof(type_name)) > 0) {
 			if (strcmp(type_name, "cpu") == 0) {
 				uint32_t mid;
 				
-				if (ofw_get_property(node, "upa-portid", &mid, sizeof(mid)) <= 0)
+				if (ofw_get_property(node, "upa-portid", &mid,
+				    sizeof(mid)) <= 0)
 					continue;
 					
@@ -88,7 +93,8 @@
 					 * Start secondary processor.
 					 */
-					(void) ofw_call("SUNW,start-cpu", 3, 1, NULL, node,
-						 KERNEL_VIRTUAL_ADDRESS,
-						 bootinfo.physmem_start | AP_PROCESSOR);
+					(void) ofw_call("SUNW,start-cpu", 3, 1,
+					    NULL, node, KERNEL_VIRTUAL_ADDRESS,
+					    bootinfo.physmem_start |
+					    AP_PROCESSOR);
 				}
 			}