patch-2.4.20 linux-2.4.20/drivers/macintosh/via-pmu.c

• Lines: 895
• Date: Thu Nov 28 15:53:13 2002
• Orig file: linux-2.4.19/drivers/macintosh/via-pmu.c
• Orig date: Fri Aug 2 17:39:44 2002

diff -urN linux-2.4.19/drivers/macintosh/via-pmu.c linux-2.4.20/drivers/macintosh/via-pmu.c
@@ -9,11 +9,7 @@
  * and the RTC (real time clock) chip.
  *
  * Copyright (C) 1998 Paul Mackerras and Fabio Riccardi.
- * Copyright (C) 2001 Benjamin Herrenschmidt
- * 
- * todo: - Cleanup synchro between VIA interrupt and GPIO-based PMU
- *         interrupt.
- *
+ * Copyright (C) 2001-2002 Benjamin Herrenschmidt
  *
  */
 #include <stdarg.h>
@@ -113,14 +109,24 @@
 	reading_intr,
 } pmu_state;
 
+static volatile enum int_data_state {
+	int_data_empty,
+	int_data_fill,
+	int_data_ready,
+	int_data_flush
+} int_data_state[2] = { int_data_empty, int_data_empty };
+
 static struct adb_request *current_req;
 static struct adb_request *last_req;
 static struct adb_request *req_awaiting_reply;
-static unsigned char interrupt_data[256]; /* Made bigger: I've been told that might happen */
+static unsigned char interrupt_data[2][32];
+static int interrupt_data_len[2];
+static int int_data_last;
 static unsigned char *reply_ptr;
 static int data_index;
 static int data_len;
 static volatile int adb_int_pending;
+static volatile int disable_poll;
 static struct adb_request bright_req_1, bright_req_2, bright_req_3;
 static struct device_node *vias;
 static int pmu_kind = PMU_UNKNOWN;
@@ -176,12 +182,6 @@
 static int pmu_queue_request(struct adb_request *req);
 static void pmu_start(void);
 static void via_pmu_interrupt(int irq, void *arg, struct pt_regs *regs);
-static void send_byte(int x);
-static void recv_byte(void);
-static void pmu_sr_intr(struct pt_regs *regs);
-static void pmu_done(struct adb_request *req);
-static void pmu_handle_data(unsigned char *data, int len,
-			    struct pt_regs *regs);
 static void gpio1_interrupt(int irq, void *arg, struct pt_regs *regs);
 static int proc_get_info(char *page, char **start, off_t off,
 			  int count, int *eof, void *data);
@@ -219,9 +219,9 @@
 extern void enable_kernel_altivec(void);
 extern void enable_kernel_fp(void);
 
-#ifdef DEBUG_SLEEP
+#if defined(DEBUG_SLEEP) || defined(DEBUG_FREQ)
 int pmu_polled_request(struct adb_request *req);
-int pmu_wink(struct adb_request *req);
+void pmu_blink(int n);
 #endif
 
 #if defined(CONFIG_PMAC_PBOOK) && defined(CONFIG_PM)
@@ -349,6 +349,7 @@
 	if (pmac_call_feature(PMAC_FTR_SLEEP_STATE,NULL,0,-1) >= 0)
 		can_sleep = 1;
 #endif /* CONFIG_PMAC_PBOOK */
+
 	via = (volatile unsigned char *) ioremap(vias->addrs->address, 0x2000);
 
 	out_8(&via[IER], IER_CLR | 0x7f);	/* disable all intrs */
@@ -510,8 +511,8 @@
 
 	/* ack all pending interrupts */
 	timeout = 100000;
-	interrupt_data[0] = 1;
-	while (interrupt_data[0] || pmu_state != idle) {
+	interrupt_data[0][0] = 1;
+	while (interrupt_data[0][0] || pmu_state != idle) {
 		if (--timeout < 0) {
 			printk(KERN_ERR "init_pmu: timed out acking intrs\n");
 			return 0;
@@ -545,6 +546,16 @@
 	return pmu_kind;
 }
 
+static inline void wakeup_decrementer(void)
+{
+	set_dec(tb_ticks_per_jiffy);
+	/* No currently-supported powerbook has a 601,
+	 * so use get_tbl, not native
+	 */
+	last_jiffy_stamp(0) = tb_last_stamp = get_tbl();
+}
+
+
 #ifdef CONFIG_PMAC_PBOOK
 
 /* 
@@ -952,9 +963,10 @@
 	while(*val == ' ')
 		val++;
 #ifdef CONFIG_PMAC_PBOOK
-	if (pmu_kind == PMU_KEYLARGO_BASED && can_sleep)
+	if (pmu_kind == PMU_KEYLARGO_BASED && can_sleep) {
 		if (!strcmp(label, "lid_wakeup"))
 			option_lid_wakeup = ((*val) == '1');
+	}
 #endif /* CONFIG_PMAC_PBOOK */
 	return fcount;
 }
@@ -1165,7 +1177,7 @@
 	return 0;
 }
 
-static void __openfirmware
+static inline void
 wait_for_ack(void)
 {
 	/* Sightly increased the delay, I had one occurence of the message
@@ -1183,7 +1195,7 @@
 
 /* New PMU seems to be very sensitive to those timings, so we make sure
  * PCI is flushed immediately */
-static void __openfirmware
+static inline void
 send_byte(int x)
 {
 	volatile unsigned char *v = via;
@@ -1194,8 +1206,8 @@
 	(void)in_8(&v[B]);
 }
 
-static void __openfirmware
-recv_byte()
+static inline void
+recv_byte(void)
 {
 	volatile unsigned char *v = via;
 
@@ -1205,7 +1217,13 @@
 	(void)in_8(&v[B]);
 }
 
-static volatile int disable_poll;
+static inline void
+pmu_done(struct adb_request *req)
+{
+	req->complete = 1;
+	if (req->done)
+		(*req->done)(req);
+}
 
 static void __openfirmware
 pmu_start()
@@ -1270,9 +1288,9 @@
 	}
 
 	do {
-		spin_unlock(&pmu_lock);
+		spin_unlock_irqrestore(&pmu_lock, flags);
 		via_pmu_interrupt(0, 0, 0);
-		spin_lock(&pmu_lock);
+		spin_lock_irqsave(&pmu_lock, flags);
 		if (!adb_int_pending && pmu_state == idle && !req_awaiting_reply) {
 #ifdef SUSPEND_USES_PMU
 			pmu_request(&req, NULL, 2, PMU_SET_INTR_MASK, 0);
@@ -1319,61 +1337,83 @@
 #endif /* SUSPEND_USES_PMU */
 }
 
+/* Interrupt data could be the result data from an ADB cmd */
 static void __openfirmware
-via_pmu_interrupt(int irq, void *arg, struct pt_regs *regs)
+pmu_handle_data(unsigned char *data, int len, struct pt_regs *regs)
 {
-	unsigned long flags;
-	int intr;
-	int nloop = 0;
-
-	/* This is a bit brutal, we can probably do better */
-	spin_lock_irqsave(&pmu_lock, flags);
-	++disable_poll;
-		
-	for (;;) {
-		intr = in_8(&via[IFR]) & (SR_INT | CB1_INT);
-		if (intr == 0)
-			break;
-		if (++nloop > 1000) {
-			printk(KERN_DEBUG "PMU: stuck in intr loop, "
-			       "intr=%x, ier=%x pmu_state=%d\n",
-			       intr, in_8(&via[IER]), pmu_state);
-			break;
-		}
-		out_8(&via[IFR], intr);
-		if (intr & SR_INT)
-			pmu_sr_intr(regs);
-		if (intr & CB1_INT)
-			adb_int_pending = 1;
+	asleep = 0;
+	if (drop_interrupts || len < 1) {
+		adb_int_pending = 0;
+		return;
 	}
-
-	if (pmu_state == idle) {
-		if (adb_int_pending) {
-			pmu_state = intack;
-			/* Sounds safer to make sure ACK is high before writing.
-			 * This helped kill a problem with ADB and some iBooks
+	/* Note: for some reason, we get an interrupt with len=1,
+	 * data[0]==0 after each normal ADB interrupt, at least
+	 * on the Pismo. Still investigating...  --BenH
+	 */
+	if (data[0] & PMU_INT_ADB) {
+		if ((data[0] & PMU_INT_ADB_AUTO) == 0) {
+			struct adb_request *req = req_awaiting_reply;
+			if (req == 0) {
+				printk(KERN_ERR "PMU: extra ADB reply\n");
+				return;
+			}
+			req_awaiting_reply = 0;
+			if (len <= 2)
+				req->reply_len = 0;
+			else {
+				memcpy(req->reply, data + 1, len - 1);
+				req->reply_len = len - 1;
+			}
+			pmu_done(req);
+		} else {
+#ifdef CONFIG_XMON
+			if (len == 4 && data[1] == 0x2c) {
+				extern int xmon_wants_key, xmon_adb_keycode;
+				if (xmon_wants_key) {
+					xmon_adb_keycode = data[2];
+					return;
+				}
+			}
+#endif /* CONFIG_XMON */
+#ifdef CONFIG_ADB
+			/*
+			 * XXX On the [23]400 the PMU gives us an up
+			 * event for keycodes 0x74 or 0x75 when the PC
+			 * card eject buttons are released, so we
+			 * ignore those events.
 			 */
-			wait_for_ack();
-			send_byte(PMU_INT_ACK);
-			adb_int_pending = 0;
-		} else if (current_req)
-			pmu_start();
-	}
-	
-	--disable_poll;
-	spin_unlock_irqrestore(&pmu_lock, flags);
-}
-
-static void __openfirmware
-gpio1_interrupt(int irq, void *arg, struct pt_regs *regs)
-{
-	if ((in_8(gpio_reg + 0x9) & 0x02) == 0) {
-		adb_int_pending = 1;
-		via_pmu_interrupt(0, 0, 0);
+			if (!(pmu_kind == PMU_OHARE_BASED && len == 4
+			      && data[1] == 0x2c && data[3] == 0xff
+			      && (data[2] & ~1) == 0xf4))
+				adb_input(data+1, len-1, regs, 1);
+#endif /* CONFIG_ADB */		
+		}
+	} else {
+		/* Sound/brightness button pressed */
+		if ((data[0] & PMU_INT_SNDBRT) && len == 3) {
+#ifdef CONFIG_PMAC_BACKLIGHT
+#ifdef CONFIG_INPUT_ADBHID
+			if (!disable_kernel_backlight)
+#endif /* CONFIG_INPUT_ADBHID */
+				set_backlight_level(data[1] >> 4);
+#endif /* CONFIG_PMAC_BACKLIGHT */
+		}
+#ifdef CONFIG_PMAC_PBOOK
+		/* Environement or tick interrupt, query batteries */
+		if (pmu_battery_count && (data[0] & PMU_INT_TICK)) {
+			if ((--query_batt_timer) == 0) {
+				query_battery_state();
+				query_batt_timer = BATTERY_POLLING_COUNT;
+			}
+		} else if (pmu_battery_count && (data[0] & PMU_INT_ENVIRONMENT))
+			query_battery_state();
+ 		if (data[0])
+			pmu_pass_intr(data, len);
+#endif /* CONFIG_PMAC_PBOOK */
 	}
 }
 
-static void __openfirmware
+static struct adb_request* __openfirmware
 pmu_sr_intr(struct pt_regs *regs)
 {
 	struct adb_request *req;
@@ -1382,7 +1422,7 @@
 	if (via[B] & TREQ) {
 		printk(KERN_ERR "PMU: spurious SR intr (%x)\n", via[B]);
 		out_8(&via[IFR], SR_INT);
-		return;
+		return NULL;
 	}
 	/* The ack may not yet be low when we get the interrupt */
 	while ((in_8(&via[B]) & TACK) != 0)
@@ -1415,11 +1455,8 @@
 			current_req = req->next;
 			if (req->reply_expected)
 				req_awaiting_reply = req;
-			else {
-				spin_unlock(&pmu_lock);
-				pmu_done(req);
-				spin_lock(&pmu_lock);
-			}
+			else
+				return req;
 		} else {
 			pmu_state = reading;
 			data_index = 0;
@@ -1432,7 +1469,7 @@
 		data_index = 0;
 		data_len = -1;
 		pmu_state = reading_intr;
-		reply_ptr = interrupt_data;
+		reply_ptr = interrupt_data[int_data_last];
 		recv_byte();
 		break;
 
@@ -1451,108 +1488,113 @@
 		}
 
 		if (pmu_state == reading_intr) {
-			spin_unlock(&pmu_lock);
-			pmu_handle_data(interrupt_data, data_index, regs);
-			spin_lock(&pmu_lock);
+			pmu_state = idle;
+			int_data_state[int_data_last] = int_data_ready;
+			interrupt_data_len[int_data_last] = data_len;
 		} else {
 			req = current_req;
 			current_req = req->next;
 			req->reply_len += data_index;
-			spin_unlock(&pmu_lock);
-			pmu_done(req);
-			spin_lock(&pmu_lock);
+			pmu_state = idle;
+			return req;
 		}
-		pmu_state = idle;
-
 		break;
 
 	default:
 		printk(KERN_ERR "via_pmu_interrupt: unknown state %d?\n",
 		       pmu_state);
 	}
+	return NULL;
 }
 
 static void __openfirmware
-pmu_done(struct adb_request *req)
+via_pmu_interrupt(int irq, void *arg, struct pt_regs *regs)
 {
-	req->complete = 1;
-	if (req->done)
-		(*req->done)(req);
+	unsigned long flags;
+	int intr;
+	int nloop = 0;
+	int int_data = -1;
+	struct adb_request *req = NULL;
+	
+	/* This is a bit brutal, we can probably do better */
+	spin_lock_irqsave(&pmu_lock, flags);
+	++disable_poll;
+	
+	for (;;) {
+		intr = in_8(&via[IFR]) & (SR_INT | CB1_INT);
+		if (intr == 0)
+			break;
+		if (++nloop > 1000) {
+			printk(KERN_DEBUG "PMU: stuck in intr loop, "
+			       "intr=%x, ier=%x pmu_state=%d\n",
+			       intr, in_8(&via[IER]), pmu_state);
+			break;
+		}
+		out_8(&via[IFR], intr);
+		if (intr & CB1_INT)
+			adb_int_pending = 1;
+		if (intr & SR_INT) {
+			req = pmu_sr_intr(regs);
+			if (req)
+				break;
+		}
+	}
+
+recheck:
+	if (pmu_state == idle) {
+		if (adb_int_pending) {
+			if (int_data_state[0] == int_data_empty)
+				int_data_last = 0;
+			else if (int_data_state[1] == int_data_empty)
+				int_data_last = 1;
+			else
+				goto no_free_slot;
+			pmu_state = intack;
+			int_data_state[int_data_last] = int_data_fill;
+			/* Sounds safer to make sure ACK is high before writing.
+			 * This helped kill a problem with ADB and some iBooks
+			 */
+			wait_for_ack();
+			send_byte(PMU_INT_ACK);
+			adb_int_pending = 0;
+no_free_slot:			
+		} else if (current_req)
+			pmu_start();
+	}
+	/* Mark the oldest buffer for flushing */
+	if (int_data_state[!int_data_last] == int_data_ready) {
+		int_data_state[!int_data_last] = int_data_flush;
+		int_data = !int_data_last;
+	} else if (int_data_state[int_data_last] == int_data_ready) {
+		int_data_state[int_data_last] = int_data_flush;
+		int_data = int_data_last;
+	}
+	--disable_poll;
+	spin_unlock_irqrestore(&pmu_lock, flags);
+
+	/* Deal with completed PMU requests outside of the lock */
+	if (req) {
+		pmu_done(req);
+		req = NULL;
+	}
+		
+	/* Deal with interrupt datas outside of the lock */
+	if (int_data >= 0) {
+		pmu_handle_data(interrupt_data[int_data], interrupt_data_len[int_data], regs);
+		spin_lock_irqsave(&pmu_lock, flags);
+		++disable_poll;
+		int_data_state[int_data] = int_data_empty;
+		int_data = -1;
+		goto recheck;
+	}
 }
 
-/* Interrupt data could be the result data from an ADB cmd */
 static void __openfirmware
-pmu_handle_data(unsigned char *data, int len, struct pt_regs *regs)
+gpio1_interrupt(int irq, void *arg, struct pt_regs *regs)
 {
-	asleep = 0;
-	if (drop_interrupts || len < 1) {
-		adb_int_pending = 0;
-		return;
-	}
-	/* Note: for some reason, we get an interrupt with len=1,
-	 * data[0]==0 after each normal ADB interrupt, at least
-	 * on the Pismo. Still investigating...  --BenH
-	 */
-	if (data[0] & PMU_INT_ADB) {
-		if ((data[0] & PMU_INT_ADB_AUTO) == 0) {
-			struct adb_request *req = req_awaiting_reply;
-			if (req == 0) {
-				printk(KERN_ERR "PMU: extra ADB reply\n");
-				return;
-			}
-			req_awaiting_reply = 0;
-			if (len <= 2)
-				req->reply_len = 0;
-			else {
-				memcpy(req->reply, data + 1, len - 1);
-				req->reply_len = len - 1;
-			}
-			pmu_done(req);
-		} else {
-#ifdef CONFIG_XMON
-			if (len == 4 && data[1] == 0x2c) {
-				extern int xmon_wants_key, xmon_adb_keycode;
-				if (xmon_wants_key) {
-					xmon_adb_keycode = data[2];
-					return;
-				}
-			}
-#endif /* CONFIG_XMON */
-#ifdef CONFIG_ADB
-			/*
-			 * XXX On the [23]400 the PMU gives us an up
-			 * event for keycodes 0x74 or 0x75 when the PC
-			 * card eject buttons are released, so we
-			 * ignore those events.
-			 */
-			if (!(pmu_kind == PMU_OHARE_BASED && len == 4
-			      && data[1] == 0x2c && data[3] == 0xff
-			      && (data[2] & ~1) == 0xf4))
-				adb_input(data+1, len-1, regs, 1);
-#endif /* CONFIG_ADB */		
-		}
-	} else {
-		/* Sound/brightness button pressed */
-		if ((data[0] & PMU_INT_SNDBRT) && len == 3) {
-#ifdef CONFIG_PMAC_BACKLIGHT
-#ifdef CONFIG_INPUT_ADBHID
-			if (!disable_kernel_backlight)
-#endif /* CONFIG_INPUT_ADBHID */
-				set_backlight_level(data[1] >> 4);
-#endif /* CONFIG_PMAC_BACKLIGHT */
-		}
-#ifdef CONFIG_PMAC_PBOOK
-		/* Environement or tick interrupt, query batteries */
-		if (pmu_battery_count && (data[0] & PMU_INT_TICK)) {
-			if ((--query_batt_timer) == 0) {
-				query_battery_state();
-				query_batt_timer = BATTERY_POLLING_COUNT;
-			}
-		} else if (pmu_battery_count && (data[0] & PMU_INT_ENVIRONMENT))
-			query_battery_state();
- 		if (data[0])
-			pmu_pass_intr(data, len);
-#endif /* CONFIG_PMAC_PBOOK */
+	if ((in_8(gpio_reg + 0x9) & 0x02) == 0) {
+		adb_int_pending = 1;
+		via_pmu_interrupt(0, 0, 0);
 	}
 }
 
@@ -1624,7 +1666,7 @@
 {
 	struct adb_request req;
 
-	cli();
+	local_irq_disable();
 
 	drop_interrupts = 1;
 	
@@ -1647,7 +1689,7 @@
 {
 	struct adb_request req;
 
-	cli();
+	local_irq_disable();
 
 	drop_interrupts = 1;
 
@@ -1672,6 +1714,43 @@
 	return via != 0;
 }
 
+#if defined(DEBUG_SLEEP) || defined(DEBUG_FREQ)
+/* N.B. This doesn't work on the 3400 */
+void
+pmu_blink(int n)
+{
+	struct adb_request req;
+
+	memset(&req, 0, sizeof(req));
+
+	for (; n > 0; --n) {
+		req.nbytes = 4;
+		req.done = NULL;
+		req.data[0] = 0xee;
+		req.data[1] = 4;
+		req.data[2] = 0;
+		req.data[3] = 1;
+		req.reply[0] = ADB_RET_OK;
+		req.reply_len = 1;
+		req.reply_expected = 0;
+		pmu_polled_request(&req);
+		mdelay(50);
+		req.nbytes = 4;
+		req.done = NULL;
+		req.data[0] = 0xee;
+		req.data[1] = 4;
+		req.data[2] = 0;
+		req.data[3] = 0;
+		req.reply[0] = ADB_RET_OK;
+		req.reply_len = 1;
+		req.reply_expected = 0;
+		pmu_polled_request(&req);
+		mdelay(50);
+	}
+	mdelay(50);
+}
+#endif /* defined(DEBUG_SLEEP) || defined(DEBUG_FREQ) */
+
 #ifdef CONFIG_PMAC_PBOOK
 
 static LIST_HEAD(sleep_notifiers);
@@ -1887,43 +1966,6 @@
 	}
 }
 
-#ifdef DEBUG_SLEEP
-/* N.B. This doesn't work on the 3400 */
-void
-pmu_blink(int n)
-{
-	struct adb_request req;
-
-	memset(&req, 0, sizeof(req));
-
-	for (; n > 0; --n) {
-		req.nbytes = 4;
-		req.done = NULL;
-		req.data[0] = 0xee;
-		req.data[1] = 4;
-		req.data[2] = 0;
-		req.data[3] = 1;
-		req.reply[0] = ADB_RET_OK;
-		req.reply_len = 1;
-		req.reply_expected = 0;
-		pmu_polled_request(&req);
-		mdelay(50);
-		req.nbytes = 4;
-		req.done = NULL;
-		req.data[0] = 0xee;
-		req.data[1] = 4;
-		req.data[2] = 0;
-		req.data[3] = 0;
-		req.reply[0] = ADB_RET_OK;
-		req.reply_len = 1;
-		req.reply_expected = 0;
-		pmu_polled_request(&req);
-		mdelay(50);
-	}
-	mdelay(50);
-}
-#endif
-
 /*
  * Put the powerbook to sleep.
  */
@@ -1955,15 +1997,6 @@
 	out_8(&via[IER], IER_SET | SR_INT | CB1_INT);
 }
 
-static inline void wakeup_decrementer(void)
-{
-	set_dec(tb_ticks_per_jiffy);
-	/* No currently-supported powerbook has a 601,
-	 * so use get_tbl, not native
-	 */
-	last_jiffy_stamp(0) = tb_last_stamp = get_tbl();
-}
-
 #define	GRACKLE_PM	(1<<7)
 #define GRACKLE_DOZE	(1<<5)
 #define	GRACKLE_NAP	(1<<4)
@@ -1975,7 +2008,7 @@
 	unsigned long wait;
 	unsigned short pmcr1;
 	struct adb_request req;
-	int ret, timeout;
+	int ret;
 	struct pci_dev *grackle;
 
 	grackle = pci_find_slot(0, 0);
@@ -2036,17 +2069,16 @@
 	mb();
 	asm volatile("mtdec %0" : : "r" (0x7fffffff));
 	
-	/* Giveup the FPU */
-	if (current->thread.regs && (current->thread.regs->msr & MSR_FP) != 0)
-		giveup_fpu(current);
-
 	/* We can now disable MSR_EE */
-	cli();
+	local_irq_disable();
+
+	/* Giveup the FPU */
+	enable_kernel_fp();
 
 	/* For 750, save backside cache setting and disable it */
-	save_l2cr = _get_L2CR();	/* (returns 0 if not 750) */
-	if (save_l2cr)
-		_set_L2CR(0);
+	save_l2cr = _get_L2CR();	/* (returns -1 if not available) */
+	if (save_l2cr != 0xffffffff && (save_l2cr & L2CR_L2E) != 0)
+		_set_L2CR(save_l2cr & 0x7fffffff);
 
 	/* Ask the PMU to put us to sleep */
 	pmu_request(&req, NULL, 5, PMU_SLEEP, 'M', 'A', 'T', 'T');
@@ -2077,7 +2109,7 @@
 	restore_via_state();
 	
 	/* Restore L2 cache */
-	if (save_l2cr)
+	if (save_l2cr != 0xffffffff && (save_l2cr & L2CR_L2E) != 0)
  		_set_L2CR(save_l2cr);
 	
 	/* Restore userland MMU context */
@@ -2096,18 +2128,6 @@
 	while (!req.complete)
 		pmu_poll();
 
-	/* ack all pending interrupts */
-	timeout = 100000;
-	interrupt_data[0] = 1;
-	while (interrupt_data[0] || pmu_state != idle) {
-		if (--timeout < 0)
-			break;
-		if (pmu_state == idle)
-			adb_int_pending = 1;
-		via_pmu_interrupt(0, 0, 0);
-		udelay(10);
-	}
-
 	/* reenable interrupt controller */
 	pmac_sleep_restore_intrs();
 
@@ -2116,7 +2136,13 @@
 
 	/* Restart jiffies & scheduling */
 	wakeup_decrementer();
-	sti();
+
+	/* Force a poll of ADB interrupts */
+	adb_int_pending = 1;
+	via_pmu_interrupt(0, 0, 0);
+
+	/* Re-enable local CPU interrupts */
+	local_irq_enable();
 
 	/* Notify drivers */
 	broadcast_wake();
@@ -2127,9 +2153,10 @@
 int __openfirmware powerbook_sleep_Core99(void)
 {
 	unsigned long save_l2cr;
+	unsigned long save_l3cr;
 	unsigned long wait;
 	struct adb_request req;
-	int ret, timeout;
+	int ret;
 	
 	if (!can_sleep) {
 		printk(KERN_ERR "Sleep mode not supported on this machine\n");
@@ -2192,6 +2219,9 @@
 	mb();
 	asm volatile("mtdec %0" : : "r" (0x7fffffff));
 
+	/* We can now disable MSR_EE */
+	local_irq_disable();
+
 	/* Giveup the FPU & vec */
 	enable_kernel_fp();
 
@@ -2200,12 +2230,12 @@
 		enable_kernel_altivec();
 #endif /* CONFIG_ALTIVEC */
 
-	/* We can now disable MSR_EE */
-	cli();
-
-	/* For 750, save backside cache setting and disable it */
-	save_l2cr = _get_L2CR();	/* (returns 0 if not 750) */
-	if (save_l2cr)
+	/* Save & disable L2 and L3 caches*/
+	save_l3cr = _get_L3CR();	/* (returns -1 if not available) */
+	save_l2cr = _get_L2CR();	/* (returns -1 if not available) */
+	if (save_l3cr != 0xffffffff && (save_l3cr & L3CR_L3E) != 0)
+		_set_L3CR(save_l3cr & 0x7fffffff);
+	if (save_l2cr != 0xffffffff && (save_l2cr & L2CR_L2E) != 0)
 		_set_L2CR(save_l2cr & 0x7fffffff);
 
 	/* Save the state of PCI config space for some slots */
@@ -2250,11 +2280,15 @@
 	/* Don't restore PCI for now, it crashes. Maybe unnecessary on pbook */
 	//pbook_pci_restore();
 
+#ifdef DEBUG_SLEEP
 	pmu_blink(2);
-		
+#endif		
 	/* Restore L2 cache */
-	if (save_l2cr)
+	if (save_l2cr != 0xffffffff && (save_l2cr & L2CR_L2E) != 0)
  		_set_L2CR(save_l2cr);
+	/* Restore L3 cache */
+	if (save_l3cr != 0xffffffff && (save_l3cr & L3CR_L3E) != 0)
+ 		_set_L3CR(save_l3cr);
 	
 	/* Restore userland MMU context */
 	set_context(current->active_mm->context, current->active_mm->pgd);
@@ -2267,18 +2301,6 @@
 	while (!req.complete)
 		pmu_poll();
 		
-	/* ack all pending interrupts */
-	timeout = 100000;
-	interrupt_data[0] = 1;
-	while (interrupt_data[0] || pmu_state != idle) {
-		if (--timeout < 0)
-			break;
-		if (pmu_state == idle)
-			adb_int_pending = 1;
-		via_pmu_interrupt(0, 0, 0);
-		udelay(10);
-	}
-
 	/* reenable interrupt controller */
 	openpic_sleep_restore_intrs();
 
@@ -2287,7 +2309,13 @@
 
 	/* Restart jiffies & scheduling */
 	wakeup_decrementer();
-	sti();
+
+	/* Force a poll of ADB interrupts */
+	adb_int_pending = 1;
+	via_pmu_interrupt(0, 0, 0);
+
+	/* Re-enable local CPU interrupts */
+	local_irq_enable();
 
 	/* Notify drivers */
 	broadcast_wake();
@@ -2411,7 +2439,9 @@
 
 	/* Restart jiffies & scheduling */
 	wakeup_decrementer();
-	sti();
+
+	/* Re-enable local CPU interrupts */
+	local_irq_enable();
 
 	/* Notify drivers */
 	broadcast_wake();
@@ -2686,8 +2716,9 @@
 }
 #endif /* CONFIG_PMAC_PBOOK */
 
-#ifdef DEBUG_SLEEP
-static inline void polled_handshake(volatile unsigned char *via)
+#if defined(DEBUG_SLEEP) || defined(DEBUG_FREQ)
+static inline void __pmac
+polled_handshake(volatile unsigned char *via)
 {
 	via[B] &= ~TREQ; eieio();
 	while ((via[B] & TACK) != 0)
@@ -2697,14 +2728,16 @@
 		;
 }
 
-static inline void polled_send_byte(volatile unsigned char *via, int x)
+static inline void __pmac
+polled_send_byte(volatile unsigned char *via, int x)
 {
 	via[ACR] |= SR_OUT | SR_EXT; eieio();
 	via[SR] = x; eieio();
 	polled_handshake(via);
 }
 
-static inline int polled_recv_byte(volatile unsigned char *via)
+static inline int __pmac
+polled_recv_byte(volatile unsigned char *via)
 {
 	int x;
 
@@ -2715,7 +2748,7 @@
 	return x;
 }
 
-int
+int __pmac
 pmu_polled_request(struct adb_request *req)
 {
 	unsigned long flags;
@@ -2728,7 +2761,7 @@
 	if (l >= 0 && req->nbytes != l + 1)
 		return -EINVAL;
 
-	save_flags(flags); cli();
+	local_irq_save(flags);
 	while (pmu_state != idle)
 		pmu_poll();
 
@@ -2751,10 +2784,11 @@
 	if (req->done)
 		(*req->done)(req);
 
-	restore_flags(flags);
+	local_irq_restore(flags);
 	return 0;
 }
-#endif /* DEBUG_SLEEP */
+#endif /* defined(DEBUG_SLEEP) || defined(DEBUG_FREQ) */
+
 
 EXPORT_SYMBOL(pmu_request);
 EXPORT_SYMBOL(pmu_poll);