Index: boot/arch/sparc64/src/asm.S
===================================================================
--- boot/arch/sparc64/src/asm.S	(revision 4872160f9c176d257f1757469d1adec8033d95f9)
+++ boot/arch/sparc64/src/asm.S	(revision 4872160f9c176d257f1757469d1adec8033d95f9)
@@ -0,0 +1,242 @@
+#
+# Copyright (c) 2006 Martin Decky
+# 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 <arch/arch.h>
+
+#define ICACHE_SIZE       8192
+#define ICACHE_LINE_SIZE  32
+#define ICACHE_SET_BIT    (1 << 13)
+#define ASI_ICACHE_TAG    0x67
+
+.register %g2, #scratch
+.register %g3, #scratch
+.register %g6, #scratch
+.register %g7, #scratch
+
+.global start
+.global halt
+.global memcpy
+.global jump_to_kernel
+
+.section BOOTSTRAP, "ax"
+
+start:
+	ba %xcc, 1f
+	nop
+
+/*
+ * This header forces SILO to load the image at 0x4000.
+ * More precisely, SILO will think this is an old version of Linux.
+ */
+.ascii "HdrS"
+.word 0
+.half 0
+.half 0
+.half 0
+.half 0
+.word 0
+.word 0
+
+.align 8
+1:
+	! Disable interrupts and disable address masking.
+	
+	wrpr %g0, PSTATE_PRIV_BIT, %pstate
+	
+	wrpr %g0, NWINDOWS - 2, %cansave   ! Set maximum saveable windows
+	wrpr %g0, 0, %canrestore           ! Get rid of windows we will never need again
+	wrpr %g0, 0, %otherwin             ! Make sure the window state is consistent
+	wrpr %g0, NWINDOWS - 1, %cleanwin  ! Prevent needless clean_window traps for kernel
+	
+	set initial_stack, %sp
+	add %sp, -STACK_BIAS, %sp
+	
+	set ofw_cif, %l0
+	
+	! Initialize OpenFirmware
+	
+	call ofw_init
+	stx %o4, [%l0]
+	
+	ba %xcc, bootstrap
+	nop
+
+.align STACK_ALIGNMENT
+	.space STACK_SIZE
+initial_stack:
+	.space STACK_WINDOW_SAVE_AREA_SIZE
+
+.text
+
+halt:
+	ba %xcc, halt
+	nop
+
+memcpy:
+	! Save dst
+	
+	mov %o0, %o3
+	add %o1, 7, %g1
+	and %g1, -8, %g1
+	cmp %o1, %g1
+	be,pn %xcc, 3f
+	add %o0, 7, %g1
+	mov 0, %g3
+	
+	0:
+		brz,pn %o2, 2f
+		mov 0, %g2
+	
+	1:
+		ldub [%g3 + %o1], %g1
+		add %g2, 1, %g2
+		cmp %o2, %g2
+		stb %g1, [%g3 + %o0]
+		bne,pt %xcc, 1b
+		mov %g2, %g3
+	
+	2:
+		! Exit point
+		
+		jmp %o7 + 8
+		mov %o3, %o0
+	
+	3:
+		and %g1, -8, %g1
+		cmp %o0, %g1
+		bne,pt %xcc, 0b
+		mov 0, %g3
+		srlx %o2, 3, %g4
+		brz,pn %g4, 5f
+		mov 0, %g5
+	
+	4:
+		sllx %g3, 3, %g2
+		add %g5, 1, %g3
+		ldx [%o1 + %g2], %g1
+		mov %g3, %g5
+		cmp %g4, %g3
+		bne,pt %xcc, 4b
+		stx %g1, [%o0 + %g2]
+	
+	5:
+		and %o2, 7, %o2
+		brz,pn %o2, 2b
+		sllx %g4, 3, %g1
+		mov 0, %g2
+		add %g1, %o0, %o0
+		add %g1, %o1, %g4
+		mov 0, %g3
+	
+	6:
+		ldub [%g2 + %g4], %g1
+		stb %g1, [%g2 + %o0]
+		add %g3, 1, %g2
+		cmp %o2, %g2
+		bne,pt %xcc, 6b
+		mov %g2, %g3
+	
+	! Exit point
+	
+	jmp %o7 + 8
+	mov %o3, %o0
+
+jump_to_kernel:
+	/*
+	 * 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.
+	 */
+	
+	/*
+	 * US3 processors have a write-invalidate cache, so explicitly
+	 * invalidating it is not required. Whether to invalidate I-cache
+	 * or not is decided according to the value of the 3rd argument
+	 * (subarch).
+	 */
+	cmp %i2, SUBARCH_US3
+	be %xcc, 1f
+	nop
+	
+	0:
+		call icache_flush
+		nop
+	
+	1:
+		membar #StoreStore
+	
+	/*
+	 * Flush the instruction pipeline.
+	 */
+	flush %i7
+	
+	! Jump to kernel
+	mov %o0, %l1
+	mov %o1, %o0
+	mov %o3, %o1
+	
+	jmp %l1
+	nop
+
+# 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
+
+.global ofw
+ofw:
+	save %sp, -STACK_WINDOW_SAVE_AREA_SIZE, %sp
+	set ofw_cif, %l0
+	ldx [%l0], %l0
+	
+	rdpr %pstate, %l1
+	and %l1, ~PSTATE_AM_BIT, %l2
+	wrpr %l2, 0, %pstate
+	
+	jmpl %l0, %o7
+	mov %i0, %o0
+	
+	wrpr %l1, 0, %pstate
+	
+	ret
+	restore %o0, 0, %o0
Index: boot/arch/sparc64/src/main.c
===================================================================
--- boot/arch/sparc64/src/main.c	(revision 4872160f9c176d257f1757469d1adec8033d95f9)
+++ boot/arch/sparc64/src/main.c	(revision 4872160f9c176d257f1757469d1adec8033d95f9)
@@ -0,0 +1,309 @@
+/*
+ * Copyright (c) 2005 Martin Decky
+ * 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 <arch/main.h>
+#include <arch/arch.h>
+#include <arch/asm.h>
+#include <arch/ofw.h>
+#include <arch/_components.h>
+#include <genarch/ofw.h>
+#include <genarch/ofw_tree.h>
+#include <halt.h>
+#include <printf.h>
+#include <memstr.h>
+#include <version.h>
+#include <macros.h>
+#include <align.h>
+#include <str.h>
+#include <errno.h>
+#include <inflate.h>
+
+/* The lowest ID (read from the VER register) of some US3 CPU model */
+#define FIRST_US3_CPU  0x14
+
+/* The greatest ID (read from the VER register) of some US3 CPU model */
+#define LAST_US3_CPU  0x19
+
+/* UltraSPARC IIIi processor implementation code */
+#define US_IIIi_CODE  0x15
+
+#define OBP_BIAS  0x400000
+
+#define BALLOC_MAX_SIZE  131072
+
+#define TOP2ADDR(top)  (((void *) KERNEL_ADDRESS) + (top))
+
+/** UltraSPARC architecture */
+static uint8_t arch;
+
+/** UltraSPARC subarchitecture */
+static uint8_t subarch;
+
+/** Mask of the MID field inside the ICBUS_CONFIG register
+ *
+ * Shifted by MID_SHIFT bits to the right
+ *
+ */
+static uint16_t mid_mask;
+
+static bootinfo_t bootinfo;
+
+/** Detect the UltraSPARC architecture
+ *
+ * Detection is done by inspecting the property called "compatible"
+ * in the OBP root node. Currently sun4u and sun4v are supported.
+ * Set global variable "arch" to the correct value.
+ *
+ */
+static void arch_detect(void)
+{
+	phandle root = ofw_find_device("/");
+	char compatible[OFW_TREE_PROPERTY_MAX_VALUELEN];
+	
+	if (ofw_get_property(root, "compatible", compatible,
+	    OFW_TREE_PROPERTY_MAX_VALUELEN) <= 0) {
+		printf("Warning: Unable to determine architecture, assuming sun4u.\n");
+		arch = ARCH_SUN4U;
+		return;
+	}
+	
+	if (str_cmp(compatible, "sun4v") != 0) {
+		/*
+		 * As not all sun4u machines have "sun4u" in their "compatible"
+		 * OBP property (e.g. Serengeti's OBP "compatible" property is
+		 * "SUNW,Serengeti"), we will by default fallback to sun4u if
+		 * an unknown value of the "compatible" property is encountered.
+		 */
+		printf("Warning: Unknown architecture, assuming sun4u.\n");
+		arch = ARCH_SUN4U;
+	} else
+		arch = ARCH_SUN4V;
+}
+
+
+/** Detect the subarchitecture (US, US3) of sun4u
+ *
+ * Set the global variables "subarch" and "mid_mask" to
+ * correct values.
+ *
+ */
+static void sun4u_subarch_detect(void)
+{
+	uint64_t ver;
+	asm volatile (
+		"rdpr %%ver, %[ver]\n"
+		: [ver] "=r" (ver)
+	);
+	
+	ver = (ver << 16) >> 48;
+	
+	if ((ver >= FIRST_US3_CPU) && (ver <= LAST_US3_CPU)) {
+		subarch = SUBARCH_US3;
+		
+		if (ver == US_IIIi_CODE)
+			mid_mask = (1 << 5) - 1;
+		else
+			mid_mask = (1 << 10) - 1;
+		
+	} else if (ver < FIRST_US3_CPU) {
+		subarch = SUBARCH_US;
+		mid_mask = (1 << 5) - 1;
+	} else {
+		printf("Warning: This CPU is not supported.");
+		subarch = SUBARCH_UNKNOWN;
+	}
+}
+
+/** Perform sun4u-specific SMP initialization.
+ *
+ */
+static void sun4u_smp(void)
+{
+#ifdef CONFIG_AP
+	printf("Checking for secondary processors ...\n");
+	ofw_cpu(mid_mask, bootinfo.physmem_start);
+#endif
+}
+
+/** Perform sun4v-specific fixups.
+ *
+ */
+static void sun4v_fixups(void)
+{
+	/*
+	 * When SILO booted, the OBP had established a virtual to physical
+	 * memory mapping. This mapping is not an identity since the
+	 * physical memory starts at non-zero address.
+	 *
+	 * However, the mapping even does not map virtual address 0
+	 * onto the starting address of the physical memory, but onto an
+	 * address which is 0x400000 (OBP_BIAS) bytes higher.
+	 *
+	 * The reason is that the OBP had already used the memory just
+	 * at the beginning of the physical memory. Thus that memory cannot
+	 * be used by SILO (nor the bootloader).
+	 *
+	 * So far, we solve it by a nasty workaround:
+	 *
+	 * We pretend that the physical memory starts 0x400000 (OBP_BIAS)
+	 * bytes further than it actually does (and hence pretend that the
+	 * physical memory is 0x400000 bytes smaller). Of course, the value
+	 * 0x400000 will most probably depend on the machine and OBP version
+	 * (the workaround works on Simics).
+	 *
+	 * A proper solution would be to inspect the "available" property
+	 * of the "/memory" node to find out which parts of memory
+	 * are used by OBP and redesign the algorithm of copying
+	 * kernel/init tasks/ramdisk from the bootable image to memory
+	 * (which we must do anyway because of issues with claiming the memory
+	 * on Serengeti).
+	 *
+	 */
+	bootinfo.physmem_start += OBP_BIAS;
+	bootinfo.memmap.zones[0].start += OBP_BIAS;
+	bootinfo.memmap.zones[0].size -= OBP_BIAS;
+}
+
+void bootstrap(void)
+{
+	version_print();
+	
+	arch_detect();
+	if (arch == ARCH_SUN4U)
+		sun4u_subarch_detect();
+	else
+		subarch = SUBARCH_UNKNOWN;
+	
+	bootinfo.physmem_start = ofw_get_physmem_start();
+	ofw_memmap(&bootinfo.memmap);
+	
+	void *bootinfo_pa = ofw_translate(&bootinfo);
+	void *loader_address_pa = ofw_translate((void *) LOADER_ADDRESS);
+	
+	printf("\nMemory statistics (total %llu MB, starting at %p)\n",
+	    bootinfo.memmap.total >> 20, bootinfo.physmem_start);
+	printf(" %p|%p: boot info structure\n", &bootinfo, bootinfo_pa);
+	printf(" %p|%p: kernel entry point\n", KERNEL_ADDRESS, KERNEL_ADDRESS);
+	printf(" %p|%p: loader entry pount\n", LOADER_ADDRESS, loader_address_pa);
+	
+	size_t i;
+	for (i = 0; i < COMPONENTS; i++)
+		printf(" %p|%p: %s image (%u/%u bytes)\n", components[i].start,
+		    ofw_translate(components[i].start), components[i].name,
+		    components[i].inflated, components[i].size);
+	
+	void *dest[COMPONENTS];
+	size_t top = KERNEL_ADDRESS;
+	size_t cnt = 0;
+	bootinfo.taskmap.cnt = 0;
+	for (i = 0; i < min(COMPONENTS, TASKMAP_MAX_RECORDS); i++) {
+		top = ALIGN_UP(top, PAGE_SIZE);
+		
+		if (i > 0) {
+			bootinfo.taskmap.tasks[bootinfo.taskmap.cnt].addr =
+			    (void *) top;
+			bootinfo.taskmap.tasks[bootinfo.taskmap.cnt].size =
+			    components[i].inflated;
+			
+			str_cpy(bootinfo.taskmap.tasks[bootinfo.taskmap.cnt].name,
+			    BOOTINFO_TASK_NAME_BUFLEN, components[i].name);
+			
+			bootinfo.taskmap.cnt++;
+		}
+		
+		dest[i] = (void *) top;
+		top += components[i].inflated;
+		cnt++;
+	}
+	
+	printf("\nInflating components ... ");
+	
+	for (i = cnt; i > 0; i--) {
+		printf("%s ", components[i - 1].name);
+		
+		/*
+		 * At this point, we claim the physical memory that we are
+		 * going to use. We should be safe in case of the virtual
+		 * address space because the OpenFirmware, according to its
+		 * SPARC binding, should restrict its use of virtual memory
+		 * to addresses from [0xffd00000; 0xffefffff] and
+		 * [0xfe000000; 0xfeffffff].
+		 *
+		 * We don't map this piece of memory. We simply rely on
+		 * SILO to have it done for us already in this case.
+		 *
+		 * XXX SILO only maps 8 MB for us here. We should improve
+		 *     this code to be totally independent on the behavior
+		 *     of SILO.
+		 *
+		 */
+		ofw_claim_phys(bootinfo.physmem_start + dest[i - 1],
+		    ALIGN_UP(components[i - 1].inflated, PAGE_SIZE));
+		
+		int err = inflate(components[i - 1].start, components[i - 1].size,
+		    dest[i - 1], components[i - 1].inflated);
+		
+		if (err != EOK) {
+			printf("\n%s: Inflating error %d, halting.\n",
+			    components[i - 1].name, err);
+			halt();
+		}
+	}
+	
+	printf(".\n");
+	
+	/*
+	 * Claim and map the physical memory for the boot allocator.
+	 * Initialize the boot allocator.
+	 */
+	printf("Setting up boot allocator ...\n");
+	void *balloc_base = (void *) ALIGN_UP(top, PAGE_SIZE);
+	ofw_claim_phys(bootinfo.physmem_start + balloc_base, BALLOC_MAX_SIZE);
+	ofw_map(bootinfo.physmem_start + balloc_base, balloc_base,
+	    BALLOC_MAX_SIZE, -1);
+	balloc_init(&bootinfo.ballocs, balloc_base, (uintptr_t) balloc_base,
+	    BALLOC_MAX_SIZE);
+	
+	printf("Setting up screens ...\n");
+	ofw_setup_screens();
+	
+	printf("Canonizing OpenFirmware device tree ...\n");
+	bootinfo.ofw_root = ofw_tree_build();
+	
+	if (arch == ARCH_SUN4U)
+		sun4u_smp();
+	
+	if (arch == ARCH_SUN4V)
+		sun4v_fixups();
+	
+	printf("Booting the kernel ...\n");
+	ofw_quiesce();
+	jump_to_kernel((void *) KERNEL_ADDRESS, &bootinfo, subarch,
+	    bootinfo.physmem_start | BSP_PROCESSOR);
+}
Index: boot/arch/sparc64/src/ofw.c
===================================================================
--- boot/arch/sparc64/src/ofw.c	(revision 4872160f9c176d257f1757469d1adec8033d95f9)
+++ boot/arch/sparc64/src/ofw.c	(revision 4872160f9c176d257f1757469d1adec8033d95f9)
@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2005 Martin Decky
+ * 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.
+ */
+
+/**
+ * @file
+ * @brief Architecture dependent parts of OpenFirmware interface.
+ */
+
+#include <arch/arch.h>
+#include <arch/ofw.h>
+#include <genarch/ofw.h>
+#include <typedefs.h>
+#include <printf.h>
+#include <halt.h>
+#include <putchar.h>
+#include <str.h>
+
+void putchar(const wchar_t ch)
+{
+	if (ch == '\n')
+		ofw_putchar('\r');
+	
+	if (ascii_check(ch))
+		ofw_putchar(ch);
+	else
+		ofw_putchar(U_SPECIAL);
+}
+
+/** Start all CPUs represented by following siblings of the given node.
+ *
+ * Except for the current CPU.
+ *
+ * @param child         The first child of the OFW tree node whose children
+ *                      represent CPUs to be woken up.
+ * @param current_mid   MID of the current CPU, the current CPU will
+ *                      (of course) not be woken up.
+ * @param physmem_start Starting address of the physical memory.
+ *
+ * @return Number of CPUs which have the same parent node as
+ *         "child".
+ *
+ */
+static size_t wake_cpus_in_node(phandle child, uint64_t current_mid,
+    uintptr_t physmem_start)
+{
+	size_t cpus;
+	
+	for (cpus = 0; (child != 0) && (child != (phandle) -1);
+	    child = ofw_get_peer_node(child), cpus++) {
+		char type_name[OFW_TREE_PROPERTY_MAX_VALUELEN];
+		
+		if (ofw_get_property(child, "device_type", type_name,
+		    OFW_TREE_PROPERTY_MAX_VALUELEN) > 0) {
+			type_name[OFW_TREE_PROPERTY_MAX_VALUELEN - 1] = 0;
+			
+			if (str_cmp(type_name, "cpu") == 0) {
+				uint32_t mid;
+				
+				/*
+				 * "upa-portid" for US, "portid" for US-III,
+				 * "cpuid" for US-IV
+				 */
+				if ((ofw_get_property(child, "upa-portid", &mid, sizeof(mid)) <= 0)
+				    && (ofw_get_property(child, "portid", &mid, sizeof(mid)) <= 0)
+				    && (ofw_get_property(child, "cpuid", &mid, sizeof(mid)) <= 0))
+					continue;
+				
+				if (current_mid != mid) {
+					/*
+					 * Start secondary processor.
+					 */
+					(void) ofw_call("SUNW,start-cpu", 3, 1,
+					    NULL, child, KERNEL_ADDRESS,
+					    physmem_start | AP_PROCESSOR);
+				}
+			}
+		}
+	}
+	
+	return cpus;
+}
+
+/** Find out the current CPU's MID and wake up all AP processors.
+ *
+ */
+void ofw_cpu(uint16_t mid_mask, uintptr_t physmem_start)
+{
+	/* Get the current CPU MID */
+	uint64_t current_mid;
+	
+	asm volatile (
+		"ldxa [%[zero]] %[asi], %[current_mid]\n"
+		: [current_mid] "=r" (current_mid)
+		: [zero] "r" (0),
+		  [asi] "i" (ASI_ICBUS_CONFIG)
+	);
+	
+	current_mid >>= ICBUS_CONFIG_MID_SHIFT;
+	current_mid &= mid_mask;
+	
+	/* Wake up the CPUs */
+	
+	phandle cpus_parent = ofw_find_device("/ssm@0,0");
+	if ((cpus_parent == 0) || (cpus_parent == (phandle) -1))
+		cpus_parent = ofw_find_device("/");
+	
+	phandle node = ofw_get_child_node(cpus_parent);
+	size_t cpus = wake_cpus_in_node(node, current_mid, physmem_start);
+	
+	while ((node != 0) && (node != (phandle) -1)) {
+		char name[OFW_TREE_PROPERTY_MAX_VALUELEN];
+		
+		if (ofw_get_property(node, "name", name,
+		    OFW_TREE_PROPERTY_MAX_VALUELEN) > 0) {
+			name[OFW_TREE_PROPERTY_MAX_VALUELEN - 1] = 0;
+			
+			if (str_cmp(name, "cmp") == 0) {
+				phandle subnode = ofw_get_child_node(node);
+				cpus += wake_cpus_in_node(subnode,
+				    current_mid, physmem_start);
+			}
+		}
+		
+		node = ofw_get_peer_node(node);
+	}
+	
+	if (cpus == 0)
+		printf("Warning: Unable to get CPU properties.\n");
+}
+
+/** Get physical memory starting address.
+ *
+ * @return Physical memory starting address.
+ *
+ */
+uintptr_t ofw_get_physmem_start(void)
+{
+	uint32_t memreg[4];
+	if ((ofw_ret_t) ofw_get_property(ofw_memory, "reg", &memreg,
+	    sizeof(memreg)) <= 0) {
+		printf("Error: Unable to get physical memory starting address, halting.\n");
+		halt();
+	}
+	
+	return ((((uintptr_t) memreg[0]) << 32) | memreg[1]);
+}
