MFC r259284,r259287

Add PMU-based CPU frequency scalling.  This is used on most Titanium
PowerBooks.

14 files changed, 564 insertions, 33 deletions

diff --git a/sys/conf/files.powerpc b/sys/conf/files.powerpc
index 275d730..254e3a7 100644
--- a/sys/conf/files.powerpc
+++ b/sys/conf/files.powerpc
@@ -116,6 +116,7 @@ powerpc/booke/trap.c		optional	booke
 powerpc/booke/vm_machdep.c	optional	booke
 powerpc/cpufreq/dfs.c		optional	cpufreq
 powerpc/cpufreq/pcr.c		optional	cpufreq aim
+powerpc/cpufreq/pmufreq.c	optional	cpufreq aim pmu
 powerpc/fpu/fpu_add.c		optional	fpu_emu powerpc
 powerpc/fpu/fpu_compare.c	optional	fpu_emu powerpc
 powerpc/fpu/fpu_div.c		optional	fpu_emu powerpc
diff --git a/sys/powerpc/aim/mp_cpudep.c b/sys/powerpc/aim/mp_cpudep.c
index 607256c..3fcab83 100644
--- a/sys/powerpc/aim/mp_cpudep.c
+++ b/sys/powerpc/aim/mp_cpudep.c
@@ -322,17 +322,13 @@ cpudep_ap_setup()
 		mtspr(SPR_CELL_TSRL, bsp_state[5]);
 
 		break;
-	case MPC7450:
-	case MPC7455:
-	case MPC7457:
-		/* Only MPC745x CPUs have an L3 cache. */
-		reg = mpc745x_l3_enable(bsp_state[3]);
-		
-		/* Fallthrough */
 	case MPC7400:
 	case MPC7410:
 	case MPC7447A:
 	case MPC7448:
+	case MPC7450:
+	case MPC7455:
+	case MPC7457:
 		/* XXX: Program the CPU ID into PIR */
 		__asm __volatile("mtspr 1023,%0" :: "r"(PCPU_GET(cpuid)));
 
@@ -342,6 +338,17 @@ cpudep_ap_setup()
 		mtspr(SPR_HID0, bsp_state[0]); isync();
 		mtspr(SPR_HID1, bsp_state[1]); isync();
 
+		/* Now enable the L3 cache. */
+		switch (vers) {
+		case MPC7450:
+		case MPC7455:
+		case MPC7457:
+			/* Only MPC745x CPUs have an L3 cache. */
+			reg = mpc745x_l3_enable(bsp_state[3]);
+		default:
+			break;
+		}
+		
 		reg = mpc74xx_l2_enable(bsp_state[2]);
 		reg = mpc74xx_l1d_enable();
 		reg = mpc74xx_l1i_enable();
diff --git a/sys/powerpc/cpufreq/pmufreq.c b/sys/powerpc/cpufreq/pmufreq.c
new file mode 100644
index 0000000..2fbeef3
--- /dev/null
+++ b/sys/powerpc/cpufreq/pmufreq.c
@@ -0,0 +1,221 @@
+/*-
+ * Copyright (c) 2011 Justin Hibbits
+ * 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/limits.h>
+#include <sys/module.h>
+
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/openfirm.h>
+
+#include "cpufreq_if.h"
+#include "powerpc/powermac/pmuvar.h"
+
+struct pmufreq_softc {
+	device_t dev;
+	uint32_t minfreq;
+	uint32_t maxfreq;
+	uint32_t curfreq;
+};
+
+static void	pmufreq_identify(driver_t *driver, device_t parent);
+static int	pmufreq_probe(device_t dev);
+static int	pmufreq_attach(device_t dev);
+static int	pmufreq_settings(device_t dev, struct cf_setting *sets, int *count);
+static int	pmufreq_set(device_t dev, const struct cf_setting *set);
+static int	pmufreq_get(device_t dev, struct cf_setting *set);
+static int	pmufreq_type(device_t dev, int *type);
+
+static device_method_t pmufreq_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_identify,	pmufreq_identify),
+	DEVMETHOD(device_probe,		pmufreq_probe),
+	DEVMETHOD(device_attach,	pmufreq_attach),
+
+	/* cpufreq interface */
+	DEVMETHOD(cpufreq_drv_set,	pmufreq_set),
+	DEVMETHOD(cpufreq_drv_get,	pmufreq_get),
+	DEVMETHOD(cpufreq_drv_type,	pmufreq_type),
+	DEVMETHOD(cpufreq_drv_settings,	pmufreq_settings),
+
+	{0, 0}
+};
+
+static driver_t pmufreq_driver = {
+	"pmufreq",
+	pmufreq_methods,
+	sizeof(struct pmufreq_softc)
+};
+
+static devclass_t pmufreq_devclass;
+DRIVER_MODULE(pmufreq, cpu, pmufreq_driver, pmufreq_devclass, 0, 0);
+
+static void
+pmufreq_identify(driver_t *driver, device_t parent)
+{
+	phandle_t node;
+	uint32_t min_freq;
+
+	node = ofw_bus_get_node(parent);
+	if (OF_getprop(node, "min-clock-frequency", &min_freq, sizeof(min_freq)) == -1)
+		return;
+
+	/* Make sure we're not being doubly invoked. */
+	if (device_find_child(parent, "pmufreq", -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, "pmufreq", -1) == NULL)
+		device_printf(parent, "add pmufreq child failed\n");
+}
+
+static int
+pmufreq_probe(device_t dev)
+{
+	struct pmufreq_softc *sc;
+	phandle_t node;
+	uint32_t min_freq;
+
+	if (resource_disabled("pmufreq", 0))
+		return (ENXIO);
+
+	sc = device_get_softc(dev);
+	node = ofw_bus_get_node(device_get_parent(dev));
+	/*
+	 * A scalable MPC7455 has min-clock-frequency/max-clock-frequency as OFW
+	 * properties of the 'cpu' node.
+	 */
+	if (OF_getprop(node, "min-clock-frequency", &min_freq, sizeof(min_freq)) == -1)
+		return (ENXIO);
+	device_set_desc(dev, "PMU-based frequency scaling");
+	return (0);
+}
+
+static int
+pmufreq_attach(device_t dev)
+{
+	struct pmufreq_softc *sc;
+	phandle_t node;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	node = ofw_bus_get_node(device_get_parent(dev));
+	OF_getprop(node, "min-clock-frequency", &sc->minfreq, sizeof(sc->minfreq));
+	OF_getprop(node, "max-clock-frequency", &sc->maxfreq, sizeof(sc->maxfreq));
+	OF_getprop(node, "rounded-clock-frequency", &sc->curfreq, sizeof(sc->curfreq));
+	sc->minfreq /= 1000000;
+	sc->maxfreq /= 1000000;
+	sc->curfreq /= 1000000;
+
+	cpufreq_register(dev);
+	return (0);
+}
+
+static int
+pmufreq_settings(device_t dev, struct cf_setting *sets, int *count)
+{
+	struct pmufreq_softc *sc;
+
+	sc = device_get_softc(dev);
+	if (sets == NULL || count == NULL)
+		return (EINVAL);
+	if (*count < 2)
+		return (E2BIG);
+
+	/* Return a list of valid settings for this driver. */
+	memset(sets, CPUFREQ_VAL_UNKNOWN, sizeof(*sets) * 2);
+
+	sets[0].freq = sc->maxfreq; sets[0].dev = dev;
+	sets[1].freq = sc->minfreq; sets[1].dev = dev;
+	/* Set high latency for CPU frequency changes, it's a tedious process. */
+	sets[0].lat = INT_MAX;
+	sets[1].lat = INT_MAX;
+	*count = 2;
+
+	return (0);
+}
+
+static int
+pmufreq_set(device_t dev, const struct cf_setting *set)
+{
+	struct pmufreq_softc *sc;
+	int error, speed_sel;
+
+	if (set == NULL)
+		return (EINVAL);
+
+	sc = device_get_softc(dev);
+
+	if (set->freq == sc->maxfreq)
+		speed_sel = 0;
+	else
+		speed_sel = 1;
+
+	error = pmu_set_speed(speed_sel);
+	if (error == 0)
+		sc->curfreq = set->freq;
+
+	return (error);
+}
+
+static int
+pmufreq_get(device_t dev, struct cf_setting *set)
+{
+	struct pmufreq_softc *sc;
+
+	if (set == NULL)
+		return (EINVAL);
+	sc = device_get_softc(dev);
+
+	set->freq = sc->curfreq;
+	set->dev = dev;
+
+	return (0);
+}
+
+static int
+pmufreq_type(device_t dev, int *type)
+{
+
+	if (type == NULL)
+		return (EINVAL);
+
+	*type = CPUFREQ_TYPE_ABSOLUTE;
+	return (0);
+}
+
diff --git a/sys/powerpc/include/cpu.h b/sys/powerpc/include/cpu.h
index bf4db83..d684d68 100644
--- a/sys/powerpc/include/cpu.h
+++ b/sys/powerpc/include/cpu.h
@@ -98,4 +98,6 @@ void	cpu_reset(void);
 void	fork_trampoline(void);
 void	swi_vm(void *);
 
+void	flush_disable_caches(void);
+
 #endif	/* _MACHINE_CPU_H_ */
diff --git a/sys/powerpc/include/dbdma.h b/sys/powerpc/include/dbdma.h
index 2095ad3..22e4b0c 100644
--- a/sys/powerpc/include/dbdma.h
+++ b/sys/powerpc/include/dbdma.h
@@ -147,4 +147,7 @@ void dbdma_insert_branch(dbdma_channel_t *chan, int slot, int to_slot);
 
 void dbdma_sync_commands(dbdma_channel_t *chan, bus_dmasync_op_t op);
 
+void dbdma_save_state(dbdma_channel_t *chan);
+void dbdma_restore_state(dbdma_channel_t *chan);
+
 #endif /* _MACHINE_DBDMA_H_ */
diff --git a/sys/powerpc/include/pcpu.h b/sys/powerpc/include/pcpu.h
index 7ab184e..96e26cb 100644
--- a/sys/powerpc/include/pcpu.h
+++ b/sys/powerpc/include/pcpu.h
@@ -49,7 +49,8 @@ struct pmap;
 	uint32_t	pc_ipimask;					\
 	register_t	pc_tempsave[CPUSAVE_LEN];			\
 	register_t	pc_disisave[CPUSAVE_LEN];			\
-	register_t	pc_dbsave[CPUSAVE_LEN];
+	register_t	pc_dbsave[CPUSAVE_LEN];				\
+	void		*pc_restore;
 
 #define PCPU_MD_AIM32_FIELDS						\
 	/* char		__pad[0] */
diff --git a/sys/powerpc/include/spr.h b/sys/powerpc/include/spr.h
index f99b67f..f275069 100644
--- a/sys/powerpc/include/spr.h
+++ b/sys/powerpc/include/spr.h
@@ -519,6 +519,8 @@
 #define	  MSSCR0_EMODE		  0x00200000 /* 10: MPX bus mode (read-only) */
 #define	  MSSCR0_ABD		  0x00100000 /* 11: address bus driven (read-only) */
 #define	  MSSCR0_MBZ		  0x000fffff /* 12-31: must be zero */
+#define	  MSSCR0_L2PFE		  0x00000003 /* 30-31: L2 prefetch enable */
+#define	SPR_LDSTCR		0x3f8	/* .6. Load/Store Control Register */
 #define	SPR_L2PM		0x3f8	/* .6. L2 Private Memory Control Register */
 #define	SPR_L2CR		0x3f9	/* .6. L2 Control Register */
 #define	  L2CR_L2E		  0x80000000 /* 0: L2 enable */
@@ -543,12 +545,14 @@
 				      Setting this bit disables instruction
 				      caching. */
 #define	  L2CR_L2I		  0x00200000 /* 10: L2 global invalidate. */
+#define	  L2CR_L2IO_7450	  0x00010000 /* 11: L2 instruction-only (MPC745x). */
 #define	  L2CR_L2CTL		  0x00100000 /* 11: L2 RAM control (ZZ enable).
 				      Enables automatic operation of the
 				      L2ZZ (low-power mode) signal. */
 #define	  L2CR_L2WT		  0x00080000 /* 12: L2 write-through. */
 #define	  L2CR_L2TS		  0x00040000 /* 13: L2 test support. */
 #define	  L2CR_L2OH		  0x00030000 /* 14-15: L2 output hold. */
+#define	  L2CR_L2DO_7450	  0x00010000 /* 15: L2 data-only (MPC745x). */
 #define	  L2CR_L2SL		  0x00008000 /* 16: L2 DLL slow. */
 #define	  L2CR_L2DF		  0x00004000 /* 17: L2 differential clock. */
 #define	  L2CR_L2BYP		  0x00002000 /* 18: L2 DLL bypass. */
diff --git a/sys/powerpc/powermac/platform_powermac.c b/sys/powerpc/powermac/platform_powermac.c
index cddc883..82420dc 100644
--- a/sys/powerpc/powermac/platform_powermac.c
+++ b/sys/powerpc/powermac/platform_powermac.c
@@ -326,6 +326,110 @@ powermac_smp_start_cpu(platform_t plat, struct pcpu *pc)
 #endif
 }
 
+/* From p3-53 of the MPC7450 RISC Microprocessor Family Reference Manual */
+void
+flush_disable_caches(void)
+{
+	register_t msr;
+	register_t msscr0;
+	register_t cache_reg;
+	volatile uint32_t *memp;
+	uint32_t temp;
+	int i;
+	int x;
+
+	msr = mfmsr();
+	powerpc_sync();
+	mtmsr(msr & ~(PSL_EE | PSL_DR));
+	msscr0 = mfspr(SPR_MSSCR0);
+	msscr0 &= ~MSSCR0_L2PFE;
+	mtspr(SPR_MSSCR0, msscr0);
+	powerpc_sync();
+	isync();
+	__asm__ __volatile__("dssall; sync");
+	powerpc_sync();
+	isync();
+	__asm__ __volatile__("dcbf 0,%0" :: "r"(0));
+	__asm__ __volatile__("dcbf 0,%0" :: "r"(0));
+	__asm__ __volatile__("dcbf 0,%0" :: "r"(0));
+
+	/* Lock the L1 Data cache. */
+	mtspr(SPR_LDSTCR, mfspr(SPR_LDSTCR) | 0xFF);
+	powerpc_sync();
+	isync();
+
+	mtspr(SPR_LDSTCR, 0);
+
+	/*
+	 * Perform this in two stages: Flush the cache starting in RAM, then do it
+	 * from ROM.
+	 */
+	memp = (volatile uint32_t *)0x00000000;
+	for (i = 0; i < 128 * 1024; i++) {
+		temp = *memp;
+		__asm__ __volatile__("dcbf 0,%0" :: "r"(memp));
+		memp += 32/sizeof(*memp);
+	}
+
+	memp = (volatile uint32_t *)0xfff00000;
+	x = 0xfe;
+
+	for (; x != 0xff;) {
+		mtspr(SPR_LDSTCR, x);
+		for (i = 0; i < 128; i++) {
+			temp = *memp;
+			__asm__ __volatile__("dcbf 0,%0" :: "r"(memp));
+			memp += 32/sizeof(*memp);
+		}
+		x = ((x << 1) | 1) & 0xff;
+	}
+	mtspr(SPR_LDSTCR, 0);
+
+	cache_reg = mfspr(SPR_L2CR);
+	if (cache_reg & L2CR_L2E) {
+		cache_reg &= ~(L2CR_L2IO_7450 | L2CR_L2DO_7450);
+		mtspr(SPR_L2CR, cache_reg);
+		powerpc_sync();
+		mtspr(SPR_L2CR, cache_reg | L2CR_L2HWF);
+		while (mfspr(SPR_L2CR) & L2CR_L2HWF)
+			; /* Busy wait for cache to flush */
+		powerpc_sync();
+		cache_reg &= ~L2CR_L2E;
+		mtspr(SPR_L2CR, cache_reg);
+		powerpc_sync();
+		mtspr(SPR_L2CR, cache_reg | L2CR_L2I);
+		powerpc_sync();
+		while (mfspr(SPR_L2CR) & L2CR_L2I)
+			; /* Busy wait for L2 cache invalidate */
+		powerpc_sync();
+	}
+
+	cache_reg = mfspr(SPR_L3CR);
+	if (cache_reg & L3CR_L3E) {
+		cache_reg &= ~(L3CR_L3IO | L3CR_L3DO);
+		mtspr(SPR_L3CR, cache_reg);
+		powerpc_sync();
+		mtspr(SPR_L3CR, cache_reg | L3CR_L3HWF);
+		while (mfspr(SPR_L3CR) & L3CR_L3HWF)
+			; /* Busy wait for cache to flush */
+		powerpc_sync();
+		cache_reg &= ~L3CR_L3E;
+		mtspr(SPR_L3CR, cache_reg);
+		powerpc_sync();
+		mtspr(SPR_L3CR, cache_reg | L3CR_L3I);
+		powerpc_sync();
+		while (mfspr(SPR_L3CR) & L3CR_L3I)
+			; /* Busy wait for L3 cache invalidate */
+		powerpc_sync();
+	}
+
+	mtspr(SPR_HID0, mfspr(SPR_HID0) & ~HID0_DCE);
+	powerpc_sync();
+	isync();
+
+	mtmsr(msr);
+}
+
 static void
 powermac_reset(platform_t platform)
 {
diff --git a/sys/powerpc/powermac/pmu.c b/sys/powerpc/powermac/pmu.c
index 9777004..ca979d8 100644
--- a/sys/powerpc/powermac/pmu.c
+++ b/sys/powerpc/powermac/pmu.c
@@ -36,6 +36,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/conf.h>
 #include <sys/kernel.h>
 #include <sys/clock.h>
+#include <sys/proc.h>
 #include <sys/reboot.h>
 #include <sys/sysctl.h>
 
@@ -43,11 +44,18 @@ __FBSDID("$FreeBSD$");
 #include <dev/ofw/openfirm.h>
 #include <dev/led/led.h>
 
+#include <machine/_inttypes.h>
+#include <machine/altivec.h>	/* For save_vec() */
 #include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/fpu.h>	/* For save_fpu() */
+#include <machine/hid.h>
 #include <machine/intr_machdep.h>
 #include <machine/md_var.h>
+#include <machine/pcb.h>
 #include <machine/pio.h>
 #include <machine/resource.h>
+#include <machine/setjmp.h>
 
 #include <vm/vm.h>
 #include <vm/pmap.h>
@@ -59,6 +67,11 @@ __FBSDID("$FreeBSD$");
 #include "clock_if.h"
 #include "pmuvar.h"
 #include "viareg.h"
+#include "uninorthvar.h"	/* For unin_chip_sleep()/unin_chip_wake() */
+
+#define PMU_DEFAULTS	PMU_INT_TICK | PMU_INT_ADB | \
+	PMU_INT_PCEJECT | PMU_INT_SNDBRT | \
+	PMU_INT_BATTERY | PMU_INT_ENVIRONMENT
 
 /*
  * Bus interface
@@ -93,6 +106,7 @@ static int	pmu_acline_state(SYSCTL_HANDLER_ARGS);
 static int	pmu_query_battery(struct pmu_softc *sc, int batt, 
 		    struct pmu_battstate *info);
 static int	pmu_battquery_sysctl(SYSCTL_HANDLER_ARGS);
+static void	pmu_sleep_int(void);
 
 /*
  * List of battery-related sysctls we might ask for
@@ -115,8 +129,6 @@ static device_method_t  pmu_methods[] = {
 	DEVMETHOD(device_attach,	pmu_attach),
         DEVMETHOD(device_detach,        pmu_detach),
         DEVMETHOD(device_shutdown,      bus_generic_shutdown),
-        DEVMETHOD(device_suspend,       bus_generic_suspend),
-        DEVMETHOD(device_resume,        bus_generic_resume),
 
 	/* ADB bus interface */
 	DEVMETHOD(adb_hb_send_raw_packet,   pmu_adb_send),
@@ -193,7 +205,7 @@ static signed char pm_send_cmd_type[] = {
 	0x02,   -1,   -1,   -1,   -1,   -1,   -1,   -1,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,   -1,   -1,
 	0x01, 0x01, 0x01,   -1,   -1,   -1,   -1,   -1,
-	0x00, 0x00,   -1,   -1,   -1,   -1, 0x04, 0x04,
+	0x00, 0x00,   -1,   -1,   -1, 0x05, 0x04, 0x04,
 	0x04,   -1, 0x00,   -1,   -1,   -1,   -1,   -1,
 	0x00,   -1,   -1,   -1,   -1,   -1,   -1,   -1,
 	0x01, 0x02,   -1,   -1,   -1,   -1,   -1,   -1,
@@ -229,7 +241,7 @@ static signed char pm_receive_cmd_type[] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	0x04, 0x04, 0x03, 0x09,   -1,   -1,   -1,   -1,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  -1,   -1,   -1,   -1,   -1,   -1, 0x01, 0x01,
+	  -1,   -1,   -1,   -1,   -1, 0x01, 0x01, 0x01,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	0x06,   -1,   -1,   -1,   -1,   -1,   -1,   -1,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -357,12 +369,13 @@ pmu_attach(device_t dev)
 
 	/* Init PMU */
 
-	reg = PMU_INT_TICK | PMU_INT_ADB | PMU_INT_PCEJECT | PMU_INT_SNDBRT;
-	reg |= PMU_INT_BATTERY;
-	reg |= PMU_INT_ENVIRONMENT;
+	pmu_write_reg(sc, vBufB, pmu_read_reg(sc, vBufB) | vPB4);
+	pmu_write_reg(sc, vDirB, (pmu_read_reg(sc, vDirB) | vPB4) & ~vPB3);
+
+	reg = PMU_DEFAULTS;
 	pmu_send(sc, PMU_SET_IMASK, 1, &reg, 16, resp);
 
-	pmu_write_reg(sc, vIER, 0x90); /* make sure VIA interrupts are on */
+	pmu_write_reg(sc, vIER, 0x94); /* make sure VIA interrupts are on */
 
 	pmu_send(sc, PMU_SYSTEM_READY, 1, cmd, 16, resp);
 	pmu_send(sc, PMU_GET_VERSION, 1, cmd, 16, resp);
@@ -1018,3 +1031,95 @@ pmu_settime(device_t dev, struct timespec *ts)
 	return (0);
 }
 
+static register_t sprgs[4];
+static register_t srrs[2];
+extern void *ap_pcpu;
+
+void pmu_sleep_int(void)
+{
+	static u_quad_t timebase = 0;
+	jmp_buf resetjb;
+	struct thread *fputd;
+	struct thread *vectd;
+	register_t hid0;
+	register_t msr;
+	register_t saved_msr;
+
+	ap_pcpu = pcpup;
+
+	PCPU_SET(restore, &resetjb);
+
+	*(unsigned long *)0x80 = 0x100;
+	saved_msr = mfmsr();
+	fputd = PCPU_GET(fputhread);
+	vectd = PCPU_GET(vecthread);
+	if (fputd != NULL)
+		save_fpu(fputd);
+	if (vectd != NULL)
+		save_vec(vectd);
+	if (setjmp(resetjb) == 0) {
+		sprgs[0] = mfspr(SPR_SPRG0);
+		sprgs[1] = mfspr(SPR_SPRG1);
+		sprgs[2] = mfspr(SPR_SPRG2);
+		sprgs[3] = mfspr(SPR_SPRG3);
+		srrs[0] = mfspr(SPR_SRR0);
+		srrs[1] = mfspr(SPR_SRR1);
+		timebase = mftb();
+		powerpc_sync();
+		flush_disable_caches();
+		hid0 = mfspr(SPR_HID0);
+		hid0 = (hid0 & ~(HID0_DOZE | HID0_NAP)) | HID0_SLEEP;
+		powerpc_sync();
+		isync();
+		msr = mfmsr() | PSL_POW;
+		mtspr(SPR_HID0, hid0);
+		powerpc_sync();
+
+		while (1)
+			mtmsr(msr);
+	}
+	mttb(timebase);
+	PCPU_SET(curthread, curthread);
+	PCPU_SET(curpcb, curthread->td_pcb);
+	pmap_activate(curthread);
+	powerpc_sync();
+	mtspr(SPR_SPRG0, sprgs[0]);
+	mtspr(SPR_SPRG1, sprgs[1]);
+	mtspr(SPR_SPRG2, sprgs[2]);
+	mtspr(SPR_SPRG3, sprgs[3]);
+	mtspr(SPR_SRR0, srrs[0]);
+	mtspr(SPR_SRR1, srrs[1]);
+	mtmsr(saved_msr);
+	if (fputd == curthread)
+		enable_fpu(curthread);
+	if (vectd == curthread)
+		enable_vec(curthread);
+	powerpc_sync();
+}
+
+int
+pmu_set_speed(int low_speed)
+{
+	struct pmu_softc *sc;
+	uint8_t sleepcmd[] = {'W', 'O', 'O', 'F', 0};
+	uint8_t resp[16];
+
+	sc = device_get_softc(pmu);
+	pmu_write_reg(sc, vIER, 0x10);
+	spinlock_enter();
+	mtdec(0x7fffffff);
+	mb();
+	mtdec(0x7fffffff);
+
+	sleepcmd[4] = low_speed;
+	pmu_send(sc, PMU_CPU_SPEED, 5, sleepcmd, 16, resp);
+	unin_chip_sleep(NULL, 1);
+	pmu_sleep_int();
+	unin_chip_wake(NULL);
+
+	mtdec(1);	/* Force a decrementer exception */
+	spinlock_exit();
+	pmu_write_reg(sc, vIER, 0x90);
+
+	return (0);
+}
diff --git a/sys/powerpc/powermac/pmuvar.h b/sys/powerpc/powermac/pmuvar.h
index 25a85c7..98209f8 100644
--- a/sys/powerpc/powermac/pmuvar.h
+++ b/sys/powerpc/powermac/pmuvar.h
@@ -160,7 +160,8 @@ struct pmu_softc {
 	volatile int	sc_autopoll;
 	int		sc_batteries;
 	struct cdev	*sc_leddev;
-	int	lid_closed;
+	int		lid_closed;
+	uint8_t		saved_regs[9];
 };
 
 struct pmu_battstate {
@@ -172,4 +173,6 @@ struct pmu_battstate {
 	int voltage;
 };
 
+int pmu_set_speed(int low_speed);
+
 #endif /* PMUVAR_H */
diff --git a/sys/powerpc/powermac/uninorth.c b/sys/powerpc/powermac/uninorth.c
index e9ab291..7976a81 100644
--- a/sys/powerpc/powermac/uninorth.c
+++ b/sys/powerpc/powermac/uninorth.c
@@ -136,6 +136,13 @@ static driver_t	unin_chip_driver = {
 
 static devclass_t	unin_chip_devclass;
 
+/*
+ * Assume there is only one unin chip in a PowerMac, so that pmu.c functions can
+ * suspend the chip after the whole rest of the device tree is suspended, not
+ * earlier.
+ */
+static device_t		unin_chip;
+
 DRIVER_MODULE(unin, nexus, unin_chip_driver, unin_chip_devclass, 0, 0);
 
 /*
@@ -210,31 +217,30 @@ unin_chip_add_reg(phandle_t devnode, struct unin_chip_devinfo *dinfo)
 }
 
 static void
-unin_enable_gmac(device_t dev)
+unin_update_reg(device_t dev, uint32_t regoff, uint32_t set, uint32_t clr)
 {
-	volatile u_int *clkreg;
+	volatile u_int *reg;
 	struct unin_chip_softc *sc;
 	u_int32_t tmpl;
 
 	sc = device_get_softc(dev);
-	clkreg = (void *)(sc->sc_addr + UNIN_CLOCKCNTL);
-	tmpl = inl(clkreg);
-	tmpl |= UNIN_CLOCKCNTL_GMAC;
-	outl(clkreg, tmpl);
+	reg = (void *)(sc->sc_addr + regoff);
+	tmpl = inl(reg);
+	tmpl &= ~clr;
+	tmpl |= set;
+	outl(reg, tmpl);
 }
 
 static void
-unin_enable_mpic(device_t dev)
+unin_enable_gmac(device_t dev)
 {
-	volatile u_int *toggle;
-	struct unin_chip_softc *sc;
-	u_int32_t tmpl;
+	unin_update_reg(dev, UNIN_CLOCKCNTL, UNIN_CLOCKCNTL_GMAC, 0);
+}
 
-	sc = device_get_softc(dev);
-	toggle = (void *)(sc->sc_addr + UNIN_TOGGLE_REG);
-	tmpl = inl(toggle);
-	tmpl |= UNIN_MPIC_RESET | UNIN_MPIC_OUTPUT_ENABLE;
-	outl(toggle, tmpl);
+static void
+unin_enable_mpic(device_t dev)
+{
+	unin_update_reg(dev, UNIN_TOGGLE_REG, UNIN_MPIC_RESET | UNIN_MPIC_OUTPUT_ENABLE, 0);
 }
 
 static int
@@ -301,6 +307,9 @@ unin_chip_attach(device_t dev)
 		return (error);
 	}
 
+	if (unin_chip == NULL)
+		unin_chip = dev;
+
         /*
 	 * Iterate through the sub-devices
 	 */
@@ -621,3 +630,33 @@ unin_chip_get_devinfo(device_t dev, device_t child)
 	return (&dinfo->udi_obdinfo);
 }
 
+int
+unin_chip_wake(device_t dev)
+{
+
+	if (dev == NULL)
+		dev = unin_chip;
+	unin_update_reg(dev, UNIN_PWR_MGMT, UNIN_PWR_NORMAL, UNIN_PWR_MASK);
+	DELAY(10);
+	unin_update_reg(dev, UNIN_HWINIT_STATE, UNIN_RUNNING, 0);
+	DELAY(100);
+
+	return (0);
+}
+
+int
+unin_chip_sleep(device_t dev, int idle)
+{
+	if (dev == NULL)
+		dev = unin_chip;
+
+	unin_update_reg(dev, UNIN_HWINIT_STATE, UNIN_SLEEPING, 0);
+	DELAY(10);
+	if (idle)
+		unin_update_reg(dev, UNIN_PWR_MGMT, UNIN_PWR_IDLE2, UNIN_PWR_MASK);
+	else
+		unin_update_reg(dev, UNIN_PWR_MGMT, UNIN_PWR_SLEEP, UNIN_PWR_MASK);
+	DELAY(10);
+
+	return (0);
+}
diff --git a/sys/powerpc/powermac/uninorthvar.h b/sys/powerpc/powermac/uninorthvar.h
index 43aa01d..c99a11b 100644
--- a/sys/powerpc/powermac/uninorthvar.h
+++ b/sys/powerpc/powermac/uninorthvar.h
@@ -28,6 +28,8 @@
 #ifndef	_POWERPC_POWERMAC_UNINORTHVAR_H_
 #define	_POWERPC_POWERMAC_UNINORTHVAR_H_
 
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/ofw/ofw_pci.h>
 #include <powerpc/ofw/ofw_pci.h>
 
 struct uninorth_softc {
@@ -76,10 +78,30 @@ struct unin_chip_devinfo {
 #define UNIN_CLOCKCNTL_GMAC	0x2
 
 /*
+ * Power management register
+ */
+#define UNIN_PWR_MGMT		0x30
+#define UNIN_PWR_NORMAL		0x00
+#define UNIN_PWR_IDLE2		0x01
+#define UNIN_PWR_SLEEP		0x02
+#define UNIN_PWR_SAVE		0x03
+#define UNIN_PWR_MASK		0x03
+
+/*
+ * Hardware initialization state register
+ */
+#define UNIN_HWINIT_STATE	0x70
+#define UNIN_SLEEPING		0x01
+#define UNIN_RUNNING		0x02
+
+
+/*
  * Toggle registers
  */
 #define UNIN_TOGGLE_REG		0xe0
 #define UNIN_MPIC_RESET		0x2
 #define UNIN_MPIC_OUTPUT_ENABLE	0x4
 
+extern int unin_chip_sleep(device_t dev, int idle);
+extern int unin_chip_wake(device_t dev);
 #endif  /* _POWERPC_POWERMAC_UNINORTHVAR_H_ */
diff --git a/sys/powerpc/powermac/viareg.h b/sys/powerpc/powermac/viareg.h
index 85739cd..2f519b2 100644
--- a/sys/powerpc/powermac/viareg.h
+++ b/sys/powerpc/powermac/viareg.h
@@ -30,14 +30,16 @@
 
 /* VIA interface registers */
 #define vBufB		0x0000	/* register B */
-#define vBufA		0x0200	/* register A */
 #define vDirB		0x0400	/* data direction register */
 #define vDirA		0x0600	/* data direction register */
+#define vT1C		0x0800	/* Timer 1 counter Lo */
+#define vT1CH		0x0a00	/* Timer 1 counter Hi */
 #define vSR		0x1400	/* shift register */
 #define vACR		0x1600	/* aux control register */
 #define vPCR		0x1800	/* peripheral control register */
 #define vIFR		0x1a00	/* interrupt flag register */
 #define vIER		0x1c00	/* interrupt enable register */
+#define vBufA		0x1e00	/* register A */
 
 #define vPB		0x0000
 #define vPB3		0x08
diff --git a/sys/powerpc/powerpc/mp_machdep.c b/sys/powerpc/powerpc/mp_machdep.c
index 6835d0c..28d9a08 100644
--- a/sys/powerpc/powerpc/mp_machdep.c
+++ b/sys/powerpc/powerpc/mp_machdep.c
@@ -54,6 +54,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/pcb.h>
 #include <machine/platform.h>
 #include <machine/md_var.h>
+#include <machine/setjmp.h>
 #include <machine/smp.h>
 
 #include "pic_if.h"
@@ -66,10 +67,21 @@ volatile static u_quad_t ap_timebase;
 static u_int ipi_msg_cnt[32];
 static struct mtx ap_boot_mtx;
 struct pcb stoppcbs[MAXCPU];
+int longfault(faultbuf, int);
 
 void
 machdep_ap_bootstrap(void)
 {
+	jmp_buf *restore;
+
+	/* The following is needed for restoring from sleep. */
+#ifdef __powerpc64__
+	/* Writing to the time base register is hypervisor-privileged */
+	if (mfmsr() & PSL_HV)
+		mttb(0);
+#else
+	mttb(0);
+#endif
 	/* Set up important bits on the CPU (HID registers, etc.) */
 	cpudep_ap_setup();
 
@@ -78,6 +90,11 @@ machdep_ap_bootstrap(void)
 	PCPU_SET(awake, 1);
 	__asm __volatile("msync; isync");
 
+	restore = PCPU_GET(restore);
+	if (restore != NULL) {
+		longjmp(*restore, 1);
+	}
+
 	while (ap_letgo == 0)
 		;