Introduce support for cpufreq on PowerPC with the dynamic frequency

switching capabilities of the MPC7447A and MPC7448.

11 files changed, 595 insertions, 22 deletions

diff --git a/sys/conf/files.powerpc b/sys/conf/files.powerpc
index 91b5668..ca4df8a 100644
--- a/sys/conf/files.powerpc
+++ b/sys/conf/files.powerpc
@@ -97,6 +97,7 @@ powerpc/booke/pmap.c		optional	e500
 powerpc/booke/swtch.S		optional	e500
 powerpc/booke/trap.c		optional	e500
 powerpc/booke/vm_machdep.c	optional	e500
+powerpc/cpufreq/dfs.c		optional	cpufreq
 powerpc/fpu/fpu_add.c		optional	fpu_emu
 powerpc/fpu/fpu_compare.c	optional	fpu_emu
 powerpc/fpu/fpu_div.c		optional	fpu_emu
@@ -114,6 +115,7 @@ powerpc/mpc85xx/nexus.c		optional	mpc85xx
 powerpc/mpc85xx/ocpbus.c	optional	mpc85xx
 powerpc/mpc85xx/opic.c		optional	mpc85xx
 powerpc/mpc85xx/pci_ocp.c	optional	pci mpc85xx
+powerpc/ofw/ofw_cpu.c		optional	aim
 powerpc/ofw/ofw_pcibus.c	optional	pci aim
 powerpc/ofw/ofw_pcib_pci.c	optional	pci aim
 powerpc/ofw/ofw_real.c		optional	aim
@@ -133,6 +135,7 @@ powerpc/powermac/cuda.c		optional	powermac cuda
 powerpc/powermac/pmu.c		optional	powermac pmu 
 powerpc/powermac/macgpio.c	optional	powermac pci 
 powerpc/powermac/cpcht.c	optional	powermac pci
+powerpc/powermac/vcoregpio.c	optional	powermac 
 powerpc/powerpc/altivec.c	optional	aim
 powerpc/powerpc/atomic.S	standard
 powerpc/powerpc/autoconf.c	standard
diff --git a/sys/modules/Makefile b/sys/modules/Makefile
index ef6373a..6a04d5e 100644
--- a/sys/modules/Makefile
+++ b/sys/modules/Makefile
@@ -576,6 +576,7 @@ _xe=		xe
 .if ${MACHINE_ARCH} == "powerpc"
 _an=		an
 _bm=		bm
+_cpufreq=	cpufreq
 _nvram=		powermac_nvram
 _smbfs=		smbfs
 _sound=		sound
diff --git a/sys/modules/cpufreq/Makefile b/sys/modules/cpufreq/Makefile
index 108c2b1..4e385b4 100644
--- a/sys/modules/cpufreq/Makefile
+++ b/sys/modules/cpufreq/Makefile
@@ -19,4 +19,9 @@ SRCS+=	est.c hwpstate.c p4tcc.c powernow.c
 SRCS+=	smist.c
 .endif
 
+.if ${MACHINE} == "powerpc"
+.PATH:	${.CURDIR}/../../powerpc/cpufreq
+SRCS+= dfs.c
+.endif
+
 .include <bsd.kmod.mk>
diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 662fd14..5d2665a 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -881,14 +881,6 @@ cpu_initclocks(void)
 	decr_tc_init();
 }
 
-/* Get current clock frequency for the given cpu id. */
-int
-cpu_est_clockrate(int cpu_id, uint64_t *rate)
-{
-
-	return (ENXIO);
-}
-
 /*
  * Shutdown the CPU as much as possible.
  */
diff --git a/sys/powerpc/booke/machdep.c b/sys/powerpc/booke/machdep.c
index c753fd5..8787098 100644
--- a/sys/powerpc/booke/machdep.c
+++ b/sys/powerpc/booke/machdep.c
@@ -587,14 +587,6 @@ cpu_flush_dcache(void *ptr, size_t len)
 	/* TBD */
 }
 
-/* Get current clock frequency for the given cpu id. */
-int
-cpu_est_clockrate(int cpu_id, uint64_t *rate)
-{
-
-	return (ENXIO);
-}
-
 /*
  * Construct a PCB from a trapframe. This is called from kdb_trap() where
  * we want to start a backtrace from the function that caused us to enter
diff --git a/sys/powerpc/conf/GENERIC b/sys/powerpc/conf/GENERIC
index 36231c7..819d768 100644
--- a/sys/powerpc/conf/GENERIC
+++ b/sys/powerpc/conf/GENERIC
@@ -76,6 +76,9 @@ options 	WITNESS_SKIPSPIN	#Don't run witness on spinlocks for speed
 # To make an SMP kernel, the next line is needed
 #options 	SMP			# Symmetric MultiProcessor Kernel
 
+# CPU frequency control
+device		cpufreq
+
 # Standard busses
 device		pci
 
diff --git a/sys/powerpc/conf/NOTES b/sys/powerpc/conf/NOTES
index c22d86f..d6084ed 100644
--- a/sys/powerpc/conf/NOTES
+++ b/sys/powerpc/conf/NOTES
@@ -21,6 +21,9 @@ options 	PSIM			#GDB PSIM ppc simulator
 
 options 	SC_OFWFB		# OFW frame buffer
 
+# The cpufreq(4) driver provides support for CPU frequency control
+device		cpufreq
+
 # Standard busses
 device		pci
 
diff --git a/sys/powerpc/cpufreq/dfs.c b/sys/powerpc/cpufreq/dfs.c
new file mode 100644
index 0000000..2eb61cb
--- /dev/null
+++ b/sys/powerpc/cpufreq/dfs.c
@@ -0,0 +1,230 @@
+/*-
+ * Copyright (c) 2009 Nathan Whitehorn
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/cpu.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+
+#include "cpufreq_if.h"
+
+struct dfs_softc {
+	device_t dev;
+	int	 dfs4;
+};
+
+static void	dfs_identify(driver_t *driver, device_t parent);
+static int	dfs_probe(device_t dev);
+static int	dfs_attach(device_t dev);
+static int	dfs_settings(device_t dev, struct cf_setting *sets, int *count);
+static int	dfs_set(device_t dev, const struct cf_setting *set);
+static int	dfs_get(device_t dev, struct cf_setting *set);
+static int	dfs_type(device_t dev, int *type);
+
+static device_method_t dfs_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_identify,	dfs_identify),
+	DEVMETHOD(device_probe,		dfs_probe),
+	DEVMETHOD(device_attach,	dfs_attach),
+
+	/* cpufreq interface */
+	DEVMETHOD(cpufreq_drv_set,	dfs_set),
+	DEVMETHOD(cpufreq_drv_get,	dfs_get),
+	DEVMETHOD(cpufreq_drv_type,	dfs_type),
+	DEVMETHOD(cpufreq_drv_settings,	dfs_settings),
+
+	{0, 0}
+};
+
+static driver_t dfs_driver = {
+	"dfs",
+	dfs_methods,
+	sizeof(struct dfs_softc)
+};
+
+static devclass_t dfs_devclass;
+DRIVER_MODULE(dfs, cpu, dfs_driver, dfs_devclass, 0, 0);
+
+/*
+ * Bits of the HID1 register to enable DFS. See page 2-24 of "MPC7450
+ * RISC Microprocessor Family Reference Manual", rev. 5.
+ */
+
+#define	HID1_DFS2	(1UL << 22)
+#define	HID1_DFS4	(1UL << 23)
+
+static void
+dfs_identify(driver_t *driver, device_t parent)
+{
+	uint16_t vers;
+	vers = mfpvr() >> 16;
+
+	/* Check for an MPC 7447A or 7448 CPU */
+	switch (vers) {
+		case MPC7447A:
+		case MPC7448:
+			break;
+		default:
+			return;
+	}
+
+	/* Make sure we're not being doubly invoked. */
+	if (device_find_child(parent, "dfs", -1) != NULL)
+		return;
+
+	/*
+	 * We attach a child for every CPU since settings need to
+	 * be performed on every CPU in the SMP case.
+	 */
+	if (BUS_ADD_CHILD(parent, 10, "dfs", -1) == NULL)
+		device_printf(parent, "add dfs child failed\n");
+}
+
+static int
+dfs_probe(device_t dev)
+{
+	if (resource_disabled("dfs", 0))
+		return (ENXIO);
+
+	device_set_desc(dev, "Dynamic Frequency Switching");
+	return (0);
+}
+
+static int
+dfs_attach(device_t dev)
+{
+	struct dfs_softc *sc;
+	uint16_t vers;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+	sc->dfs4 = 0;
+	vers = mfpvr() >> 16;
+
+	/* The 7448 supports divide-by-four as well */
+	if (vers == MPC7448)
+		sc->dfs4 = 1;
+
+	cpufreq_register(dev);
+	return (0);
+}
+
+static int
+dfs_settings(device_t dev, struct cf_setting *sets, int *count)
+{
+	struct dfs_softc *sc;
+	int states;
+
+	sc = device_get_softc(dev);
+	states = sc->dfs4 ? 3 : 2;
+	if (sets == NULL || count == NULL)
+		return (EINVAL);
+	if (*count < states)
+		return (E2BIG);
+
+	/* Return a list of valid settings for this driver. */
+	memset(sets, CPUFREQ_VAL_UNKNOWN, sizeof(*sets) * states);
+
+	sets[0].freq = 10000; sets[0].dev = dev;
+	sets[1].freq = 5000; sets[1].dev = dev;
+	if (sc->dfs4)
+		sets[2].freq = 2500; sets[2].dev = dev;
+	*count = states;
+
+	return (0);
+}
+
+static int
+dfs_set(device_t dev, const struct cf_setting *set)
+{
+	struct dfs_softc *sc;
+	register_t hid1;
+	
+	if (set == NULL)
+		return (EINVAL);
+	sc = device_get_softc(dev);
+
+	hid1 = mfspr(SPR_HID1);
+	hid1 &= ~(HID1_DFS2 | HID1_DFS4);
+
+	if (set->freq == 5000)
+		hid1 |= HID1_DFS2;
+	else if (set->freq == 2500)
+		hid1 |= HID1_DFS4;
+	
+	/*
+	 * Now set the HID1 register with new values. Calling sequence
+	 * taken from page 2-26 of the MPC7450 family CPU manual.
+	 */
+
+	powerpc_sync();
+	mtspr(SPR_HID1, hid1);
+	powerpc_sync(); isync();
+
+	return (0);
+}
+
+static int
+dfs_get(device_t dev, struct cf_setting *set)
+{
+	struct dfs_softc *sc;
+	register_t hid1;
+
+	if (set == NULL)
+		return (EINVAL);
+	sc = device_get_softc(dev);
+
+	memset(set, CPUFREQ_VAL_UNKNOWN, sizeof(*set));
+
+	hid1 = mfspr(SPR_HID1);
+
+	set->freq = 10000;
+	if (hid1 & HID1_DFS2)
+		set->freq = 5000;
+	else if (sc->dfs4 && (hid1 & HID1_DFS4))
+		set->freq = 2500;
+
+	set->dev = dev;
+
+	return (0);
+}
+
+static int
+dfs_type(device_t dev, int *type)
+{
+
+	if (type == NULL)
+		return (EINVAL);
+
+	*type = CPUFREQ_TYPE_RELATIVE;
+	return (0);
+}
+
diff --git a/sys/powerpc/ofw/ofw_cpu.c b/sys/powerpc/ofw/ofw_cpu.c
new file mode 100644
index 0000000..846556e
--- /dev/null
+++ b/sys/powerpc/ofw/ofw_cpu.c
@@ -0,0 +1,206 @@
+/*-
+ * Copyright (C) 2009 Nathan Whitehorn
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Benno Rice ``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 TOOLS GMBH 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.
+ *
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/bus.h>
+#include <sys/cpu.h>
+#include <machine/bus.h>
+
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+static int	ofw_cpulist_probe(device_t);
+static int	ofw_cpulist_attach(device_t);
+static const struct ofw_bus_devinfo *ofw_cpulist_get_devinfo(device_t dev,
+    device_t child);
+
+static MALLOC_DEFINE(M_OFWCPU, "ofwcpu", "OFW CPU device information");
+
+static device_method_t ofw_cpulist_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		ofw_cpulist_probe),
+	DEVMETHOD(device_attach,	ofw_cpulist_attach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_driver_added,	bus_generic_driver_added),
+	DEVMETHOD(bus_add_child,	bus_generic_add_child),
+	DEVMETHOD(bus_print_child,	bus_generic_print_child),
+	DEVMETHOD(bus_child_pnpinfo_str, ofw_bus_gen_child_pnpinfo_str),
+
+	/* ofw_bus interface */
+	DEVMETHOD(ofw_bus_get_devinfo,	ofw_cpulist_get_devinfo),
+	DEVMETHOD(ofw_bus_get_compat,	ofw_bus_gen_get_compat),
+	DEVMETHOD(ofw_bus_get_model,	ofw_bus_gen_get_model),
+	DEVMETHOD(ofw_bus_get_name,	ofw_bus_gen_get_name),
+	DEVMETHOD(ofw_bus_get_node,	ofw_bus_gen_get_node),
+	DEVMETHOD(ofw_bus_get_type,	ofw_bus_gen_get_type),
+
+	{ 0, 0 }
+};
+
+static driver_t ofw_cpulist_driver = {
+	"cpulist",
+	ofw_cpulist_methods,
+	0
+};
+
+static devclass_t ofw_cpulist_devclass;
+
+DRIVER_MODULE(ofw_cpulist, nexus, ofw_cpulist_driver, ofw_cpulist_devclass,
+    0, 0);
+
+static int 
+ofw_cpulist_probe(device_t dev) 
+{
+	const char	*name;
+
+	name = ofw_bus_get_name(dev);
+
+	if (name == NULL || strcmp(name, "cpus") != 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "Open Firmware CPU Group");
+
+	return (0);
+}
+
+static int 
+ofw_cpulist_attach(device_t dev) 
+{
+	phandle_t root, child;
+	device_t cdev;
+	struct ofw_bus_devinfo *dinfo;
+
+	root = ofw_bus_get_node(dev);
+
+	for (child = OF_child(root); child != 0; child = OF_peer(child)) {
+		dinfo = malloc(sizeof(*dinfo), M_OFWCPU, M_WAITOK | M_ZERO);
+
+                if (ofw_bus_gen_setup_devinfo(dinfo, child) != 0) {
+                        free(dinfo, M_OFWCPU);
+                        continue;
+                }
+                cdev = device_add_child(dev, NULL, -1);
+                if (cdev == NULL) {
+                        device_printf(dev, "<%s>: device_add_child failed\n",
+                            dinfo->obd_name);
+                        ofw_bus_gen_destroy_devinfo(dinfo);
+                        free(dinfo, M_OFWCPU);
+                        continue;
+                }
+		device_set_ivars(cdev, dinfo);
+	}
+
+	return (bus_generic_attach(dev));
+}
+
+static const struct ofw_bus_devinfo *
+ofw_cpulist_get_devinfo(device_t dev, device_t child) 
+{
+	return (device_get_ivars(child));	
+}
+
+static int	ofw_cpu_probe(device_t);
+static int	ofw_cpu_attach(device_t);
+static int	ofw_cpu_read_ivar(device_t dev, device_t child, int index,
+    uintptr_t *result);
+
+static device_method_t ofw_cpu_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		ofw_cpu_probe),
+	DEVMETHOD(device_attach,	ofw_cpu_attach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_driver_added,	bus_generic_driver_added),
+	DEVMETHOD(bus_add_child,	bus_generic_add_child),
+	DEVMETHOD(bus_print_child,	bus_generic_print_child),
+	DEVMETHOD(bus_read_ivar,	ofw_cpu_read_ivar),
+	DEVMETHOD(bus_setup_intr,	bus_generic_setup_intr),
+	DEVMETHOD(bus_teardown_intr,	bus_generic_teardown_intr),
+	DEVMETHOD(bus_alloc_resource,	bus_generic_alloc_resource),
+	DEVMETHOD(bus_release_resource,	bus_generic_release_resource),
+	DEVMETHOD(bus_activate_resource,bus_generic_activate_resource),
+
+	{ 0, 0 }
+};
+
+static driver_t ofw_cpu_driver = {
+	"cpu",
+	ofw_cpu_methods,
+	0
+};
+
+static devclass_t ofw_cpu_devclass;
+
+DRIVER_MODULE(ofw_cpu, cpulist, ofw_cpu_driver, ofw_cpu_devclass, 0, 0);
+
+static int
+ofw_cpu_probe(device_t dev)
+{
+	const char *type = ofw_bus_get_type(dev);
+
+	if (strcmp(type, "cpu") != 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "Open Firmware CPU");
+	return (0);
+}
+
+static int
+ofw_cpu_attach(device_t dev)
+{
+	bus_generic_probe(dev);
+	return (bus_generic_attach(dev));
+}
+
+static int
+ofw_cpu_read_ivar(device_t dev, device_t child, int index, uintptr_t *result)
+{
+	uint32_t cell;
+
+	switch (index) {
+	case CPU_IVAR_PCPU:
+		OF_getprop(ofw_bus_get_node(dev), "reg", &cell, sizeof(cell));
+		*result = (uintptr_t)(pcpu_find(cell));
+		return (0);
+	case CPU_IVAR_NOMINAL_MHZ:
+		cell = 0;
+		OF_getprop(ofw_bus_get_node(dev), "clock-frequency",
+		    &cell, sizeof(cell));
+		cell /= 1000000; /* convert to MHz */
+		*result = (uintptr_t)(cell);
+		return (0);
+	}
+
+	return (ENOENT);
+}
+
diff --git a/sys/powerpc/powermac/vcoregpio.c b/sys/powerpc/powermac/vcoregpio.c
new file mode 100644
index 0000000..463e8cd
--- /dev/null
+++ b/sys/powerpc/powermac/vcoregpio.c
@@ -0,0 +1,112 @@
+/*-
+ * Copyright (c) 2009 Nathan Whitehorn
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/module.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/cpu.h>
+#include <sys/kernel.h>
+#include <sys/sysctl.h>
+
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/openfirm.h>
+
+#include <powerpc/powermac/macgpiovar.h>
+
+static int	vcoregpio_probe(device_t);
+static int	vcoregpio_attach(device_t);
+static void	vcoregpio_pre_change(device_t, const struct cf_level *level);
+static void	vcoregpio_post_change(device_t, const struct cf_level *level);
+
+static device_method_t  vcoregpio_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_probe,		vcoregpio_probe),
+	DEVMETHOD(device_attach,	vcoregpio_attach),
+	{ 0, 0 },
+};
+
+static driver_t vcoregpio_driver = {
+	"vcoregpio",
+	vcoregpio_methods,
+	0
+};
+
+static devclass_t vcoregpio_devclass;
+
+DRIVER_MODULE(vcoregpio, macgpio, vcoregpio_driver, vcoregpio_devclass, 0, 0);
+
+static int
+vcoregpio_probe(device_t dev)
+{
+	const char *name = ofw_bus_get_name(dev);
+
+	if (strcmp(name, "cpu-vcore-select") != 0)
+		return (ENXIO);
+
+	device_set_desc(dev, "CPU Core Voltage Control");
+	return (0);
+}
+
+static int
+vcoregpio_attach(device_t dev)
+{
+	EVENTHANDLER_REGISTER(cpufreq_pre_change, vcoregpio_pre_change, dev,
+	    EVENTHANDLER_PRI_ANY);
+	EVENTHANDLER_REGISTER(cpufreq_post_change, vcoregpio_post_change, dev,
+	    EVENTHANDLER_PRI_ANY);
+
+	return (0);
+}
+
+static void
+vcoregpio_pre_change(device_t dev, const struct cf_level *level)
+{
+	if (level->rel_set[0].freq == 10000 /* max */) {
+		/*
+		 * Make sure the CPU voltage is raised before we raise
+		 * the clock.
+		 */
+		macgpio_write(dev, GPIO_DDR_OUTPUT | 1);
+		DELAY(1000);
+	}
+}
+
+static void
+vcoregpio_post_change(device_t dev, const struct cf_level *level)
+{
+	if (level->rel_set[0].freq < 10000 /* max */) {
+		DELAY(1000);
+		/* We are safe to reduce CPU voltage now */
+		macgpio_write(dev, GPIO_DDR_OUTPUT | 0);
+	}
+}
+
diff --git a/sys/powerpc/powerpc/cpu.c b/sys/powerpc/powerpc/cpu.c
index 467bea9..091fa55 100644
--- a/sys/powerpc/powerpc/cpu.c
+++ b/sys/powerpc/powerpc/cpu.c
@@ -62,6 +62,7 @@
 #include <sys/systm.h>
 #include <sys/bus.h>
 #include <sys/conf.h>
+#include <sys/cpu.h>
 #include <sys/kernel.h>
 #include <sys/sysctl.h>
 
@@ -288,13 +289,38 @@ void
 cpu_print_speed(void)
 {
 	uint64_t	cps;
+	
+	if (cpu_est_clockrate(0, &cps) == 0)
+		printf(", %lld.%02lld MHz", cps / 1000000, (cps / 10000) % 100);
+}
+
+/* Get current clock frequency for the given cpu id. */
+int
+cpu_est_clockrate(int cpu_id, uint64_t *cps)
+{
+	uint16_t	vers;
+
+	vers = mfpvr() >> 16;
 
-	mtspr(SPR_MMCR0, SPR_MMCR0_FC);
-	mtspr(SPR_PMC1, 0);
-	mtspr(SPR_MMCR0, SPR_MMCR0_PMC1SEL(PMCN_CYCLES));
-	DELAY(100000);
-	cps = (mfspr(SPR_PMC1) * 10) + 4999;
-	printf(", %lld.%02lld MHz", cps / 1000000, (cps / 10000) % 100);
+	switch (vers) {
+		case MPC7450:
+		case MPC7455:
+		case MPC7457:
+		case MPC750:
+		case IBM750FX:
+		case MPC7400:
+		case MPC7410:
+		case MPC7447A:
+		case MPC7448:
+			mtspr(SPR_MMCR0, SPR_MMCR0_FC);
+			mtspr(SPR_PMC1, 0);
+			mtspr(SPR_MMCR0, SPR_MMCR0_PMC1SEL(PMCN_CYCLES));
+			DELAY(100000);
+			*cps = (mfspr(SPR_PMC1) * 10) + 4999;
+			return (0);
+	}
+		
+	return (ENXIO);
 }
 
 void