16 files changed, 4916 insertions, 1031 deletions

diff --git a/arch/powerpc/platforms/powermac/Makefile b/arch/powerpc/platforms/powermac/Makefile
index c9df44fcf57..78093d7f97a 100644
--- a/arch/powerpc/platforms/powermac/Makefile
+++ b/arch/powerpc/platforms/powermac/Makefile
@@ -1,9 +1,14 @@
+CFLAGS_bootx_init.o  		+= -fPIC
+
 obj-y				+= pic.o setup.o time.o feature.o pci.o \
-				   sleep.o low_i2c.o cache.o
+				   sleep.o low_i2c.o cache.o pfunc_core.o \
+				   pfunc_base.o
 obj-$(CONFIG_PMAC_BACKLIGHT)	+= backlight.o
 obj-$(CONFIG_CPU_FREQ_PMAC)	+= cpufreq_32.o
 obj-$(CONFIG_CPU_FREQ_PMAC64)	+= cpufreq_64.o
 obj-$(CONFIG_NVRAM)		+= nvram.o
 # ppc64 pmac doesn't define CONFIG_NVRAM but needs nvram stuff
 obj-$(CONFIG_PPC64)		+= nvram.o
+obj-$(CONFIG_PPC32)		+= bootx_init.o
 obj-$(CONFIG_SMP)		+= smp.o
+obj-$(CONFIG_PPC_MERGE)		+= udbg_scc.o udbg_adb.o
diff --git a/arch/powerpc/platforms/powermac/bootx_init.c b/arch/powerpc/platforms/powermac/bootx_init.c
new file mode 100644
index 00000000000..fa8b4d7b5de
--- /dev/null
+++ b/arch/powerpc/platforms/powermac/bootx_init.c
@@ -0,0 +1,547 @@
+/*
+ *  Early boot support code for BootX bootloader
+ *
+ *  Copyright (C) 2005 Ben. Herrenschmidt (benh@kernel.crashing.org)
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License
+ *  as published by the Free Software Foundation; either version
+ *  2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/config.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/init.h>
+#include <linux/version.h>
+#include <asm/sections.h>
+#include <asm/prom.h>
+#include <asm/page.h>
+#include <asm/bootx.h>
+#include <asm/bootinfo.h>
+#include <asm/btext.h>
+#include <asm/io.h>
+
+#undef DEBUG
+#define SET_BOOT_BAT
+
+#ifdef DEBUG
+#define DBG(fmt...) do { bootx_printf(fmt); } while(0)
+#else
+#define DBG(fmt...) do { } while(0)
+#endif
+
+extern void __start(unsigned long r3, unsigned long r4, unsigned long r5);
+
+static unsigned long __initdata bootx_dt_strbase;
+static unsigned long __initdata bootx_dt_strend;
+static unsigned long __initdata bootx_node_chosen;
+static boot_infos_t * __initdata bootx_info;
+static char __initdata bootx_disp_path[256];
+
+/* Is boot-info compatible ? */
+#define BOOT_INFO_IS_COMPATIBLE(bi) \
+	((bi)->compatible_version <= BOOT_INFO_VERSION)
+#define BOOT_INFO_IS_V2_COMPATIBLE(bi)	((bi)->version >= 2)
+#define BOOT_INFO_IS_V4_COMPATIBLE(bi)	((bi)->version >= 4)
+
+#ifdef CONFIG_BOOTX_TEXT
+static void __init bootx_printf(const char *format, ...)
+{
+	const char *p, *q, *s;
+	va_list args;
+	unsigned long v;
+
+	va_start(args, format);
+	for (p = format; *p != 0; p = q) {
+		for (q = p; *q != 0 && *q != '\n' && *q != '%'; ++q)
+			;
+		if (q > p)
+			btext_drawtext(p, q - p);
+		if (*q == 0)
+			break;
+		if (*q == '\n') {
+			++q;
+			btext_flushline();
+			btext_drawstring("\r\n");
+			btext_flushline();
+			continue;
+		}
+		++q;
+		if (*q == 0)
+			break;
+		switch (*q) {
+		case 's':
+			++q;
+			s = va_arg(args, const char *);
+			if (s == NULL)
+				s = "<NULL>";
+			btext_drawstring(s);
+			break;
+		case 'x':
+			++q;
+			v = va_arg(args, unsigned long);
+			btext_drawhex(v);
+			break;
+		}
+	}
+}
+#else /* CONFIG_BOOTX_TEXT */
+static void __init bootx_printf(const char *format, ...) {}
+#endif /* CONFIG_BOOTX_TEXT */
+
+static void * __init bootx_early_getprop(unsigned long base,
+					 unsigned long node,
+					 char *prop)
+{
+	struct bootx_dt_node *np = (struct bootx_dt_node *)(base + node);
+	u32 *ppp = &np->properties;
+
+	while(*ppp) {
+		struct bootx_dt_prop *pp =
+			(struct bootx_dt_prop *)(base + *ppp);
+
+		if (strcmp((char *)((unsigned long)pp->name + base),
+			   prop) == 0) {
+			return (void *)((unsigned long)pp->value + base);
+		}
+		ppp = &pp->next;
+	}
+	return NULL;
+}
+
+#define dt_push_token(token, mem) \
+	do { \
+		*(mem) = _ALIGN_UP(*(mem),4); \
+		*((u32 *)*(mem)) = token; \
+		*(mem) += 4; \
+	} while(0)
+
+static unsigned long __init bootx_dt_find_string(char *str)
+{
+	char *s, *os;
+
+	s = os = (char *)bootx_dt_strbase;
+	s += 4;
+	while (s <  (char *)bootx_dt_strend) {
+		if (strcmp(s, str) == 0)
+			return s - os;
+		s += strlen(s) + 1;
+	}
+	return 0;
+}
+
+static void __init bootx_dt_add_prop(char *name, void *data, int size,
+				  unsigned long *mem_end)
+{
+	unsigned long soff = bootx_dt_find_string(name);
+	if (data == NULL)
+		size = 0;
+	if (soff == 0) {
+		bootx_printf("WARNING: Can't find string index for <%s>\n",
+			     name);
+		return;
+	}
+	if (size > 0x20000) {
+		bootx_printf("WARNING: ignoring large property ");
+		bootx_printf("%s length 0x%x\n", name, size);
+		return;
+	}
+	dt_push_token(OF_DT_PROP, mem_end);
+	dt_push_token(size, mem_end);
+	dt_push_token(soff, mem_end);
+
+	/* push property content */
+	if (size && data) {
+		memcpy((void *)*mem_end, data, size);
+		*mem_end = _ALIGN_UP(*mem_end + size, 4);
+	}
+}
+
+static void __init bootx_add_chosen_props(unsigned long base,
+					  unsigned long *mem_end)
+{
+	u32 val = _MACH_Pmac;
+
+	bootx_dt_add_prop("linux,platform", &val, 4, mem_end);
+
+	if (bootx_info->kernelParamsOffset) {
+		char *args = (char *)((unsigned long)bootx_info) +
+			bootx_info->kernelParamsOffset;
+		bootx_dt_add_prop("bootargs", args, strlen(args) + 1, mem_end);
+	}
+	if (bootx_info->ramDisk) {
+		val = ((unsigned long)bootx_info) + bootx_info->ramDisk;
+		bootx_dt_add_prop("linux,initrd-start", &val, 4, mem_end);
+		val += bootx_info->ramDiskSize;
+		bootx_dt_add_prop("linux,initrd-end", &val, 4, mem_end);
+	}
+	if (strlen(bootx_disp_path))
+		bootx_dt_add_prop("linux,stdout-path", bootx_disp_path,
+				  strlen(bootx_disp_path) + 1, mem_end);
+}
+
+static void __init bootx_add_display_props(unsigned long base,
+					   unsigned long *mem_end)
+{
+	bootx_dt_add_prop("linux,boot-display", NULL, 0, mem_end);
+	bootx_dt_add_prop("linux,opened", NULL, 0, mem_end);
+}
+
+static void __init bootx_dt_add_string(char *s, unsigned long *mem_end)
+{
+	unsigned int l = strlen(s) + 1;
+	memcpy((void *)*mem_end, s, l);
+	bootx_dt_strend = *mem_end = *mem_end + l;
+}
+
+static void __init bootx_scan_dt_build_strings(unsigned long base,
+					       unsigned long node,
+					       unsigned long *mem_end)
+{
+	struct bootx_dt_node *np = (struct bootx_dt_node *)(base + node);
+	u32 *cpp, *ppp = &np->properties;
+	unsigned long soff;
+	char *namep;
+
+	/* Keep refs to known nodes */
+	namep = np->full_name ? (char *)(base + np->full_name) : NULL;
+       	if (namep == NULL) {
+		bootx_printf("Node without a full name !\n");
+		namep = "";
+	}
+	DBG("* strings: %s\n", namep);
+
+	if (!strcmp(namep, "/chosen")) {
+		DBG(" detected /chosen ! adding properties names !\n");
+		bootx_dt_add_string("linux,platform", mem_end);
+		bootx_dt_add_string("linux,stdout-path", mem_end);
+		bootx_dt_add_string("linux,initrd-start", mem_end);
+		bootx_dt_add_string("linux,initrd-end", mem_end);
+		bootx_dt_add_string("bootargs", mem_end);
+		bootx_node_chosen = node;
+	}
+	if (node == bootx_info->dispDeviceRegEntryOffset) {
+		DBG(" detected display ! adding properties names !\n");
+		bootx_dt_add_string("linux,boot-display", mem_end);
+		bootx_dt_add_string("linux,opened", mem_end);
+		strncpy(bootx_disp_path, namep, 255);
+	}
+
+	/* get and store all property names */
+	while (*ppp) {
+		struct bootx_dt_prop *pp =
+			(struct bootx_dt_prop *)(base + *ppp);
+
+		namep = pp->name ? (char *)(base + pp->name) : NULL;
+ 		if (namep == NULL || strcmp(namep, "name") == 0)
+ 			goto next;
+		/* get/create string entry */
+		soff = bootx_dt_find_string(namep);
+		if (soff == 0)
+			bootx_dt_add_string(namep, mem_end);
+	next:
+		ppp = &pp->next;
+	}
+
+	/* do all our children */
+	cpp = &np->child;
+	while(*cpp) {
+		np = (struct bootx_dt_node *)(base + *cpp);
+		bootx_scan_dt_build_strings(base, *cpp, mem_end);
+		cpp = &np->sibling;
+	}
+}
+
+static void __init bootx_scan_dt_build_struct(unsigned long base,
+					      unsigned long node,
+					      unsigned long *mem_end)
+{
+	struct bootx_dt_node *np = (struct bootx_dt_node *)(base + node);
+	u32 *cpp, *ppp = &np->properties;
+	char *namep, *p, *ep, *lp;
+	int l;
+
+	dt_push_token(OF_DT_BEGIN_NODE, mem_end);
+
+	/* get the node's full name */
+	namep = np->full_name ? (char *)(base + np->full_name) : NULL;
+	if (namep == NULL)
+		namep = "";
+	l = strlen(namep);
+
+	DBG("* struct: %s\n", namep);
+
+	/* Fixup an Apple bug where they have bogus \0 chars in the
+	 * middle of the path in some properties, and extract
+	 * the unit name (everything after the last '/').
+	 */
+	memcpy((void *)*mem_end, namep, l + 1);
+	namep = (char *)*mem_end;
+	for (lp = p = namep, ep = namep + l; p < ep; p++) {
+		if (*p == '/')
+			lp = namep;
+		else if (*p != 0)
+			*lp++ = *p;
+	}
+	*lp = 0;
+	*mem_end = _ALIGN_UP((unsigned long)lp + 1, 4);
+
+	/* get and store all properties */
+	while (*ppp) {
+		struct bootx_dt_prop *pp =
+			(struct bootx_dt_prop *)(base + *ppp);
+
+		namep = pp->name ? (char *)(base + pp->name) : NULL;
+		/* Skip "name" */
+ 		if (namep == NULL || !strcmp(namep, "name"))
+ 			goto next;
+		/* Skip "bootargs" in /chosen too as we replace it */
+		if (node == bootx_node_chosen && !strcmp(namep, "bootargs"))
+			goto next;
+
+		/* push property head */
+		bootx_dt_add_prop(namep,
+				  pp->value ? (void *)(base + pp->value): NULL,
+				  pp->length, mem_end);
+	next:
+		ppp = &pp->next;
+	}
+
+	if (node == bootx_node_chosen)
+		bootx_add_chosen_props(base, mem_end);
+	if (node == bootx_info->dispDeviceRegEntryOffset)
+		bootx_add_display_props(base, mem_end);
+
+	/* do all our children */
+	cpp = &np->child;
+	while(*cpp) {
+		np = (struct bootx_dt_node *)(base + *cpp);
+		bootx_scan_dt_build_struct(base, *cpp, mem_end);
+		cpp = &np->sibling;
+	}
+
+	dt_push_token(OF_DT_END_NODE, mem_end);
+}
+
+static unsigned long __init bootx_flatten_dt(unsigned long start)
+{
+	boot_infos_t *bi = bootx_info;
+	unsigned long mem_start, mem_end;
+	struct boot_param_header *hdr;
+	unsigned long base;
+	u64 *rsvmap;
+
+	/* Start using memory after the big blob passed by BootX, get
+	 * some space for the header
+	 */
+	mem_start = mem_end = _ALIGN_UP(((unsigned long)bi) + start, 4);
+	DBG("Boot params header at: %x\n", mem_start);
+	hdr = (struct boot_param_header *)mem_start;
+	mem_end += sizeof(struct boot_param_header);
+	rsvmap = (u64 *)(_ALIGN_UP(mem_end, 8));
+	hdr->off_mem_rsvmap = ((unsigned long)rsvmap) - mem_start;
+	mem_end = ((unsigned long)rsvmap) + 8 * sizeof(u64);
+
+	/* Get base of tree */
+	base = ((unsigned long)bi) + bi->deviceTreeOffset;
+
+	/* Build string array */
+	DBG("Building string array at: %x\n", mem_end);
+	DBG("Device Tree Base=%x\n", base);
+	bootx_dt_strbase = mem_end;
+	mem_end += 4;
+	bootx_dt_strend = mem_end;
+	bootx_scan_dt_build_strings(base, 4, &mem_end);
+	hdr->off_dt_strings = bootx_dt_strbase - mem_start;
+	hdr->dt_strings_size = bootx_dt_strend - bootx_dt_strbase;
+
+	/* Build structure */
+	mem_end = _ALIGN(mem_end, 16);
+	DBG("Building device tree structure at: %x\n", mem_end);
+	hdr->off_dt_struct = mem_end - mem_start;
+	bootx_scan_dt_build_struct(base, 4, &mem_end);
+	dt_push_token(OF_DT_END, &mem_end);
+
+	/* Finish header */
+	hdr->boot_cpuid_phys = 0;
+	hdr->magic = OF_DT_HEADER;
+	hdr->totalsize = mem_end - mem_start;
+	hdr->version = OF_DT_VERSION;
+	/* Version 16 is not backward compatible */
+	hdr->last_comp_version = 0x10;
+
+	/* Reserve the whole thing and copy the reserve map in, we
+	 * also bump mem_reserve_cnt to cause further reservations to
+	 * fail since it's too late.
+	 */
+	mem_end = _ALIGN(mem_end, PAGE_SIZE);
+	DBG("End of boot params: %x\n", mem_end);
+	rsvmap[0] = mem_start;
+	rsvmap[1] = mem_end;
+	rsvmap[2] = 0;
+	rsvmap[3] = 0;
+
+	return (unsigned long)hdr;
+}
+
+
+#ifdef CONFIG_BOOTX_TEXT
+static void __init btext_welcome(boot_infos_t *bi)
+{
+	unsigned long flags;
+	unsigned long pvr;
+
+	bootx_printf("Welcome to Linux, kernel " UTS_RELEASE "\n");
+	bootx_printf("\nlinked at        : 0x%x", KERNELBASE);
+	bootx_printf("\nframe buffer at  : 0x%x", bi->dispDeviceBase);
+	bootx_printf(" (phys), 0x%x", bi->logicalDisplayBase);
+	bootx_printf(" (log)");
+	bootx_printf("\nklimit           : 0x%x",(unsigned long)klimit);
+	bootx_printf("\nboot_info at     : 0x%x", bi);
+	__asm__ __volatile__ ("mfmsr %0" : "=r" (flags));
+	bootx_printf("\nMSR              : 0x%x", flags);
+	__asm__ __volatile__ ("mfspr %0, 287" : "=r" (pvr));
+	bootx_printf("\nPVR              : 0x%x", pvr);
+	pvr >>= 16;
+	if (pvr > 1) {
+	    __asm__ __volatile__ ("mfspr %0, 1008" : "=r" (flags));
+	    bootx_printf("\nHID0             : 0x%x", flags);
+	}
+	if (pvr == 8 || pvr == 12 || pvr == 0x800c) {
+	    __asm__ __volatile__ ("mfspr %0, 1019" : "=r" (flags));
+	    bootx_printf("\nICTC             : 0x%x", flags);
+	}
+#ifdef DEBUG
+	bootx_printf("\n\n");
+	bootx_printf("bi->deviceTreeOffset   : 0x%x\n",
+		     bi->deviceTreeOffset);
+	bootx_printf("bi->deviceTreeSize     : 0x%x\n",
+		     bi->deviceTreeSize);
+#endif
+	bootx_printf("\n\n");
+}
+#endif /* CONFIG_BOOTX_TEXT */
+
+void __init bootx_init(unsigned long r3, unsigned long r4)
+{
+	boot_infos_t *bi = (boot_infos_t *) r4;
+	unsigned long hdr;
+	unsigned long space;
+	unsigned long ptr, x;
+	char *model;
+	unsigned long offset = reloc_offset();
+
+	reloc_got2(offset);
+
+	bootx_info = bi;
+
+	/* We haven't cleared any bss at this point, make sure
+	 * what we need is initialized
+	 */
+	bootx_dt_strbase = bootx_dt_strend = 0;
+	bootx_node_chosen = 0;
+	bootx_disp_path[0] = 0;
+
+	if (!BOOT_INFO_IS_V2_COMPATIBLE(bi))
+		bi->logicalDisplayBase = bi->dispDeviceBase;
+
+#ifdef CONFIG_BOOTX_TEXT
+	btext_setup_display(bi->dispDeviceRect[2] - bi->dispDeviceRect[0],
+			    bi->dispDeviceRect[3] - bi->dispDeviceRect[1],
+			    bi->dispDeviceDepth, bi->dispDeviceRowBytes,
+			    (unsigned long)bi->logicalDisplayBase);
+	btext_clearscreen();
+	btext_flushscreen();
+#endif /* CONFIG_BOOTX_TEXT */
+
+	/*
+	 * Test if boot-info is compatible.  Done only in config
+	 * CONFIG_BOOTX_TEXT since there is nothing much we can do
+	 * with an incompatible version, except display a message
+	 * and eventually hang the processor...
+	 *
+	 * I'll try to keep enough of boot-info compatible in the
+	 * future to always allow display of this message;
+	 */
+	if (!BOOT_INFO_IS_COMPATIBLE(bi)) {
+		bootx_printf(" !!! WARNING - Incompatible version"
+			     " of BootX !!!\n\n\n");
+		for (;;)
+			;
+	}
+	if (bi->architecture != BOOT_ARCH_PCI) {
+		bootx_printf(" !!! WARNING - Usupported machine"
+			     " architecture !\n");
+		for (;;)
+			;
+	}
+
+#ifdef CONFIG_BOOTX_TEXT
+	btext_welcome(bi);
+#endif
+	/* New BootX enters kernel with MMU off, i/os are not allowed
+	 * here. This hack will have been done by the boostrap anyway.
+	 */
+	if (bi->version < 4) {
+		/*
+		 * XXX If this is an iMac, turn off the USB controller.
+		 */
+		model = (char *) bootx_early_getprop(r4 + bi->deviceTreeOffset,
+						     4, "model");
+		if (model
+		    && (strcmp(model, "iMac,1") == 0
+			|| strcmp(model, "PowerMac1,1") == 0)) {
+			bootx_printf("iMac,1 detected, shutting down USB \n");
+			out_le32((unsigned *)0x80880008, 1);	/* XXX */
+		}
+	}
+
+	/* Get a pointer that points above the device tree, args, ramdisk,
+	 * etc... to use for generating the flattened tree
+	 */
+	if (bi->version < 5) {
+		space = bi->deviceTreeOffset + bi->deviceTreeSize;
+		if (bi->ramDisk)
+			space = bi->ramDisk + bi->ramDiskSize;
+	} else
+		space = bi->totalParamsSize;
+
+	bootx_printf("Total space used by parameters & ramdisk: %x \n", space);
+
+	/* New BootX will have flushed all TLBs and enters kernel with
+	 * MMU switched OFF, so this should not be useful anymore.
+	 */
+	if (bi->version < 4) {
+		bootx_printf("Touching pages...\n");
+
+		/*
+		 * Touch each page to make sure the PTEs for them
+		 * are in the hash table - the aim is to try to avoid
+		 * getting DSI exceptions while copying the kernel image.
+		 */
+		for (ptr = ((unsigned long) &_stext) & PAGE_MASK;
+		     ptr < (unsigned long)bi + space; ptr += PAGE_SIZE)
+			x = *(volatile unsigned long *)ptr;
+	}
+
+	/* Ok, now we need to generate a flattened device-tree to pass
+	 * to the kernel
+	 */
+	bootx_printf("Preparing boot params...\n");
+
+	hdr = bootx_flatten_dt(space);
+
+#ifdef CONFIG_BOOTX_TEXT
+#ifdef SET_BOOT_BAT
+	bootx_printf("Preparing BAT...\n");
+	btext_prepare_BAT();
+#else
+	btext_unmap();
+#endif
+#endif
+
+	reloc_got2(-offset);
+
+	__start(hdr, KERNELBASE + offset, 0);
+}
diff --git a/arch/powerpc/platforms/powermac/cpufreq_64.c b/arch/powerpc/platforms/powermac/cpufreq_64.c
index 39150342c6f..a4b50c4109c 100644
--- a/arch/powerpc/platforms/powermac/cpufreq_64.c
+++ b/arch/powerpc/platforms/powermac/cpufreq_64.c
@@ -28,6 +28,7 @@
 #include <asm/cputable.h>
 #include <asm/time.h>
 #include <asm/smu.h>
+#include <asm/pmac_pfunc.h>
 
 #undef DEBUG
 
@@ -85,6 +86,10 @@ static u32 *g5_pmode_data;
 static int g5_pmode_max;
 static int g5_pmode_cur;
 
+static void (*g5_switch_volt)(int speed_mode);
+static int (*g5_switch_freq)(int speed_mode);
+static int (*g5_query_freq)(void);
+
 static DECLARE_MUTEX(g5_switch_mutex);
 
 
@@ -92,9 +97,11 @@ static struct smu_sdbp_fvt *g5_fvt_table;	/* table of op. points */
 static int g5_fvt_count;			/* number of op. points */
 static int g5_fvt_cur;				/* current op. point */
 
-/* ----------------- real hardware interface */
+/*
+ * SMU based voltage switching for Neo2 platforms
+ */
 
-static void g5_switch_volt(int speed_mode)
+static void g5_smu_switch_volt(int speed_mode)
 {
 	struct smu_simple_cmd	cmd;
 
@@ -105,26 +112,57 @@ static void g5_switch_volt(int speed_mode)
 	wait_for_completion(&comp);
 }
 
-static int g5_switch_freq(int speed_mode)
+/*
+ * Platform function based voltage/vdnap switching for Neo2
+ */
+
+static struct pmf_function *pfunc_set_vdnap0;
+static struct pmf_function *pfunc_vdnap0_complete;
+
+static void g5_vdnap_switch_volt(int speed_mode)
 {
-	struct cpufreq_freqs freqs;
-	int to;
+	struct pmf_args args;
+	u32 slew, done = 0;
+	unsigned long timeout;
 
-	if (g5_pmode_cur == speed_mode)
-		return 0;
+	slew = (speed_mode == CPUFREQ_LOW) ? 1 : 0;
+	args.count = 1;
+	args.u[0].p = &slew;
 
-	down(&g5_switch_mutex);
+	pmf_call_one(pfunc_set_vdnap0, &args);
 
-	freqs.old = g5_cpu_freqs[g5_pmode_cur].frequency;
-	freqs.new = g5_cpu_freqs[speed_mode].frequency;
-	freqs.cpu = 0;
+	/* It's an irq GPIO so we should be able to just block here,
+	 * I'll do that later after I've properly tested the IRQ code for
+	 * platform functions
+	 */
+	timeout = jiffies + HZ/10;
+	while(!time_after(jiffies, timeout)) {
+		args.count = 1;
+		args.u[0].p = &done;
+		pmf_call_one(pfunc_vdnap0_complete, &args);
+		if (done)
+			break;
+		msleep(1);
+	}
+	if (done == 0)
+		printk(KERN_WARNING "cpufreq: Timeout in clock slewing !\n");
+}
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+/*
+ * SCOM based frequency switching for 970FX rev3
+ */
+static int g5_scom_switch_freq(int speed_mode)
+{
+	unsigned long flags;
+	int to;
 
 	/* If frequency is going up, first ramp up the voltage */
 	if (speed_mode < g5_pmode_cur)
 		g5_switch_volt(speed_mode);
 
+	local_irq_save(flags);
+
 	/* Clear PCR high */
 	scom970_write(SCOM_PCR, 0);
 	/* Clear PCR low */
@@ -147,6 +185,8 @@ static int g5_switch_freq(int speed_mode)
 		udelay(100);
 	}
 
+	local_irq_restore(flags);
+
 	/* If frequency is going down, last ramp the voltage */
 	if (speed_mode > g5_pmode_cur)
 		g5_switch_volt(speed_mode);
@@ -154,14 +194,10 @@ static int g5_switch_freq(int speed_mode)
 	g5_pmode_cur = speed_mode;
 	ppc_proc_freq = g5_cpu_freqs[speed_mode].frequency * 1000ul;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-
-	up(&g5_switch_mutex);
-
 	return 0;
 }
 
-static int g5_query_freq(void)
+static int g5_scom_query_freq(void)
 {
 	unsigned long psr = scom970_read(SCOM_PSR);
 	int i;
@@ -173,7 +209,104 @@ static int g5_query_freq(void)
 	return i;
 }
 
-/* ----------------- cpufreq bookkeeping */
+/*
+ * Platform function based voltage switching for PowerMac7,2 & 7,3
+ */
+
+static struct pmf_function *pfunc_cpu0_volt_high;
+static struct pmf_function *pfunc_cpu0_volt_low;
+static struct pmf_function *pfunc_cpu1_volt_high;
+static struct pmf_function *pfunc_cpu1_volt_low;
+
+static void g5_pfunc_switch_volt(int speed_mode)
+{
+	if (speed_mode == CPUFREQ_HIGH) {
+		if (pfunc_cpu0_volt_high)
+			pmf_call_one(pfunc_cpu0_volt_high, NULL);
+		if (pfunc_cpu1_volt_high)
+			pmf_call_one(pfunc_cpu1_volt_high, NULL);
+	} else {
+		if (pfunc_cpu0_volt_low)
+			pmf_call_one(pfunc_cpu0_volt_low, NULL);
+		if (pfunc_cpu1_volt_low)
+			pmf_call_one(pfunc_cpu1_volt_low, NULL);
+	}
+	msleep(10); /* should be faster , to fix */
+}
+
+/*
+ * Platform function based frequency switching for PowerMac7,2 & 7,3
+ */
+
+static struct pmf_function *pfunc_cpu_setfreq_high;
+static struct pmf_function *pfunc_cpu_setfreq_low;
+static struct pmf_function *pfunc_cpu_getfreq;
+static struct pmf_function *pfunc_slewing_done;;
+
+static int g5_pfunc_switch_freq(int speed_mode)
+{
+	struct pmf_args args;
+	u32 done = 0;
+	unsigned long timeout;
+
+	/* If frequency is going up, first ramp up the voltage */
+	if (speed_mode < g5_pmode_cur)
+		g5_switch_volt(speed_mode);
+
+	/* Do it */
+	if (speed_mode == CPUFREQ_HIGH)
+		pmf_call_one(pfunc_cpu_setfreq_high, NULL);
+	else
+		pmf_call_one(pfunc_cpu_setfreq_low, NULL);
+
+	/* It's an irq GPIO so we should be able to just block here,
+	 * I'll do that later after I've properly tested the IRQ code for
+	 * platform functions
+	 */
+	timeout = jiffies + HZ/10;
+	while(!time_after(jiffies, timeout)) {
+		args.count = 1;
+		args.u[0].p = &done;
+		pmf_call_one(pfunc_slewing_done, &args);
+		if (done)
+			break;
+		msleep(1);
+	}
+	if (done == 0)
+		printk(KERN_WARNING "cpufreq: Timeout in clock slewing !\n");
+
+	/* If frequency is going down, last ramp the voltage */
+	if (speed_mode > g5_pmode_cur)
+		g5_switch_volt(speed_mode);
+
+	g5_pmode_cur = speed_mode;
+	ppc_proc_freq = g5_cpu_freqs[speed_mode].frequency * 1000ul;
+
+	return 0;
+}
+
+static int g5_pfunc_query_freq(void)
+{
+	struct pmf_args args;
+	u32 val = 0;
+
+	args.count = 1;
+	args.u[0].p = &val;
+	pmf_call_one(pfunc_cpu_getfreq, &args);
+	return val ? CPUFREQ_HIGH : CPUFREQ_LOW;
+}
+
+/*
+ * Fake voltage switching for platforms with missing support
+ */
+
+static void g5_dummy_switch_volt(int speed_mode)
+{
+}
+
+/*
+ * Common interface to the cpufreq core
+ */
 
 static int g5_cpufreq_verify(struct cpufreq_policy *policy)
 {
@@ -183,13 +316,30 @@ static int g5_cpufreq_verify(struct cpufreq_policy *policy)
 static int g5_cpufreq_target(struct cpufreq_policy *policy,
 	unsigned int target_freq, unsigned int relation)
 {
-	unsigned int    newstate = 0;
+	unsigned int newstate = 0;
+	struct cpufreq_freqs freqs;
+	int rc;
 
 	if (cpufreq_frequency_table_target(policy, g5_cpu_freqs,
 			target_freq, relation, &newstate))
 		return -EINVAL;
 
-	return g5_switch_freq(newstate);
+	if (g5_pmode_cur == newstate)
+		return 0;
+
+	down(&g5_switch_mutex);
+
+	freqs.old = g5_cpu_freqs[g5_pmode_cur].frequency;
+	freqs.new = g5_cpu_freqs[newstate].frequency;
+	freqs.cpu = 0;
+
+	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	rc = g5_switch_freq(newstate);
+	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+	up(&g5_switch_mutex);
+
+	return rc;
 }
 
 static unsigned int g5_cpufreq_get_speed(unsigned int cpu)
@@ -205,6 +355,7 @@ static int g5_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
 	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
 	policy->cur = g5_cpu_freqs[g5_query_freq()].frequency;
+	policy->cpus = cpu_possible_map;
 	cpufreq_frequency_table_get_attr(g5_cpu_freqs, policy->cpu);
 
 	return cpufreq_frequency_table_cpuinfo(policy,
@@ -224,19 +375,39 @@ static struct cpufreq_driver g5_cpufreq_driver = {
 };
 
 
-static int __init g5_cpufreq_init(void)
+static int __init g5_neo2_cpufreq_init(struct device_node *cpus)
 {
 	struct device_node *cpunode;
 	unsigned int psize, ssize;
-	struct smu_sdbp_header *shdr;
 	unsigned long max_freq;
-	u32 *valp;
+	char *freq_method, *volt_method;
+	u32 *valp, pvr_hi;
+	int use_volts_vdnap = 0;
+	int use_volts_smu = 0;
 	int rc = -ENODEV;
 
-	/* Look for CPU and SMU nodes */
-	cpunode = of_find_node_by_type(NULL, "cpu");
-	if (!cpunode) {
-		DBG("No CPU node !\n");
+	/* Check supported platforms */
+	if (machine_is_compatible("PowerMac8,1") ||
+	    machine_is_compatible("PowerMac8,2") ||
+	    machine_is_compatible("PowerMac9,1"))
+		use_volts_smu = 1;
+	else if (machine_is_compatible("PowerMac11,2"))
+		use_volts_vdnap = 1;
+	else
+		return -ENODEV;
+
+	/* Get first CPU node */
+	for (cpunode = NULL;
+	     (cpunode = of_get_next_child(cpus, cpunode)) != NULL;) {
+		u32 *reg =
+			(u32 *)get_property(cpunode, "reg", NULL);
+		if (reg == NULL || (*reg) != 0)
+			continue;
+		if (!strcmp(cpunode->type, "cpu"))
+			break;
+	}
+	if (cpunode == NULL) {
+		printk(KERN_ERR "cpufreq: Can't find any CPU 0 node\n");
 		return -ENODEV;
 	}
 
@@ -246,8 +417,9 @@ static int __init g5_cpufreq_init(void)
 		DBG("No cpu-version property !\n");
 		goto bail_noprops;
 	}
-	if (((*valp) >> 16) != 0x3c) {
-		DBG("Wrong CPU version: %08x\n", *valp);
+	pvr_hi = (*valp) >> 16;
+	if (pvr_hi != 0x3c && pvr_hi != 0x44) {
+		printk(KERN_ERR "cpufreq: Unsupported CPU version\n");
 		goto bail_noprops;
 	}
 
@@ -259,18 +431,50 @@ static int __init g5_cpufreq_init(void)
 	}
 	g5_pmode_max = psize / sizeof(u32) - 1;
 
-	/* Look for the FVT table */
-	shdr = smu_get_sdb_partition(SMU_SDB_FVT_ID, NULL);
-	if (!shdr)
-		goto bail_noprops;
-	g5_fvt_table = (struct smu_sdbp_fvt *)&shdr[1];
-	ssize = (shdr->len * sizeof(u32)) - sizeof(struct smu_sdbp_header);
-	g5_fvt_count = ssize / sizeof(struct smu_sdbp_fvt);
-	g5_fvt_cur = 0;
-
-	/* Sanity checking */
-	if (g5_fvt_count < 1 || g5_pmode_max < 1)
-		goto bail_noprops;
+	if (use_volts_smu) {
+		struct smu_sdbp_header *shdr;
+
+		/* Look for the FVT table */
+		shdr = smu_get_sdb_partition(SMU_SDB_FVT_ID, NULL);
+		if (!shdr)
+			goto bail_noprops;
+		g5_fvt_table = (struct smu_sdbp_fvt *)&shdr[1];
+		ssize = (shdr->len * sizeof(u32)) -
+			sizeof(struct smu_sdbp_header);
+		g5_fvt_count = ssize / sizeof(struct smu_sdbp_fvt);
+		g5_fvt_cur = 0;
+
+		/* Sanity checking */
+		if (g5_fvt_count < 1 || g5_pmode_max < 1)
+			goto bail_noprops;
+
+		g5_switch_volt = g5_smu_switch_volt;
+		volt_method = "SMU";
+	} else if (use_volts_vdnap) {
+		struct device_node *root;
+
+		root = of_find_node_by_path("/");
+		if (root == NULL) {
+			printk(KERN_ERR "cpufreq: Can't find root of "
+			       "device tree\n");
+			goto bail_noprops;
+		}
+		pfunc_set_vdnap0 = pmf_find_function(root, "set-vdnap0");
+		pfunc_vdnap0_complete =
+			pmf_find_function(root, "slewing-done");
+		if (pfunc_set_vdnap0 == NULL ||
+		    pfunc_vdnap0_complete == NULL) {
+			printk(KERN_ERR "cpufreq: Can't find required "
+			       "platform function\n");
+			goto bail_noprops;
+		}
+
+		g5_switch_volt = g5_vdnap_switch_volt;
+		volt_method = "GPIO";
+	} else {
+		g5_switch_volt = g5_dummy_switch_volt;
+		volt_method = "none";
+	}
 
 	/*
 	 * From what I see, clock-frequency is always the maximal frequency.
@@ -286,19 +490,23 @@ static int __init g5_cpufreq_init(void)
 	g5_cpu_freqs[0].frequency = max_freq;
 	g5_cpu_freqs[1].frequency = max_freq/2;
 
-	/* Check current frequency */
-	g5_pmode_cur = g5_query_freq();
-	if (g5_pmode_cur > 1)
-		/* We don't support anything but 1:1 and 1:2, fixup ... */
-		g5_pmode_cur = 1;
+	/* Set callbacks */
+	g5_switch_freq = g5_scom_switch_freq;
+	g5_query_freq = g5_scom_query_freq;
+	freq_method = "SCOM";
 
 	/* Force apply current frequency to make sure everything is in
 	 * sync (voltage is right for example). Firmware may leave us with
 	 * a strange setting ...
 	 */
-	g5_switch_freq(g5_pmode_cur);
+	g5_switch_volt(CPUFREQ_HIGH);
+	msleep(10);
+	g5_pmode_cur = -1;
+	g5_switch_freq(g5_query_freq());
 
 	printk(KERN_INFO "Registering G5 CPU frequency driver\n");
+	printk(KERN_INFO "Frequency method: %s, Voltage method: %s\n",
+	       freq_method, volt_method);
 	printk(KERN_INFO "Low: %d Mhz, High: %d Mhz, Cur: %d MHz\n",
 		g5_cpu_freqs[1].frequency/1000,
 		g5_cpu_freqs[0].frequency/1000,
@@ -317,6 +525,200 @@ static int __init g5_cpufreq_init(void)
 	return rc;
 }
 
+static int __init g5_pm72_cpufreq_init(struct device_node *cpus)
+{
+	struct device_node *cpuid = NULL, *hwclock = NULL, *cpunode = NULL;
+	u8 *eeprom = NULL;
+	u32 *valp;
+	u64 max_freq, min_freq, ih, il;
+	int has_volt = 1, rc = 0;
+
+	/* Get first CPU node */
+	for (cpunode = NULL;
+	     (cpunode = of_get_next_child(cpus, cpunode)) != NULL;) {
+		if (!strcmp(cpunode->type, "cpu"))
+			break;
+	}
+	if (cpunode == NULL) {
+		printk(KERN_ERR "cpufreq: Can't find any CPU node\n");
+		return -ENODEV;
+	}
+
+	/* Lookup the cpuid eeprom node */
+        cpuid = of_find_node_by_path("/u3@0,f8000000/i2c@f8001000/cpuid@a0");
+	if (cpuid != NULL)
+		eeprom = (u8 *)get_property(cpuid, "cpuid", NULL);
+	if (eeprom == NULL) {
+		printk(KERN_ERR "cpufreq: Can't find cpuid EEPROM !\n");
+		rc = -ENODEV;
+		goto bail;