x86, usb, pci: Disable usb legacy support early

Index: linux-2.6/drivers/usb/host/pci-quirks.c
===================================================================
@@ -15,122 +15,28 @@
 #include <linux/delay.h>
 #include <linux/acpi.h>
 #include "pci-quirks.h"
-#include "xhci-ext-caps.h"
 
+static void usb_handoff_udelay(unsigned long usecs)
+{
+	udelay(usecs);
+}
+
+static void usb_handoff_msleep(unsigned long msecs)
+{
+	msleep(msecs);
+}
+
+#include "usb_handoff.c"
 
-#define UHCI_USBLEGSUP		0xc0		/* legacy support */
-#define UHCI_USBCMD		0		/* command register */
-#define UHCI_USBINTR		4		/* interrupt register */
-#define UHCI_USBLEGSUP_RWC	0x8f00		/* the R/WC bits */
-#define UHCI_USBLEGSUP_RO	0x5040		/* R/O and reserved bits */
-#define UHCI_USBCMD_RUN		0x0001		/* RUN/STOP bit */
-#define UHCI_USBCMD_HCRESET	0x0002		/* Host Controller reset */
-#define UHCI_USBCMD_EGSM	0x0008		/* Global Suspend Mode */
-#define UHCI_USBCMD_CONFIGURE	0x0040		/* Config Flag */
-#define UHCI_USBINTR_RESUME	0x0002		/* Resume interrupt enable */
-
-#define OHCI_CONTROL		0x04
-#define OHCI_CMDSTATUS		0x08
-#define OHCI_INTRSTATUS		0x0c
-#define OHCI_INTRENABLE		0x10
-#define OHCI_INTRDISABLE	0x14
-#define OHCI_OCR		(1 << 3)	/* ownership change request */
-#define OHCI_CTRL_RWC		(1 << 9)	/* remote wakeup connected */
-#define OHCI_CTRL_IR		(1 << 8)	/* interrupt routing */
-#define OHCI_INTR_OC		(1 << 30)	/* ownership change */
-
-#define EHCI_HCC_PARAMS		0x08		/* extended capabilities */
-#define EHCI_USBCMD		0		/* command register */
-#define EHCI_USBCMD_RUN		(1 << 0)	/* RUN/STOP bit */
-#define EHCI_USBSTS		4		/* status register */
-#define EHCI_USBSTS_HALTED	(1 << 12)	/* HCHalted bit */
-#define EHCI_USBINTR		8		/* interrupt register */
-#define EHCI_CONFIGFLAG		0x40		/* configured flag register */
-#define EHCI_USBLEGSUP		0		/* legacy support register */
-#define EHCI_USBLEGSUP_BIOS	(1 << 16)	/* BIOS semaphore */
-#define EHCI_USBLEGSUP_OS	(1 << 24)	/* OS semaphore */
-#define EHCI_USBLEGCTLSTS	4		/* legacy control/status */
-#define EHCI_USBLEGCTLSTS_SOOE	(1 << 13)	/* SMI on ownership change */
-
-
-/*
- * Make sure the controller is completely inactive, unable to
- * generate interrupts or do DMA.
- */
 void uhci_reset_hc(struct pci_dev *pdev, unsigned long base)
 {
-	/* Turn off PIRQ enable and SMI enable.  (This also turns off the
-	 * BIOS's USB Legacy Support.)  Turn off all the R/WC bits too.
-	 */
-	pci_write_config_word(pdev, UHCI_USBLEGSUP, UHCI_USBLEGSUP_RWC);
-
-	/* Reset the HC - this will force us to get a
-	 * new notification of any already connected
-	 * ports due to the virtual disconnect that it
-	 * implies.
-	 */
-	outw(UHCI_USBCMD_HCRESET, base + UHCI_USBCMD);
-	mb();
-	udelay(5);
-	if (inw(base + UHCI_USBCMD) & UHCI_USBCMD_HCRESET)
-		dev_warn(&pdev->dev, "HCRESET not completed yet!\n");
-
-	/* Just to be safe, disable interrupt requests and
-	 * make sure the controller is stopped.
-	 */
-	outw(0, base + UHCI_USBINTR);
-	outw(0, base + UHCI_USBCMD);
+	__uhci_reset_hc(pdev, base);
 }
 EXPORT_SYMBOL_GPL(uhci_reset_hc);
 
-/*
- * Initialize a controller that was newly discovered or has just been
- * resumed.  In either case we can't be sure of its previous state.
- *
- * Returns: 1 if the controller was reset, 0 otherwise.
- */
 int uhci_check_and_reset_hc(struct pci_dev *pdev, unsigned long base)
 {
-	u16 legsup;
-	unsigned int cmd, intr;
-
-	/*
-	 * When restarting a suspended controller, we expect all the
-	 * settings to be the same as we left them:
-	 *
-	 *	PIRQ and SMI disabled, no R/W bits set in USBLEGSUP;
-	 *	Controller is stopped and configured with EGSM set;
-	 *	No interrupts enabled except possibly Resume Detect.
-	 *
-	 * If any of these conditions are violated we do a complete reset.
-	 */
-	pci_read_config_word(pdev, UHCI_USBLEGSUP, &legsup);
-	if (legsup & ~(UHCI_USBLEGSUP_RO | UHCI_USBLEGSUP_RWC)) {
-		dev_dbg(&pdev->dev, "%s: legsup = 0x%04x\n",
-				__func__, legsup);
-		goto reset_needed;
-	}
-
-	cmd = inw(base + UHCI_USBCMD);
-	if ((cmd & UHCI_USBCMD_RUN) || !(cmd & UHCI_USBCMD_CONFIGURE) ||
-			!(cmd & UHCI_USBCMD_EGSM)) {
-		dev_dbg(&pdev->dev, "%s: cmd = 0x%04x\n",
-				__func__, cmd);
-		goto reset_needed;
-	}
-
-	intr = inw(base + UHCI_USBINTR);
-	if (intr & (~UHCI_USBINTR_RESUME)) {
-		dev_dbg(&pdev->dev, "%s: intr = 0x%04x\n",
-				__func__, intr);
-		goto reset_needed;
-	}
-	return 0;
-
-reset_needed:
-	dev_dbg(&pdev->dev, "Performing full reset\n");
-	uhci_reset_hc(pdev, base);
-	return 1;
+	return	__uhci_check_and_reset_hc(pdev, base);
 }
 EXPORT_SYMBOL_GPL(uhci_check_and_reset_hc);
 

@@ -169,7 +75,6 @@ static int __devinit mmio_resource_enabl
 static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
 {
 	void __iomem *base;
-	u32 control;
 
 	if (!mmio_resource_enabled(pdev, 0))
 		return;

@@ -178,50 +83,14 @@ static void __devinit quirk_usb_handoff_
 	if (base == NULL)
 		return;
 
-	control = readl(base + OHCI_CONTROL);
-
-/* On PA-RISC, PDC can leave IR set incorrectly; ignore it there. */
-#ifdef __hppa__
-#define	OHCI_CTRL_MASK		(OHCI_CTRL_RWC | OHCI_CTRL_IR)
-#else
-#define	OHCI_CTRL_MASK		OHCI_CTRL_RWC
-
-	if (control & OHCI_CTRL_IR) {
-		int wait_time = 500; /* arbitrary; 5 seconds */
-		writel(OHCI_INTR_OC, base + OHCI_INTRENABLE);
-		writel(OHCI_OCR, base + OHCI_CMDSTATUS);
-		while (wait_time > 0 &&
-				readl(base + OHCI_CONTROL) & OHCI_CTRL_IR) {
-			wait_time -= 10;
-			msleep(10);
-		}
-		if (wait_time <= 0)
-			dev_warn(&pdev->dev, "OHCI: BIOS handoff failed"
-					" (BIOS bug?) %08x\n",
-					readl(base + OHCI_CONTROL));
-	}
-#endif
-
-	/* reset controller, preserving RWC (and possibly IR) */
-	writel(control & OHCI_CTRL_MASK, base + OHCI_CONTROL);
-
-	/*
-	 * disable interrupts
-	 */
-	writel(~(u32)0, base + OHCI_INTRDISABLE);
-	writel(~(u32)0, base + OHCI_INTRSTATUS);
+	__usb_handoff_ohci(pdev, base);
 
 	iounmap(base);
 }
 
 static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 {
-	int wait_time, delta;
-	void __iomem *base, *op_reg_base;
-	u32	hcc_params, val;
-	u8	offset, cap_length;
-	int	count = 256/4;
-	int	tried_handoff = 0;
+	void __iomem *base;
 
 	if (!mmio_resource_enabled(pdev, 0))
 		return;

@@ -230,238 +99,23 @@ static void __devinit quirk_usb_disable_
 	if (base == NULL)
 		return;
 
-	cap_length = readb(base);
-	op_reg_base = base + cap_length;
-
-	/* EHCI 0.96 and later may have "extended capabilities"
-	 * spec section 5.1 explains the bios handoff, e.g. for
-	 * booting from USB disk or using a usb keyboard
-	 */
-	hcc_params = readl(base + EHCI_HCC_PARAMS);
-	offset = (hcc_params >> 8) & 0xff;
-	while (offset && --count) {
-		u32		cap;
-		int		msec;
-
-		pci_read_config_dword(pdev, offset, &cap);
-		switch (cap & 0xff) {
-		case 1:			/* BIOS/SMM/... handoff support */
-			if ((cap & EHCI_USBLEGSUP_BIOS)) {
-				dev_dbg(&pdev->dev, "EHCI: BIOS handoff\n");
-
-#if 0
-/* [email protected] reports that some systems need SMI forced on,
- * but that seems dubious in general (the BIOS left it off intentionally)
- * and is known to prevent some systems from booting.  so we won't do this
- * unless maybe we can determine when we're on a system that needs SMI forced.
- */
-				/* BIOS workaround (?): be sure the
-				 * pre-Linux code receives the SMI
-				 */
-				pci_read_config_dword(pdev,
-						offset + EHCI_USBLEGCTLSTS,
-						&val);
-				pci_write_config_dword(pdev,
-						offset + EHCI_USBLEGCTLSTS,
-						val | EHCI_USBLEGCTLSTS_SOOE);
-#endif
-
-				/* some systems get upset if this semaphore is
-				 * set for any other reason than forcing a BIOS
-				 * handoff..
-				 */
-				pci_write_config_byte(pdev, offset + 3, 1);
-			}
-
-			/* if boot firmware now owns EHCI, spin till
-			 * it hands it over.
-			 */
-			msec = 1000;
-			while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
-				tried_handoff = 1;
-				msleep(10);
-				msec -= 10;
-				pci_read_config_dword(pdev, offset, &cap);
-			}
-
-			if (cap & EHCI_USBLEGSUP_BIOS) {
-				/* well, possibly buggy BIOS... try to shut
-				 * it down, and hope nothing goes too wrong
-				 */
-				dev_warn(&pdev->dev, "EHCI: BIOS handoff failed"
-						" (BIOS bug?) %08x\n", cap);
-				pci_write_config_byte(pdev, offset + 2, 0);
-			}
-
-			/* just in case, always disable EHCI SMIs */
-			pci_write_config_dword(pdev,
-					offset + EHCI_USBLEGCTLSTS,
-					0);
-
-			/* If the BIOS ever owned the controller then we
-			 * can't expect any power sessions to remain intact.
-			 */
-			if (tried_handoff)
-				writel(0, op_reg_base + EHCI_CONFIGFLAG);
-			break;
-		case 0:			/* illegal reserved capability */
-			cap = 0;
-			/* FALLTHROUGH */
-		default:
-			dev_warn(&pdev->dev, "EHCI: unrecognized capability "
-					"%02x\n", cap & 0xff);
-			break;
-		}
-		offset = (cap >> 8) & 0xff;
-	}
-	if (!count)
-		dev_printk(KERN_DEBUG, &pdev->dev, "EHCI: capability loop?\n");
-
-	/*
-	 * halt EHCI & disable its interrupts in any case
-	 */
-	val = readl(op_reg_base + EHCI_USBSTS);
-	if ((val & EHCI_USBSTS_HALTED) == 0) {
-		val = readl(op_reg_base + EHCI_USBCMD);
-		val &= ~EHCI_USBCMD_RUN;
-		writel(val, op_reg_base + EHCI_USBCMD);
-
-		wait_time = 2000;
-		delta = 100;
-		do {
-			writel(0x3f, op_reg_base + EHCI_USBSTS);
-			udelay(delta);
-			wait_time -= delta;
-			val = readl(op_reg_base + EHCI_USBSTS);
-			if ((val == ~(u32)0) || (val & EHCI_USBSTS_HALTED)) {
-				break;
-			}
-		} while (wait_time > 0);
-	}
-	writel(0, op_reg_base + EHCI_USBINTR);
-	writel(0x3f, op_reg_base + EHCI_USBSTS);
+	__usb_handoff_ehci(pdev, base);
 
 	iounmap(base);
 }
 
-/*
- * handshake - spin reading a register until handshake completes
- * @ptr: address of hc register to be read
- * @mask: bits to look at in result of read
- * @done: value of those bits when handshake succeeds
- * @wait_usec: timeout in microseconds
- * @delay_usec: delay in microseconds to wait between polling
- *
- * Polls a register every delay_usec microseconds.
- * Returns 0 when the mask bits have the value done.
- * Returns -ETIMEDOUT if this condition is not true after
- * wait_usec microseconds have passed.
- */
-static int handshake(void __iomem *ptr, u32 mask, u32 done,
-		int wait_usec, int delay_usec)
-{
-	u32	result;
-
-	do {
-		result = readl(ptr);
-		result &= mask;
-		if (result == done)
-			return 0;
-		udelay(delay_usec);
-		wait_usec -= delay_usec;
-	} while (wait_usec > 0);
-	return -ETIMEDOUT;
-}
-
-/**
- * PCI Quirks for xHCI.
- *
- * Takes care of the handoff between the Pre-OS (i.e. BIOS) and the OS.
- * It signals to the BIOS that the OS wants control of the host controller,
- * and then waits 5 seconds for the BIOS to hand over control.
- * If we timeout, assume the BIOS is broken and take control anyway.
- */
 static void __devinit quirk_usb_handoff_xhci(struct pci_dev *pdev)
 {
 	void __iomem *base;
-	int ext_cap_offset;
-	void __iomem *op_reg_base;
-	u32 val;
-	int timeout;
 
 	if (!mmio_resource_enabled(pdev, 0))
 		return;
 
-	base = ioremap_nocache(pci_resource_start(pdev, 0),
-				pci_resource_len(pdev, 0));
+	base = pci_ioremap_bar(pdev, 0);
 	if (base == NULL)
 		return;
 
-	/*
-	 * Find the Legacy Support Capability register -
-	 * this is optional for xHCI host controllers.
-	 */
-	ext_cap_offset = xhci_find_next_cap_offset(base, XHCI_HCC_PARAMS_OFFSET);
-	do {
-		if (!ext_cap_offset)
-			/* We've reached the end of the extended capabilities */
-			goto hc_init;
-		val = readl(base + ext_cap_offset);
-		if (XHCI_EXT_CAPS_ID(val) == XHCI_EXT_CAPS_LEGACY)
-			break;
-		ext_cap_offset = xhci_find_next_cap_offset(base, ext_cap_offset);
-	} while (1);
-
-	/* If the BIOS owns the HC, signal that the OS wants it, and wait */
-	if (val & XHCI_HC_BIOS_OWNED) {
-		writel(val & XHCI_HC_OS_OWNED, base + ext_cap_offset);
-
-		/* Wait for 5 seconds with 10 microsecond polling interval */
-		timeout = handshake(base + ext_cap_offset, XHCI_HC_BIOS_OWNED,
-				0, 5000, 10);
-
-		/* Assume a buggy BIOS and take HC ownership anyway */
-		if (timeout) {
-			dev_warn(&pdev->dev, "xHCI BIOS handoff failed"
-					" (BIOS bug ?) %08x\n", val);
-			writel(val & ~XHCI_HC_BIOS_OWNED, base + ext_cap_offset);
-		}
-	}
-
-	/* Disable any BIOS SMIs */
-	writel(XHCI_LEGACY_DISABLE_SMI,
-			base + ext_cap_offset + XHCI_LEGACY_CONTROL_OFFSET);
-
-hc_init:
-	op_reg_base = base + XHCI_HC_LENGTH(readl(base));
-
-	/* Wait for the host controller to be ready before writing any
-	 * operational or runtime registers.  Wait 5 seconds and no more.
-	 */
-	timeout = handshake(op_reg_base + XHCI_STS_OFFSET, XHCI_STS_CNR, 0,
-			5000, 10);
-	/* Assume a buggy HC and start HC initialization anyway */
-	if (timeout) {
-		val = readl(op_reg_base + XHCI_STS_OFFSET);
-		dev_warn(&pdev->dev,
-				"xHCI HW not ready after 5 sec (HC bug?) "
-				"status = 0x%x\n", val);
-	}
-
-	/* Send the halt and disable interrupts command */
-	val = readl(op_reg_base + XHCI_CMD_OFFSET);
-	val &= ~(XHCI_CMD_RUN | XHCI_IRQS);
-	writel(val, op_reg_base + XHCI_CMD_OFFSET);
-
-	/* Wait for the HC to halt - poll every 125 usec (one microframe). */
-	timeout = handshake(op_reg_base + XHCI_STS_OFFSET, XHCI_STS_HALT, 1,
-			XHCI_MAX_HALT_USEC, 125);
-	if (timeout) {
-		val = readl(op_reg_base + XHCI_STS_OFFSET);
-		dev_warn(&pdev->dev,
-				"xHCI HW did not halt within %d usec "
-				"status = 0x%x\n", XHCI_MAX_HALT_USEC, val);
-	}
+	__usb_handoff_xhci(pdev, base);
 
 	iounmap(base);
 }

402 lines not on this page · raw message

Index: linux-2.6/arch/x86/include/asm/pci-direct.h
===================================================================
@@ -18,4 +18,6 @@ extern int early_pci_allowed(void);
 extern unsigned int pci_early_dump_regs;
 extern void early_dump_pci_device(u8 bus, u8 slot, u8 func);
 extern void early_dump_pci_devices(void);
+
+struct pci_dev *get_early_pci_dev(int num, int slot, int func);
 #endif /* _ASM_X86_PCI_DIRECT_H */

78 lines not on this page · raw message

Index: linux-2.6/arch/x86/kernel/early-quirks.c
===================================================================
@@ -19,6 +19,174 @@
 #include <asm/iommu.h>
 #include <asm/gart.h>
 
+static inline void early_udelay2(void)
+{
+#ifndef CONFIG_PARAVIRT
+	native_io_delay();
+#else
+	pv_cpu_ops.io_delay();
+#endif
+}
+
+static void usb_handoff_udelay(unsigned long usecs)
+{
+	unsigned long count;
+
+	count = usecs >> 1;
+
+	if (!count)
+		count = 1;
+
+	while (count-- > 0)
+		early_udelay2();
+}
+
+static void usb_handoff_msleep(unsigned long msecs)
+{
+	while (msecs-- > 0)
+		usb_handoff_udelay(1000);
+}
+
+#include "../../../drivers/usb/host/usb_handoff.c"
+
+static inline int io_type_enabled(int num, int slot, int func, unsigned int mask)
+{
+	return read_pci_config_16(num, slot, func, PCI_COMMAND) & mask;
+}
+
+#define pio_enabled(num, slot, func) io_type_enabled(num, slot, func, PCI_COMMAND_IO)
+#define mmio_enabled(num, slot, func) io_type_enabled(num, slot, func, PCI_COMMAND_MEMORY)
+
+static __init u32 get_usb_io_port(int num, int slot, int func)
+{
+	int i;
+
+	if (!pio_enabled(num, slot, func))
+		return 0;
+
+	for (i = 0; i < PCI_ROM_RESOURCE; i++) {
+		u32 addr = read_pci_config(num, slot, func, 0x10+(i<<2));
+
+		if (!addr)
+			continue;
+		if ((addr & PCI_BASE_ADDRESS_SPACE) != PCI_BASE_ADDRESS_SPACE_IO)
+			continue;
+
+		addr &= PCI_BASE_ADDRESS_IO_MASK;
+		if (addr)
+			return addr;
+	}
+
+	return 0;
+}
+
+static void __init quirk_usb_handoff_uhci(int num, int slot, int func)
+{
+	unsigned long base;
+
+	base = get_usb_io_port(num, slot, func);
+	if (!base)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: uhci ioport = 0x%04lx\n",
+					num, slot, func, base);
+	__uhci_check_and_reset_hc(get_early_pci_dev(num, slot, func), base);
+}
+
+static __init u32 get_usb_mmio_addr(int num, int slot, int func)
+{
+	u32 addr;
+
+	if (!mmio_enabled(num, slot, func))
+		return 0;
+
+	addr = read_pci_config(num, slot, func, 0x10);
+	if (!addr)
+		return 0;
+	if ((addr & PCI_BASE_ADDRESS_SPACE) != PCI_BASE_ADDRESS_SPACE_MEMORY)
+		return 0;
+
+	addr &= PCI_BASE_ADDRESS_MEM_MASK;
+
+	return addr;
+}
+
+static __init void quirk_usb_handoff_ohci(int num, int slot, int func)
+{
+	void __iomem *base;
+	u32 addr;
+
+	addr = get_usb_mmio_addr(num, slot, func);
+	if (!addr)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: ohci mmio = 0x%08x\n", num, slot, func, addr);
+	base = early_ioremap(addr, 0x1000);
+	if (!base)
+		return;
+
+	__usb_handoff_ohci(get_early_pci_dev(num, slot, func), base);
+
+	early_iounmap(base, 0x1000);
+}
+
+static __init void quirk_usb_handoff_ehci(int num, int slot, int func)
+{
+	void __iomem *base;
+	u32 addr;
+
+	addr = get_usb_mmio_addr(num, slot, func);
+	if (!addr)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: ehci mmio = 0x%08x\n",
+				 num, slot, func, addr);
+	base = early_ioremap(addr, 0x1000);
+	if (!base)
+		return;
+
+	__usb_handoff_ehci(get_early_pci_dev(num, slot, func), base);
+
+	early_iounmap(base, 0x1000);
+}
+
+static __init void quirk_usb_handoff_xhci(int num, int slot, int func)
+{
+	void __iomem *base;
+	u32 addr;
+
+	addr = get_usb_mmio_addr(num, slot, func);
+	if (!addr)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: xhci mmio = 0x%08x\n",
+				num, slot, func, addr);
+	base = early_ioremap(addr, 0x1000);
+	if (!base)
+		return;
+
+	__usb_handoff_xhci(get_early_pci_dev(num, slot, func), base);
+
+	early_iounmap(base, 0x1000);
+}
+
+static __init void quirk_usb_handoff(int num, int slot, int func)
+{
+	u32 class;
+
+	class = read_pci_config(num, slot, func, PCI_CLASS_REVISION);
+	class >>= 8;
+
+	if (class == PCI_CLASS_SERIAL_USB_UHCI)
+		quirk_usb_handoff_uhci(num, slot, func);
+	else if (class == PCI_CLASS_SERIAL_USB_OHCI)
+		quirk_usb_handoff_ohci(num, slot, func);
+	else if (class == PCI_CLASS_SERIAL_USB_EHCI)
+		quirk_usb_handoff_ehci(num, slot, func);
+	else if (class == PCI_CLASS_SERIAL_USB_XHCI)
+		quirk_usb_handoff_xhci(num, slot, func);
+}
+
 static void __init fix_hypertransport_config(int num, int slot, int func)
 {
 	u32 htcfg;

@@ -208,6 +376,8 @@ struct chipset {
  * only matching on bus 0.
  */
 static struct chipset early_qrk[] __initdata = {
+	{ PCI_ANY_ID, PCI_ANY_ID,
+	  PCI_CLASS_SERIAL_USB, PCI_ANY_ID, 0, quirk_usb_handoff },
 	{ PCI_VENDOR_ID_NVIDIA, PCI_ANY_ID,
 	  PCI_CLASS_BRIDGE_PCI, PCI_ANY_ID, QFLAG_APPLY_ONCE, nvidia_bugs },
 	{ PCI_VENDOR_ID_VIA, PCI_ANY_ID,

Index: linux-2.6/arch/x86/kernel/early-quirks.c
===================================================================
@@ -243,7 +243,7 @@ static int __init check_dev_quirk(int nu
 	class = read_pci_config_16(num, slot, func, PCI_CLASS_DEVICE);
 
 	if (class == 0xffff)
-		return -1; /* no class, treat as single function */
+		return 0; /* no class, func gap */
 
 	vendor = read_pci_config_16(num, slot, func, PCI_VENDOR_ID);
 

@@ -263,27 +263,30 @@ static int __init check_dev_quirk(int nu
 			}
 	}
 
-	type = read_pci_config_byte(num, slot, func,
-				    PCI_HEADER_TYPE);
-	if (!(type & 0x80))
-		return -1;
+	if (func == 0) {
+		type = read_pci_config_byte(num, slot, func,
+					    PCI_HEADER_TYPE);
+		if (!(type & 0x80))
+			return -1;
+	}
 
 	return 0;
 }
 
 void __init early_quirks(void)
 {
-	int slot, func;
+	int num, slot, func;
 
 	if (!early_pci_allowed())
 		return;
 
 	/* Poor man's PCI discovery */
-	/* Only scan the root bus */
-	for (slot = 0; slot < 32; slot++)
-		for (func = 0; func < 8; func++) {
-			/* Only probe function 0 on single fn devices */
-			if (check_dev_quirk(0, slot, func))
-				break;
-		}
+	/* Only can scan first domain */
+	for (num = 0; num < 256; num++)
+		for (slot = 0; slot < 32; slot++)
+			for (func = 0; func < 8; func++) {
+				/* Only probe func 0 on single fn devices */
+				if (check_dev_quirk(num, slot, func))
+					break;
+			}
 }

Index: linux-2.6/arch/x86/include/asm/pci-direct.h
===================================================================
@@ -18,4 +18,6 @@ extern int early_pci_allowed(void);
 extern unsigned int pci_early_dump_regs;
 extern void early_dump_pci_device(u8 bus, u8 slot, u8 func);
 extern void early_dump_pci_devices(void);
+
+struct pci_dev *get_early_pci_dev(int num, int slot, int func);
 #endif /* _ASM_X86_PCI_DIRECT_H */

78 lines not on this page · raw message

Index: linux-2.6/arch/x86/kernel/early-quirks.c
===================================================================
@@ -243,7 +243,7 @@ static int __init check_dev_quirk(int nu
 	class = read_pci_config_16(num, slot, func, PCI_CLASS_DEVICE);
 
 	if (class == 0xffff)
-		return -1; /* no class, treat as single function */
+		return 0; /* no class, func gap */
 
 	vendor = read_pci_config_16(num, slot, func, PCI_VENDOR_ID);
 

@@ -263,27 +263,30 @@ static int __init check_dev_quirk(int nu
 			}
 	}
 
-	type = read_pci_config_byte(num, slot, func,
-				    PCI_HEADER_TYPE);
-	if (!(type & 0x80))
-		return -1;
+	if (func == 0) {
+		type = read_pci_config_byte(num, slot, func,
+					    PCI_HEADER_TYPE);
+		if (!(type & 0x80))
+			return -1;
+	}
 
 	return 0;
 }
 
 void __init early_quirks(void)
 {
-	int slot, func;
+	int num, slot, func;
 
 	if (!early_pci_allowed())
 		return;
 
 	/* Poor man's PCI discovery */
-	/* Only scan the root bus */
-	for (slot = 0; slot < 32; slot++)
-		for (func = 0; func < 8; func++) {
-			/* Only probe function 0 on single fn devices */
-			if (check_dev_quirk(0, slot, func))
-				break;
-		}
+	/* Only can scan first domain */
+	for (num = 0; num < 256; num++)
+		for (slot = 0; slot < 32; slot++)
+			for (func = 0; func < 8; func++) {
+				/* Only probe func 0 on single fn devices */
+				if (check_dev_quirk(num, slot, func))
+					break;
+			}
 }

Index: linux-2.6/drivers/usb/host/pci-quirks.c
===================================================================
@@ -17,6 +17,19 @@
 #include "pci-quirks.h"
 #include "xhci-ext-caps.h"
 
+static void default_usb_handoff_udelay(unsigned long usecs)
+{
+	udelay(usecs);
+}
+
+static void __devinit default_usb_handoff_msleep(unsigned long msecs)
+{
+	msleep(msecs);
+}
+
+void (*usb_handoff_udelay)(unsigned long) = default_usb_handoff_udelay;
+void (*usb_handoff_msleep)(unsigned long) __devinitdata =
+			default_usb_handoff_msleep;
 
 #define UHCI_USBLEGSUP		0xc0		/* legacy support */
 #define UHCI_USBCMD		0		/* command register */

@@ -71,7 +84,7 @@ void uhci_reset_hc(struct pci_dev *pdev,
 	 */
 	outw(UHCI_USBCMD_HCRESET, base + UHCI_USBCMD);
 	mb();
-	udelay(5);
+	(*usb_handoff_udelay)(5);
 	if (inw(base + UHCI_USBCMD) & UHCI_USBCMD_HCRESET)
 		dev_warn(&pdev->dev, "HCRESET not completed yet!\n");
 

@@ -106,78 +119,38 @@ int uhci_check_and_reset_hc(struct pci_d
 	 */
 	pci_read_config_word(pdev, UHCI_USBLEGSUP, &legsup);
 	if (legsup & ~(UHCI_USBLEGSUP_RO | UHCI_USBLEGSUP_RWC)) {
-		dev_dbg(&pdev->dev, "%s: legsup = 0x%04x\n",
-				__func__, legsup);
+		dev_printk(KERN_DEBUG, &pdev->dev,
+				 "legsup = 0x%04x\n", legsup);
 		goto reset_needed;
 	}
 
 	cmd = inw(base + UHCI_USBCMD);
 	if ((cmd & UHCI_USBCMD_RUN) || !(cmd & UHCI_USBCMD_CONFIGURE) ||
 			!(cmd & UHCI_USBCMD_EGSM)) {
-		dev_dbg(&pdev->dev, "%s: cmd = 0x%04x\n",
-				__func__, cmd);
+		dev_printk(KERN_DEBUG, &pdev->dev, "cmd = 0x%04x\n", cmd);
 		goto reset_needed;
 	}
 
 	intr = inw(base + UHCI_USBINTR);
 	if (intr & (~UHCI_USBINTR_RESUME)) {
-		dev_dbg(&pdev->dev, "%s: intr = 0x%04x\n",
-				__func__, intr);
+		dev_printk(KERN_DEBUG, &pdev->dev, "intr = 0x%04x\n", intr);
 		goto reset_needed;
 	}
 	return 0;
 
 reset_needed:
-	dev_dbg(&pdev->dev, "Performing full reset\n");
+	dev_printk(KERN_DEBUG, &pdev->dev, "Performing full reset\n");
+
 	uhci_reset_hc(pdev, base);
+
 	return 1;
 }
 EXPORT_SYMBOL_GPL(uhci_check_and_reset_hc);
 
-static inline int io_type_enabled(struct pci_dev *pdev, unsigned int mask)
-{
-	u16 cmd;
-	return !pci_read_config_word(pdev, PCI_COMMAND, &cmd) && (cmd & mask);
-}
-
-#define pio_enabled(dev) io_type_enabled(dev, PCI_COMMAND_IO)
-#define mmio_enabled(dev) io_type_enabled(dev, PCI_COMMAND_MEMORY)
-
-static void __devinit quirk_usb_handoff_uhci(struct pci_dev *pdev)
-{
-	unsigned long base = 0;
-	int i;
-
-	if (!pio_enabled(pdev))
-		return;
-
-	for (i = 0; i < PCI_ROM_RESOURCE; i++)
-		if ((pci_resource_flags(pdev, i) & IORESOURCE_IO)) {
-			base = pci_resource_start(pdev, i);
-			break;
-		}
-
-	if (base)
-		uhci_check_and_reset_hc(pdev, base);
-}
-
-static int __devinit mmio_resource_enabled(struct pci_dev *pdev, int idx)
+void __devinit __usb_handoff_ohci(struct pci_dev *pdev, void __iomem *base)
 {
-	return pci_resource_start(pdev, idx) && mmio_enabled(pdev);
-}
-
-static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
-{
-	void __iomem *base;
 	u32 control;
 
-	if (!mmio_resource_enabled(pdev, 0))
-		return;
-
-	base = pci_ioremap_bar(pdev, 0);
-	if (base == NULL)
-		return;
-
 	control = readl(base + OHCI_CONTROL);
 
 /* On PA-RISC, PDC can leave IR set incorrectly; ignore it there. */

@@ -193,7 +166,7 @@ static void __devinit quirk_usb_handoff_
 		while (wait_time > 0 &&
 				readl(base + OHCI_CONTROL) & OHCI_CTRL_IR) {
 			wait_time -= 10;
-			msleep(10);
+			(*usb_handoff_msleep)(10);
 		}
 		if (wait_time <= 0)
 			dev_warn(&pdev->dev, "OHCI: BIOS handoff failed"

@@ -210,26 +183,17 @@ static void __devinit quirk_usb_handoff_
 	 */
 	writel(~(u32)0, base + OHCI_INTRDISABLE);
 	writel(~(u32)0, base + OHCI_INTRSTATUS);
-
-	iounmap(base);
 }
 
-static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
+void __devinit __usb_handoff_ehci(struct pci_dev *pdev, void __iomem *base)
 {
 	int wait_time, delta;
-	void __iomem *base, *op_reg_base;
+	void *op_reg_base;
 	u32	hcc_params, val;
 	u8	offset, cap_length;
 	int	count = 256/4;
 	int	tried_handoff = 0;
 
-	if (!mmio_resource_enabled(pdev, 0))
-		return;
-
-	base = pci_ioremap_bar(pdev, 0);
-	if (base == NULL)
-		return;
-
 	cap_length = readb(base);
 	op_reg_base = base + cap_length;
 

@@ -247,7 +211,8 @@ static void __devinit quirk_usb_disable_
 		switch (cap & 0xff) {
 		case 1:			/* BIOS/SMM/... handoff support */
 			if ((cap & EHCI_USBLEGSUP_BIOS)) {
-				dev_dbg(&pdev->dev, "EHCI: BIOS handoff\n");
+				dev_printk(KERN_DEBUG, &pdev->dev,
+						"EHCI: BIOS handoff\n");
 
 #if 0
 /* [email protected] reports that some systems need SMI forced on,

@@ -279,7 +244,7 @@ static void __devinit quirk_usb_disable_
 			msec = 1000;
 			while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
 				tried_handoff = 1;
-				msleep(10);
+				(*usb_handoff_msleep)(10);
 				msec -= 10;
 				pci_read_config_dword(pdev, offset, &cap);
 			}

@@ -330,18 +295,15 @@ static void __devinit quirk_usb_disable_
 		delta = 100;
 		do {
 			writel(0x3f, op_reg_base + EHCI_USBSTS);
-			udelay(delta);
+			(*usb_handoff_udelay)(delta);
 			wait_time -= delta;
 			val = readl(op_reg_base + EHCI_USBSTS);
-			if ((val == ~(u32)0) || (val & EHCI_USBSTS_HALTED)) {
+			if ((val == ~(u32)0) || (val & EHCI_USBSTS_HALTED))
 				break;
-			}
 		} while (wait_time > 0);
 	}
 	writel(0, op_reg_base + EHCI_USBINTR);
 	writel(0x3f, op_reg_base + EHCI_USBSTS);
-
-	iounmap(base);
 }
 
 /*

@@ -357,7 +319,7 @@ static void __devinit quirk_usb_disable_
  * Returns -ETIMEDOUT if this condition is not true after
  * wait_usec microseconds have passed.
  */
-static int handshake(void __iomem *ptr, u32 mask, u32 done,
+static int __devinit handshake(void __iomem *ptr, u32 mask, u32 done,
 		int wait_usec, int delay_usec)
 {
 	u32	result;

@@ -367,7 +329,7 @@ static int handshake(void __iomem *ptr,
 		result &= mask;
 		if (result == done)
 			return 0;
-		udelay(delay_usec);
+		(*usb_handoff_udelay)(delay_usec);
 		wait_usec -= delay_usec;
 	} while (wait_usec > 0);
 	return -ETIMEDOUT;

@@ -381,22 +343,13 @@ static int handshake(void __iomem *ptr,
  * and then waits 5 seconds for the BIOS to hand over control.
  * If we timeout, assume the BIOS is broken and take control anyway.
  */
-static void __devinit quirk_usb_handoff_xhci(struct pci_dev *pdev)
+void __usb_handoff_xhci(struct pci_dev *pdev, void __iomem *base)
 {
-	void __iomem *base;
 	int ext_cap_offset;
 	void __iomem *op_reg_base;
 	u32 val;
 	int timeout;
 
-	if (!mmio_resource_enabled(pdev, 0))
-		return;
-
-	base = ioremap_nocache(pci_resource_start(pdev, 0),
-				pci_resource_len(pdev, 0));
-	if (base == NULL)
-		return;
-
 	/*
 	 * Find the Legacy Support Capability register -
 	 * this is optional for xHCI host controllers.

@@ -462,6 +415,86 @@ hc_init:
 				"xHCI HW did not halt within %d usec "
 				"status = 0x%x\n", XHCI_MAX_HALT_USEC, val);
 	}
+}
+
+static inline int io_type_enabled(struct pci_dev *pdev, unsigned int mask)
+{
+	u16 cmd;
+	return !pci_read_config_word(pdev, PCI_COMMAND, &cmd) && (cmd & mask);
+}
+
+#define pio_enabled(dev) io_type_enabled(dev, PCI_COMMAND_IO)
+#define mmio_enabled(dev) io_type_enabled(dev, PCI_COMMAND_MEMORY)
+
+static void __devinit quirk_usb_handoff_uhci(struct pci_dev *pdev)
+{
+	unsigned long base = 0;
+	int i;
+
+	if (!pio_enabled(pdev))
+		return;
+
+	for (i = 0; i < PCI_ROM_RESOURCE; i++)
+		if ((pci_resource_flags(pdev, i) & IORESOURCE_IO)) {
+			base = pci_resource_start(pdev, i);
+			break;
+		}
+
+	if (!base)
+		return;
+
+	uhci_check_and_reset_hc(pdev, base);
+}
+
+static int __devinit mmio_resource_enabled(struct pci_dev *pdev, int idx)
+{
+	return pci_resource_start(pdev, idx) && mmio_enabled(pdev);
+}
+
+static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
+{
+	void __iomem *base;
+
+	if (!mmio_resource_enabled(pdev, 0))
+		return;
+
+	base = pci_ioremap_bar(pdev, 0);
+	if (base == NULL)
+		return;
+
+	__usb_handoff_ohci(pdev, base);
+
+	iounmap(base);
+}
+
+static void __devinit quirk_usb_handoff_ehci(struct pci_dev *pdev)
+{
+	void __iomem *base;
+
+	if (!mmio_resource_enabled(pdev, 0))
+		return;
+
+	base = pci_ioremap_bar(pdev, 0);
+	if (base == NULL)
+		return;
+
+	__usb_handoff_ehci(pdev, base);
+
+	iounmap(base);
+}
+
+static void __devinit quirk_usb_handoff_xhci(struct pci_dev *pdev)
+{
+	void __iomem *base;
+
+	if (!mmio_resource_enabled(pdev, 0))
+		return;
+
+	base = pci_ioremap_bar(pdev, 0);
+	if (base == NULL)
+		return;
+
+	__usb_handoff_xhci(pdev, base);
 
 	iounmap(base);
 }

@@ -473,7 +506,7 @@ static void __devinit quirk_usb_early_ha
 	else if (pdev->class == PCI_CLASS_SERIAL_USB_OHCI)
 		quirk_usb_handoff_ohci(pdev);
 	else if (pdev->class == PCI_CLASS_SERIAL_USB_EHCI)
-		quirk_usb_disable_ehci(pdev);
+		quirk_usb_handoff_ehci(pdev);
 	else if (pdev->class == PCI_CLASS_SERIAL_USB_XHCI)
 		quirk_usb_handoff_xhci(pdev);
 }

Index: linux-2.6/drivers/usb/host/pci-quirks.h
===================================================================
@@ -3,5 +3,11 @@
 
 void uhci_reset_hc(struct pci_dev *pdev, unsigned long base);
 int uhci_check_and_reset_hc(struct pci_dev *pdev, unsigned long base);
+void __usb_handoff_ohci(struct pci_dev *pdev, void __iomem *base);
+void __usb_handoff_ehci(struct pci_dev *pdev, void __iomem *base);
+void __usb_handoff_xhci(struct pci_dev *pdev, void __iomem *base);
+
+extern void (*usb_handoff_udelay)(unsigned long);
+extern void (*usb_handoff_msleep)(unsigned long);
 
 #endif  /*  __LINUX_USB_PCI_QUIRKS_H  */

Index: linux-2.6/arch/x86/kernel/early-quirks.c
===================================================================
@@ -19,6 +19,179 @@
 #include <asm/iommu.h>
 #include <asm/gart.h>
 
+#include "../../../drivers/usb/host/pci-quirks.h"
+
+static inline void early_udelay2(void)
+{
+#ifndef CONFIG_PARAVIRT
+	native_io_delay();
+#else
+	pv_cpu_ops.io_delay();
+#endif
+}
+
+static void early_usb_handoff_udelay(unsigned long usecs)
+{
+	unsigned long count;
+
+	count = usecs >> 1;
+
+	if (!count)
+		count = 1;
+
+	while (count-- > 0)
+		early_udelay2();
+}
+
+static void early_usb_handoff_msleep(unsigned long msecs)
+{
+	while (msecs-- > 0)
+		early_usb_handoff_udelay(1000);
+}
+
+static __init u32 get_usb_io_port(int num, int slot, int func)
+{
+	int i;
+	u16 cmd;
+
+	cmd = read_pci_config_16(num, slot, func, PCI_COMMAND);
+	if (!(cmd & PCI_COMMAND_IO))
+		return 0;
+
+	for (i = 0; i < PCI_ROM_RESOURCE; i++) {
+		u32 addr = read_pci_config(num, slot, func, 0x10 + (i<<2));
+
+		if (!addr)
+			continue;
+		if ((addr&PCI_BASE_ADDRESS_SPACE) != PCI_BASE_ADDRESS_SPACE_IO)
+			continue;
+
+		addr &= PCI_BASE_ADDRESS_IO_MASK;
+		if (addr)
+			return addr;
+	}
+
+	return 0;
+}
+
+static void __init quirk_usb_handoff_uhci(int num, int slot, int func)
+{
+	unsigned long base;
+
+	base = get_usb_io_port(num, slot, func);
+	if (!base)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: uhci ioport = 0x%04lx\n",
+					num, slot, func, base);
+	uhci_check_and_reset_hc(get_early_pci_dev(num, slot, func), base);
+}
+
+static __init u32 get_usb_mmio_addr(int num, int slot, int func)
+{
+	u32 addr;
+	u16 cmd;
+
+	cmd = read_pci_config_16(num, slot, func, PCI_COMMAND);
+	if (!(cmd & PCI_COMMAND_MEMORY))
+		return 0;
+
+	addr = read_pci_config(num, slot, func, 0x10);
+	if (!addr)
+		return 0;
+	if ((addr & PCI_BASE_ADDRESS_SPACE) != PCI_BASE_ADDRESS_SPACE_MEMORY)
+		return 0;
+
+	addr &= PCI_BASE_ADDRESS_MEM_MASK;
+
+	return addr;
+}
+
+static __init void quirk_usb_handoff_ohci(int num, int slot, int func)
+{
+	void __iomem *base;
+	u32 addr;
+
+	addr = get_usb_mmio_addr(num, slot, func);
+	if (!addr)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: ohci mmio = 0x%08x\n",
+				num, slot, func, addr);
+	base = early_ioremap(addr, 0x1000);
+	if (!base)
+		return;
+
+	__usb_handoff_ohci(get_early_pci_dev(num, slot, func), base);
+
+	early_iounmap(base, 0x1000);
+}
+
+static __init void quirk_usb_handoff_ehci(int num, int slot, int func)
+{
+	void __iomem *base;
+	u32 addr;
+
+	addr = get_usb_mmio_addr(num, slot, func);
+	if (!addr)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: ehci mmio = 0x%08x\n",
+				 num, slot, func, addr);
+	base = early_ioremap(addr, 0x1000);
+	if (!base)
+		return;
+
+	__usb_handoff_ehci(get_early_pci_dev(num, slot, func), base);
+
+	early_iounmap(base, 0x1000);
+}
+
+static __init void quirk_usb_handoff_xhci(int num, int slot, int func)
+{
+	void __iomem *base;
+	u32 addr;
+
+	addr = get_usb_mmio_addr(num, slot, func);
+	if (!addr)
+		return;
+
+	printk(KERN_DEBUG "%02x:%02x.%01x: xhci mmio = 0x%08x\n",
+				num, slot, func, addr);
+	base = early_ioremap(addr, 0x1000);
+	if (!base)
+		return;
+
+	__usb_handoff_xhci(get_early_pci_dev(num, slot, func), base);
+
+	early_iounmap(base, 0x1000);
+}
+
+static __init void quirk_usb_handoff(int num, int slot, int func)
+{
+	u32 class;
+	void (*old_usb_handoff_udelay)(unsigned long) = usb_handoff_udelay;
+	void (*old_usb_handoff_msleep)(unsigned long) = usb_handoff_msleep;
+
+	usb_handoff_udelay = early_usb_handoff_udelay;
+	usb_handoff_msleep = early_usb_handoff_msleep;
+
+	class = read_pci_config(num, slot, func, PCI_CLASS_REVISION);
+	class >>= 8;
+
+	if (class == PCI_CLASS_SERIAL_USB_UHCI)
+		quirk_usb_handoff_uhci(num, slot, func);
+	else if (class == PCI_CLASS_SERIAL_USB_OHCI)
+		quirk_usb_handoff_ohci(num, slot, func);
+	else if (class == PCI_CLASS_SERIAL_USB_EHCI)
+		quirk_usb_handoff_ehci(num, slot, func);
+	else if (class == PCI_CLASS_SERIAL_USB_XHCI)
+		quirk_usb_handoff_xhci(num, slot, func);
+
+	usb_handoff_udelay = old_usb_handoff_udelay;
+	usb_handoff_msleep = old_usb_handoff_msleep;
+}
+
 static void __init fix_hypertransport_config(int num, int slot, int func)
 {
 	u32 htcfg;

@@ -208,6 +381,8 @@ struct chipset {
  * only matching on bus 0.
  */
 static struct chipset early_qrk[] __initdata = {
+	{ PCI_ANY_ID, PCI_ANY_ID,
+	  PCI_CLASS_SERIAL_USB, PCI_ANY_ID, 0, quirk_usb_handoff },
 	{ PCI_VENDOR_ID_NVIDIA, PCI_ANY_ID,
 	  PCI_CLASS_BRIDGE_PCI, PCI_ANY_ID, QFLAG_APPLY_ONCE, nvidia_bugs },
 	{ PCI_VENDOR_ID_VIA, PCI_ANY_ID,

Index: linux-2.6/arch/x86/include/asm/delay.h
===================================================================
@@ -28,4 +28,7 @@ extern void __delay(unsigned long loops)
 
 void use_tsc_delay(void);
 
+extern void __early_udelay(unsigned long usecs);
+extern void __early_msleep(unsigned int msecs);
+
 #endif /* _ASM_X86_DELAY_H */

Index: linux-2.6/arch/x86/kernel/setup.c
===================================================================
@@ -853,6 +853,10 @@ void __init setup_arch(char **cmdline_p)
 
 	dmi_scan_machine();
 
+	/* for early using */
+	safe_udelay = __early_udelay;
+	safe_msleep = __early_msleep;
+
 	/*
 	 * VMware detection requires dmi to be available, so this
 	 * needs to be done after dmi_scan_machine, for the BP.

Index: linux-2.6/arch/x86/lib/delay.c
===================================================================
@@ -138,3 +138,31 @@ void __ndelay(unsigned long nsecs)
 	__const_udelay(nsecs * 0x00005); /* 2**32 / 1000000000 (rounded up) */
 }
 EXPORT_SYMBOL(__ndelay);
+
+/* before cpu_info.loops_per_jiffy get set */
+static inline void __early_const_udelay(unsigned long xloops)
+{
+	int d0;
+
+	xloops *= 4;
+	asm("mull %%edx"
+		: "=d" (xloops), "=&a" (d0)
+		: "1" (xloops), "0"
+		(loops_per_jiffy * (HZ/4)));
+
+	delay_loop(++xloops);
+}
+
+/* usecs need to < 2000 */
+void __init __early_udelay(unsigned long usecs)
+{
+	/* 2**32 / 1000000 (rounded up) */
+	__early_const_udelay(usecs * 0x000010c7);
+}
+
+/* before schedular is there */
+void __init __early_msleep(unsigned int msecs)
+{
+	while (msecs--)
+		__early_udelay(1000);
+}

Index: linux-2.6/include/linux/delay.h
===================================================================
@@ -52,4 +52,6 @@ static inline void ssleep(unsigned int s
 	msleep(seconds * 1000);
 }
 
+extern void (*safe_udelay)(unsigned long);
+extern void (*safe_msleep)(unsigned int);
 #endif /* defined(_LINUX_DELAY_H) */

Index: linux-2.6/init/main.c
===================================================================
@@ -240,6 +240,9 @@ unsigned long loops_per_jiffy = (1<<12);
 
 EXPORT_SYMBOL(loops_per_jiffy);
 
+void (*safe_udelay)(unsigned long) = __udelay;
+void (*safe_msleep)(unsigned int) = msleep;
+
 static int __init debug_kernel(char *str)
 {
 	console_loglevel = 10;

@@ -879,6 +882,9 @@ static int __init kernel_init(void * unu
 	cad_pid = task_pid(current);
 
 	smp_prepare_cpus(setup_max_cpus);
+	/* set them back, x86 use it for early delay*/
+	safe_udelay = __udelay;
+	safe_msleep = msleep;
 
 	do_pre_smp_initcalls();
 	lockup_detector_init();

Index: linux-2.6/arch/x86/lib/delay.c
===================================================================
@@ -113,7 +113,7 @@ void __delay(unsigned long loops)
 }
 EXPORT_SYMBOL(__delay);
 
-inline void __const_udelay(unsigned long xloops)
+static void __normal_const_udelay(unsigned long xloops)
 {
 	int d0;
 

@@ -125,8 +125,34 @@ inline void __const_udelay(unsigned long
 
 	__delay(++xloops);
 }
+
+/* before percpu cpu_info.loops_per_jiffy is set */
+static void __init __early_const_udelay(unsigned long xloops)
+{
+	int d0;
+
+	xloops *= 4;
+	asm("mull %%edx"
+		: "=d" (xloops), "=&a" (d0)
+		: "1" (xloops), "0"
+		(loops_per_jiffy * (HZ/4)));
+
+	delay_loop(++xloops);
+}
+
+static void (*__const_udelay_fn)(unsigned long) = __early_const_udelay;
+
+void __const_udelay(unsigned long usecs)
+{
+	__const_udelay_fn(usecs);
+}
 EXPORT_SYMBOL(__const_udelay);
 
+void __init use_normal_delay(void)
+{
+	__const_udelay_fn = __normal_const_udelay;
+}
+
 void __udelay(unsigned long usecs)
 {
 	__const_udelay(usecs * 0x000010c7); /* 2**32 / 1000000 (rounded up) */

Index: linux-2.6/include/linux/delay.h
===================================================================
@@ -52,4 +52,6 @@ static inline void ssleep(unsigned int s
 	msleep(seconds * 1000);
 }
 
+extern void use_normal_delay(void);
+extern void use_normal_sleep(void);
 #endif /* defined(_LINUX_DELAY_H) */

Index: linux-2.6/init/main.c
===================================================================
@@ -879,6 +879,9 @@ static int __init kernel_init(void * unu
 	cad_pid = task_pid(current);
 
 	smp_prepare_cpus(setup_max_cpus);
+	/* set them back, x86 use it for early delay*/
+	use_normal_delay();
+	use_normal_sleep();
 
 	do_pre_smp_initcalls();
 	lockup_detector_init();

Index: linux-2.6/kernel/timer.c
===================================================================
@@ -1723,11 +1723,7 @@ void __init init_timers(void)
 	open_softirq(TIMER_SOFTIRQ, run_timer_softirq);
 }
 
-/**
- * msleep - sleep safely even with waitqueue interruptions
- * @msecs: Time in milliseconds to sleep for
- */
-void msleep(unsigned int msecs)
+static void __normal_msleep(unsigned int msecs)
 {
 	unsigned long timeout = msecs_to_jiffies(msecs) + 1;
 

@@ -1735,6 +1731,28 @@ void msleep(unsigned int msecs)
 		timeout = schedule_timeout_uninterruptible(timeout);
 }
 
+static void __init __early_msleep(unsigned int msecs)
+{
+	mdelay(msecs);
+}
+
+static void (*msleep_fn)(unsigned int) = __early_msleep;
+
+void __init use_normal_sleep(void)
+{
+	msleep_fn = __normal_msleep;
+}
+
+void __init __weak use_normal_delay(void) { }
+
+/**
+ * msleep - sleep safely even with waitqueue interruptions
+ * @msecs: Time in milliseconds to sleep for
+ */
+void msleep(unsigned int msecs)
+{
+	msleep_fn(msecs);
+}
 EXPORT_SYMBOL(msleep);
 
 /**

index d3cfe26..333694a 100644
@@ -734,6 +734,7 @@ void __init setup_arch(char **cmdline_p)
 
        early_trap_init();
        early_cpu_init();
+       cpu_data(0).loops_per_jiffy = loops_per_jiffy;
        early_ioremap_init();
 
        setup_olpc_ofw_pgd();