15 files changed, 607 insertions, 390 deletions

diff --git a/arch/powerpc/kernel/Makefile b/arch/powerpc/kernel/Makefile
index aab0ae33a42a..bf3fd6f02249 100644
--- a/arch/powerpc/kernel/Makefile
+++ b/arch/powerpc/kernel/Makefile
@@ -33,10 +33,6 @@ obj-$(CONFIG_LPARCFG)		+= lparcfg.o
 obj-$(CONFIG_IBMVIO)		+= vio.o
 obj-$(CONFIG_IBMEBUS)           += ibmebus.o
 obj-$(CONFIG_GENERIC_TBSYNC)	+= smp-tbsync.o
-obj-$(CONFIG_PPC_PSERIES)	+= udbg_16550.o
-obj-$(CONFIG_PPC_MAPLE)		+= udbg_16550.o
-udbgscc-$(CONFIG_PPC64)		:= udbg_scc.o
-obj-$(CONFIG_PPC_PMAC)		+= $(udbgscc-y)
 obj64-$(CONFIG_PPC_MULTIPLATFORM) += nvram_64.o
 
 ifeq ($(CONFIG_PPC_MERGE),y)
@@ -59,14 +55,15 @@ obj-$(CONFIG_BOOTX_TEXT)	+= btext.o
 obj-$(CONFIG_6xx)		+= idle_6xx.o
 obj-$(CONFIG_SMP)		+= smp.o
 obj-$(CONFIG_KPROBES)		+= kprobes.o
-
+obj-$(CONFIG_PPC_MULTIPLATFORM) += legacy_serial.o
+obj64-$(CONFIG_PPC_MULTIPLATFORM) += udbg_16550.o
+obj64-$(CONFIG_PPC_PMAC)	+= udbg_scc.o
 module-$(CONFIG_PPC64)		+= module_64.o
 obj-$(CONFIG_MODULES)		+= $(module-y)
 
 pci64-$(CONFIG_PPC64)		+= pci_64.o pci_dn.o pci_iommu.o \
 				   pci_direct_iommu.o iomap.o
 obj-$(CONFIG_PCI)		+= $(pci64-y)
-
 kexec-$(CONFIG_PPC64)		:= machine_kexec_64.o
 kexec-$(CONFIG_PPC32)		:= machine_kexec_32.o
 obj-$(CONFIG_KEXEC)		+= machine_kexec.o $(kexec-y)
diff --git a/arch/powerpc/kernel/legacy_serial.c b/arch/powerpc/kernel/legacy_serial.c
new file mode 100644
index 000000000000..28ad50e424a9
--- /dev/null
+++ b/arch/powerpc/kernel/legacy_serial.c
@@ -0,0 +1,478 @@
+#include <linux/config.h>
+#include <linux/kernel.h>
+#include <linux/serial.h>
+#include <linux/serial_8250.h>
+#include <linux/serial_core.h>
+#include <linux/console.h>
+#include <linux/pci.h>
+#include <asm/io.h>
+#include <asm/mmu.h>
+#include <asm/prom.h>
+#include <asm/serial.h>
+#include <asm/udbg.h>
+#include <asm/pci-bridge.h>
+#include <asm/ppc-pci.h>
+
+#undef DEBUG
+
+#ifdef DEBUG
+#define DBG(fmt...) do { printk(fmt); } while(0)
+#else
+#define DBG(fmt...) do { } while(0)
+#endif
+
+#define MAX_LEGACY_SERIAL_PORTS	8
+
+static struct plat_serial8250_port
+legacy_serial_ports[MAX_LEGACY_SERIAL_PORTS+1];
+static struct legacy_serial_info {
+	struct device_node		*np;
+	unsigned int			speed;
+	unsigned int			clock;
+	phys_addr_t			taddr;
+} legacy_serial_infos[MAX_LEGACY_SERIAL_PORTS];
+static unsigned int legacy_serial_count;
+static int legacy_serial_console = -1;
+
+static int __init add_legacy_port(struct device_node *np, int want_index,
+				  int iotype, phys_addr_t base,
+				  phys_addr_t taddr, unsigned long irq)
+{
+	u32 *clk, *spd, clock;
+	int index;
+
+	/* get clock freq. if present */
+	clk = (u32 *)get_property(np, "clock-frequency", NULL);
+	clock = clk ? *clk : BASE_BAUD * 16;
+
+	/* get default speed if present */
+	spd = (u32 *)get_property(np, "current-speed", NULL);
+
+	/* If we have a location index, then try to use it */
+	if (want_index >= 0 && want_index < MAX_LEGACY_SERIAL_PORTS)
+		index = want_index;
+	else
+		index = legacy_serial_count;
+
+	/* if our index is still out of range, that mean that
+	 * array is full, we could scan for a free slot but that
+	 * make little sense to bother, just skip the port
+	 */
+	if (index >= MAX_LEGACY_SERIAL_PORTS)
+		return -1;
+	if (index >= legacy_serial_count)
+		legacy_serial_count = index + 1;
+
+	/* Check if there is a port who already claimed our slot */
+	if (legacy_serial_infos[index].np != 0) {
+		/* if we still have some room, move it, else override */
+		if (legacy_serial_count < MAX_LEGACY_SERIAL_PORTS) {
+			printk(KERN_INFO "Moved legacy port %d -> %d\n",
+			       index, legacy_serial_count);
+			legacy_serial_ports[legacy_serial_count] =
+				legacy_serial_ports[index];
+			legacy_serial_infos[legacy_serial_count] =
+				legacy_serial_infos[index];
+			legacy_serial_count++;
+		} else {
+			printk(KERN_INFO "Replacing legacy port %d\n", index);
+		}
+	}
+
+	/* Now fill the entry */
+	memset(&legacy_serial_ports[index], 0,
+	       sizeof(struct plat_serial8250_port));
+	if (iotype == UPIO_PORT)
+		legacy_serial_ports[index].iobase = base;
+	else
+		legacy_serial_ports[index].membase = (void __iomem *)base;
+	legacy_serial_ports[index].iotype = iotype;
+	legacy_serial_ports[index].uartclk = clock;
+	legacy_serial_ports[index].irq = irq;
+	legacy_serial_ports[index].flags = ASYNC_BOOT_AUTOCONF;
+	legacy_serial_infos[index].taddr = taddr;
+	legacy_serial_infos[index].np = of_node_get(np);
+	legacy_serial_infos[index].clock = clock;
+	legacy_serial_infos[index].speed = spd ? *spd : 0;
+
+	printk(KERN_INFO "Found legacy serial port %d for %s\n",
+	       index, np->full_name);
+	printk(KERN_INFO "  %s=%llx, taddr=%llx, irq=%lx, clk=%d, speed=%d\n",
+	       (iotype == UPIO_PORT) ? "port" : "mem",
+	       (unsigned long long)base, (unsigned long long)taddr, irq,
+	       legacy_serial_ports[index].uartclk,
+	       legacy_serial_infos[index].speed);
+
+	return index;
+}
+
+static int __init add_legacy_isa_port(struct device_node *np,
+				      struct device_node *isa_bridge)
+{
+	u32 *reg;
+	char *typep;
+	int index = -1;
+	phys_addr_t taddr;
+
+	/* Get the ISA port number */
+	reg = (u32 *)get_property(np, "reg", NULL);
+	if (reg == NULL)
+		return -1;
+
+	/* Verify it's an IO port, we don't support anything else */
+	if (!(reg[0] & 0x00000001))
+		return -1;
+
+	/* Now look for an "ibm,aix-loc" property that gives us ordering
+	 * if any...
+	 */
+	typep = (char *)get_property(np, "ibm,aix-loc", NULL);
+
+	/* If we have a location index, then use it */
+	if (typep && *typep == 'S')
+		index = simple_strtol(typep+1, NULL, 0) - 1;
+
+	/* Translate ISA address */
+	taddr = of_translate_address(np, reg);
+
+	/* Add port, irq will be dealt with later */
+	return add_legacy_port(np, index, UPIO_PORT, reg[1], taddr, NO_IRQ);
+
+}
+
+static int __init add_legacy_pci_port(struct device_node *np,
+				      struct device_node *pci_dev)
+{
+	phys_addr_t addr, base;
+	u32 *addrp;
+	int iotype, index = -1;
+
+#if 0
+	/* We only support ports that have a clock frequency properly
+	 * encoded in the device-tree (that is have an fcode). Anything
+	 * else can't be used that early and will be normally probed by
+	 * the generic 8250_pci driver later on.
+	 */
+	if (get_property(np, "clock-frequency", NULL) == NULL)
+		return -1;
+#endif
+
+	/* Get the PCI address. Assume BAR 0 */
+	addrp = of_get_pci_address(pci_dev, 0, NULL);
+	if (addrp == NULL)
+		return -1;
+
+	/* We only support BAR 0 for now */
+	iotype = (addrp[0] & 0x02000000) ? UPIO_MEM : UPIO_PORT;
+	addr = of_translate_address(pci_dev, addrp);
+
+	/* Set the IO base to the same as the translated address for MMIO,
+	 * or to the domain local IO base for PIO (it will be fixed up later)
+	 */
+	if (iotype == UPIO_MEM)
+		base = addr;
+	else
+		base = addrp[2];
+
+	/* Try to guess an index... If we have subdevices of the pci dev,
+	 * we get to their "reg" property
+	 */
+	if (np != pci_dev) {
+		u32 *reg = (u32 *)get_property(np, "reg", NULL);
+		if (reg && (*reg < 4))
+			index = legacy_serial_count + *reg;
+	}
+
+	/* Add port, irq will be dealt with later. We passed a translated
+	 * IO port value. It will be fixed up later along with the irq
+	 */
+	return add_legacy_port(np, index, iotype, base, addr, NO_IRQ);
+}
+
+/*
+ * This is called very early, as part of setup_system() or eventually
+ * setup_arch(), basically before anything else in this file. This function
+ * will try to build a list of all the available 8250-compatible serial ports
+ * in the machine using the Open Firmware device-tree. It currently only deals
+ * with ISA and PCI busses but could be extended. It allows a very early boot
+ * console to be initialized, that list is also used later to provide 8250 with
+ * the machine non-PCI ports and to properly pick the default console port
+ */
+void __init find_legacy_serial_ports(void)
+{
+	struct device_node *np, *stdout;
+	char *path;
+	int index;
+
+	DBG(" -> find_legacy_serial_port()\n");
+
+	/* Now find out if one of these is out firmware console */
+	path = (char *)get_property(of_chosen, "linux,stdout-path", NULL);
+	if (path == NULL) {
+		DBG(" no linux,stdout-path !\n");
+		return;
+	}
+	stdout = of_find_node_by_path(path);
+	if (stdout) {
+		DBG("stdout is %s\n", stdout->full_name);
+	}
+
+	/* First fill our array with ISA ports */
+	for (np = NULL; (np = of_find_node_by_type(np, "serial"));) {
+		struct device_node *isa = of_get_parent(np);
+		if (isa && !strcmp(isa->name, "isa")) {
+			index = add_legacy_isa_port(np, isa);
+			if (index >= 0 && np == stdout)
+				legacy_serial_console = index;
+		}
+		of_node_put(isa);
+	}
+
+	/* Next, try to locate PCI ports */
+	for (np = NULL; (np = of_find_all_nodes(np));) {
+		struct device_node *pci, *parent = of_get_parent(np);
+		if (parent && !strcmp(parent->name, "isa")) {
+			of_node_put(parent);
+			continue;
+		}
+		if (strcmp(np->name, "serial") && strcmp(np->type, "serial")) {
+			of_node_put(parent);
+			continue;
+		}
+		/* Check for known pciclass, and also check wether we have
+		 * a device with child nodes for ports or not
+		 */
+		if (device_is_compatible(np, "pciclass,0700") ||
+		    device_is_compatible(np, "pciclass,070002"))
+			pci = np;
+		else if (device_is_compatible(parent, "pciclass,0700") ||
+			 device_is_compatible(parent, "pciclass,070002"))
+			pci = parent;
+		else {
+			of_node_put(parent);
+			continue;
+		}
+		index = add_legacy_pci_port(np, pci);
+		if (index >= 0 && np == stdout)
+			legacy_serial_console = index;
+		of_node_put(parent);
+	}
+
+	DBG("legacy_serial_console = %d\n", legacy_serial_console);
+
+	/* udbg is 64 bits only for now, that will change soon though ... */
+#ifdef CONFIG_PPC64
+	while (legacy_serial_console >= 0) {
+		struct legacy_serial_info *info =
+			&legacy_serial_infos[legacy_serial_console];
+		void __iomem *addr;
+
+		if (info->taddr == 0)
+			break;
+		addr = ioremap(info->taddr, 0x1000);
+		if (addr == NULL)
+			break;
+		if (info->speed == 0)
+			info->speed = udbg_probe_uart_speed(addr, info->clock);
+		DBG("default console speed = %d\n", info->speed);
+		udbg_init_uart(addr, info->speed, info->clock);
+		break;
+	}
+#endif /* CONFIG_PPC64 */
+
+	DBG(" <- find_legacy_serial_port()\n");
+}
+
+static struct platform_device serial_device = {
+	.name	= "serial8250",
+	.id	= PLAT8250_DEV_PLATFORM,
+	.dev	= {
+		.platform_data = legacy_serial_ports,
+	},
+};
+
+static void __init fixup_port_irq(int index,
+				  struct device_node *np,
+				  struct plat_serial8250_port *port)
+{
+	DBG("fixup_port_irq(%d)\n", index);
+
+	/* Check for interrupts in that node */
+	if (np->n_intrs > 0) {
+		port->irq = np->intrs[0].line;
+		DBG(" port %d (%s), irq=%d\n",
+		    index, np->full_name, port->irq);
+		return;
+	}
+
+	/* Check for interrupts in the parent */
+	np = of_get_parent(np);
+	if (np == NULL)
+		return;
+
+	if (np->n_intrs > 0) {
+		port->irq = np->intrs[0].line;
+		DBG(" port %d (%s), irq=%d\n",
+		    index, np->full_name, port->irq);
+	}
+	of_node_put(np);
+}
+
+static void __init fixup_port_pio(int index,
+				  struct device_node *np,
+				  struct plat_serial8250_port *port)
+{
+	struct pci_controller *hose;
+
+	DBG("fixup_port_pio(%d)\n", index);
+
+	hose = pci_find_hose_for_OF_device(np);
+	if (hose) {
+		unsigned long offset = (unsigned long)hose->io_base_virt -
+#ifdef CONFIG_PPC64
+			pci_io_base;
+#else
+			isa_io_base;
+#endif
+		DBG("port %d, IO %lx -> %lx\n",
+		    index, port->iobase, port->iobase + offset);
+		port->iobase += offset;
+	}
+}
+
+/*
+ * This is called as an arch initcall, hopefully before the PCI bus is
+ * probed and/or the 8250 driver loaded since we need to register our
+ * platform devices before 8250 PCI ones are detected as some of them
+ * must properly "override" the platform ones.
+ *
+ * This function fixes up the interrupt value for platform ports as it
+ * couldn't be done earlier before interrupt maps have been parsed. It
+ * also "corrects" the IO address for PIO ports for the same reason,
+ * since earlier, the PHBs virtual IO space wasn't assigned yet. It then
+ * registers all those platform ports for use by the 8250 driver when it
+ * finally loads.
+ */
+static int __init serial_dev_init(void)
+{
+	int i;
+
+	if (legacy_serial_count == 0)
+		return -ENODEV;
+
+	/*
+	 * Before we register the platfrom serial devices, we need
+	 * to fixup their interrutps and their IO ports.
+	 */
+	DBG("Fixing serial ports interrupts and IO ports ...\n");
+
+	for (i = 0; i < legacy_serial_count; i++) {
+		struct plat_serial8250_port *port = &legacy_serial_ports[i];
+		struct device_node *np = legacy_serial_infos[i].np;
+
+		if (port->irq == NO_IRQ)
+			fixup_port_irq(i, np, port);
+		if (port->iotype == UPIO_PORT)
+			fixup_port_pio(i, np, port);
+	}
+
+	DBG("Registering platform serial ports\n");
+
+	return platform_device_register(&serial_device);
+}
+arch_initcall(serial_dev_init);
+
+
+/*
+ * This is called very early, as part of console_init() (typically just after
+ * time_init()). This function is respondible for trying to find a good
+ * default console on serial ports. It tries to match the open firmware
+ * default output with one of the available serial console drivers, either
+ * one of the platform serial ports that have been probed earlier by
+ * find_legacy_serial_ports() or some more platform specific ones.
+ */
+static int __init check_legacy_serial_console(void)
+{
+	struct device_node *prom_stdout = NULL;
+	int speed = 0, offset = 0;
+	char *name;
+	u32 *spd;
+
+	DBG(" -> check_legacy_serial_console()\n");
+
+	/* The user has requested a console so this is already set up. */
+	if (strstr(saved_command_line, "console=")) {
+		DBG(" console was specified !\n");
+		return -EBUSY;
+	}
+
+	if (!of_chosen) {
+		DBG(" of_chosen is NULL !\n");
+		return -ENODEV;
+	}
+	/* We are getting a weird phandle from OF ... */
+	/* ... So use the full path instead */
+	name = (char *)get_property(of_chosen, "linux,stdout-path", NULL);
+	if (name == NULL) {
+		DBG(" no linux,stdout-path !\n");
+		return -ENODEV;
+	}
+	prom_stdout = of_find_node_by_path(name);
+	if (!prom_stdout) {
+		DBG(" can't find stdout package %s !\n", name);
+		return -ENODEV;
+	}
+	DBG("stdout is %s\n", prom_stdout->full_name);
+
+	name = (char *)get_property(prom_stdout, "name", NULL);
+	if (!name) {
+		DBG(" stdout package has no name !\n");
+		goto not_found;
+	}
+	spd = (u32 *)get_property(prom_stdout, "current-speed", NULL);
+	if (spd)
+		speed = *spd;
+
+	if (0)
+		;
+#ifdef CONFIG_SERIAL_8250_CONSOLE
+	else if (strcmp(name, "serial") == 0) {
+		int i;
+		/* Look for it in probed array */
+		for (i = 0; i < legacy_serial_count; i++) {
+			if (prom_stdout != legacy_serial_infos[i].np)
+				continue;
+			offset = i;
+			speed = legacy_serial_infos[i].speed;
+			break;
+		}
+		if (i >= legacy_serial_count)
+			goto not_found;
+	}
+#endif /* CONFIG_SERIAL_8250_CONSOLE */
+#ifdef CONFIG_SERIAL_PMACZILOG_CONSOLE
+	else if (strcmp(name, "ch-a") == 0)
+		offset = 0;
+	else if (strcmp(name, "ch-b") == 0)
+		offset = 1;
+#endif /* CONFIG_SERIAL_PMACZILOG_CONSOLE */
+	else
+		goto not_found;
+	of_node_put(prom_stdout);
+
+	DBG("Found serial console at ttyS%d\n", offset);
+
+	if (speed) {
+		static char __initdata opt[16];
+		sprintf(opt, "%d", speed);
+		return add_preferred_console("ttyS", offset, opt);
+	} else
+		return add_preferred_console("ttyS", offset, NULL);
+
+ not_found:
+	DBG("No preferred console found !\n");
+	of_node_put(prom_stdout);
+	return -ENODEV;
+}
+console_initcall(check_legacy_serial_console);
+
diff --git a/arch/powerpc/kernel/pci_64.c b/arch/powerpc/kernel/pci_64.c
index 8b6008ab217d..9a80cdf9efeb 100644
--- a/arch/powerpc/kernel/pci_64.c
+++ b/arch/powerpc/kernel/pci_64.c
@@ -1223,6 +1223,7 @@ void __devinit pcibios_fixup_device_resources(struct pci_dev *dev,
 }
 EXPORT_SYMBOL(pcibios_fixup_device_resources);
 
+
 static void __devinit do_bus_setup(struct pci_bus *bus)
 {
 	struct pci_dev *dev;
@@ -1306,6 +1307,20 @@ void pci_resource_to_user(const struct pci_dev *dev, int bar,
 	*end = rsrc->end + offset;
 }
 
+struct pci_controller* pci_find_hose_for_OF_device(struct device_node* node)
+{
+	if (!have_of)
+		return NULL;
+	while(node) {
+		struct pci_controller *hose, *tmp;
+		list_for_each_entry_safe(hose, tmp, &hose_list, list_node)
+			if (hose->arch_data == node)
+				return hose;
+		node = node->parent;
+	}
+	return NULL;
+}
+
 #endif /* CONFIG_PPC_MULTIPLATFORM */
 
 
diff --git a/arch/powerpc/kernel/setup-common.c b/arch/powerpc/kernel/setup-common.c
index 6088a39edc26..a6d8aebf2bc6 100644
--- a/arch/powerpc/kernel/setup-common.c
+++ b/arch/powerpc/kernel/setup-common.c
@@ -294,129 +294,6 @@ struct seq_operations cpuinfo_op = {
 	.show =	show_cpuinfo,
 };
 
-#ifdef CONFIG_PPC_MULTIPLATFORM
-static int __init set_preferred_console(void)
-{
-	struct device_node *prom_stdout = NULL;
-	char *name;
-	u32 *spd;
-	int offset = 0;
-
-	DBG(" -> set_preferred_console()\n");
-
-	/* The user has requested a console so this is already set up. */
-	if (strstr(saved_command_line, "console=")) {
-		DBG(" console was specified !\n");
-		return -EBUSY;
-	}
-
-	if (!of_chosen) {
-		DBG(" of_chosen is NULL !\n");
-		return -ENODEV;
-	}
-	/* We are getting a weird phandle from OF ... */
-	/* ... So use the full path instead */
-	name = (char *)get_property(of_chosen, "linux,stdout-path", NULL);
-	if (name == NULL) {
-		DBG(" no linux,stdout-path !\n");
-		return -ENODEV;
-	}
-	prom_stdout = of_find_node_by_path(name);
-	if (!prom_stdout) {
-		DBG(" can't find stdout package %s !\n", name);
-		return -ENODEV;
-	}	
-	DBG("stdout is %s\n", prom_stdout->full_name);
-
-	name = (char *)get_property(prom_stdout, "name", NULL);
-	if (!name) {
-		DBG(" stdout package has no name !\n");
-		goto not_found;
-	}
-	spd = (u32 *)get_property(prom_stdout, "current-speed", NULL);
-
-	if (0)
-		;
-#ifdef CONFIG_SERIAL_8250_CONSOLE
-	else if (strcmp(name, "serial") == 0) {
-		int i;
-		u32 *reg = (u32 *)get_property(prom_stdout, "reg", &i);
-		if (i > 8) {
-			switch (reg[1]) {
-				case 0x3f8:
-					offset = 0;
-					break;
-				case 0x2f8:
-					offset = 1;
-					break;
-				case 0x898:
-					offset = 2;
-					break;
-				case 0x890:
-					offset = 3;
-					break;
-				default:
-					/* We dont recognise the serial port */
-					goto not_found;
-			}
-		}
-	}
-#endif /* CONFIG_SERIAL_8250_CONSOLE */
-#ifdef CONFIG_PPC_PSERIES
-	else if (strcmp(name, "vty") == 0) {
- 		u32 *reg = (u32 *)get_property(prom_stdout, "reg", NULL);
- 		char *compat = (char *)get_property(prom_stdout, "compatible", NULL);
-
- 		if (reg && compat && (strcmp(compat, "hvterm-protocol") == 0)) {
- 			/* Host Virtual Serial Interface */
- 			switch (reg[0]) {
- 				case 0x30000000:
- 					offset = 0;
- 					break;
- 				case 0x30000001:
- 					offset = 1;
- 					break;
- 				default:
-					goto not_found;
- 			}
-			of_node_put(prom_stdout);
-			DBG("Found hvsi console at offset %d\n", offset);
- 			return add_preferred_console("hvsi", offset, NULL);
- 		} else {
- 			/* pSeries LPAR virtual console */
-			of_node_put(prom_stdout);
-			DBG("Found hvc console\n");
- 			return add_preferred_console("hvc", 0, NULL);
- 		}
-	}
-#endif /* CONFIG_PPC_PSERIES */
-#ifdef CONFIG_SERIAL_PMACZILOG_CONSOLE
-	else if (strcmp(name, "ch-a") == 0)
-		offset = 0;
-	else if (strcmp(name, "ch-b") == 0)
-		offset = 1;
-#endif /* CONFIG_SERIAL_PMACZILOG_CONSOLE */
-	else
-		goto not_found;
-	of_node_put(prom_stdout);
-
-	DBG("Found serial console at ttyS%d\n", offset);
-
-	if (spd) {
-		static char __initdata opt[16];
-		sprintf(opt, "%d", *spd);
-		return add_preferred_console("ttyS", offset, opt);
-	} else
-		return add_preferred_console("ttyS", offset, NULL);
-
- not_found:
-	DBG("No preferred console found !\n");
-	of_node_put(prom_stdout);
-	return -ENODEV;
-}
-console_initcall(set_preferred_console);
-#endif /* CONFIG_PPC_MULTIPLATFORM */
-
 void __init check_for_initrd(void)
 {
 #ifdef CONFIG_BLK_DEV_INITRD
diff --git a/arch/powerpc/kernel/setup_32.c b/arch/powerpc/kernel/setup_32.c
index e5694335bf10..02baacf04366 100644
--- a/arch/powerpc/kernel/setup_32.c
+++ b/arch/powerpc/kernel/setup_32.c
@@ -39,6 +39,7 @@
 #include <asm/nvram.h>
 #include <asm/xmon.h>
 #include <asm/time.h>
+#include <asm/serial.h>
 
 #include "setup.h"
 
@@ -282,6 +283,13 @@ void __init setup_arch(char **cmdline_p)
 
 	unflatten_device_tree();
 	check_for_initrd();
+
+	if (ppc_md.init_early)
+		ppc_md.init_early();
+
+#ifdef CONFIG_PPC_MULTIPLATFORM
+	find_legacy_serial_ports();
+#endif
 	finish_device_tree();
 
 	smp_setup_cpu_maps();
diff --git a/arch/powerpc/kernel/setup_64.c b/arch/powerpc/kernel/setup_64.c
index e3fb78397dc6..0fc442ad1d26 100644
--- a/arch/powerpc/kernel/setup_64.c
+++ b/arch/powerpc/kernel/setup_64.c
@@ -459,6 +459,15 @@ void __init setup_system(void)
 	 */
 	ppc_md.init_early();
 
+ 	/*
+	 * We can discover serial ports now since the above did setup the
+	 * hash table management for us, thus ioremap works. We do that early
+	 * so that further code can be debugged
+	 */
+#ifdef CONFIG_PPC_MULTIPLATFORM
+	find_legacy_serial_ports();
+#endif
+
 	/*
 	 * "Finish" the device-tree, that is do the actual parsing of
 	 * some of the properties like the interrupt map
@@ -657,187 +666,6 @@ void ppc64_terminate_msg(unsigned int src, const char *msg)
 	printk("[terminate]%04x %s\n", src, msg);
 }
 
-#ifndef CONFIG_PPC_ISERIES
-/*
- * This function can be used by platforms to "find" legacy serial ports.
- * It works for "serial" nodes under an "isa" node, and will try to
- * respect the "ibm,aix-loc" property if any. It works with up to 8
- * ports.
- */
-
-#define MAX_LEGACY_SERIAL_PORTS	8
-static struct plat_serial8250_port serial_ports[MAX_LEGACY_SERIAL_PORTS+1];
-static unsigned int old_serial_count;
-
-void __init generic_find_legacy_serial_ports(u64 *physport,
-		unsigned int *default_speed)
-{
-	struct device_node *np;
-	u32 *sizeprop;
-
-	struct isa_reg_property {
-		u32 space;
-		u32 address;
-		u32 size;
-	};
-	struct pci_reg_property {
-		struct pci_address addr;
-		u32 size_hi;
-		u32 size_lo;
-	};                                                                        
-
-	DBG(" -> generic_find_legacy_serial_port()\n");
-
-	*physport = 0;
-	if (default_speed)
-		*default_speed = 0;
-
-	np = of_find_node_by_path("/");
-	if (!np)
-		return;
-
-	/* First fill our array */
-	for (np = NULL; (np = of_find_node_by_type(np, "serial"));) {
-		struct device_node *isa, *pci;
-		struct isa_reg_property *reg;
-		unsigned long phys_size, addr_size, io_base;
-		u32 *rangesp;
-		u32 *interrupts, *clk, *spd;
-		char *typep;
-		int index, rlen, rentsize;
-
-		/* Ok, first check if it's under an "isa" parent */
-		isa = of_get_parent(np);
-		if (!isa || strcmp(isa->name, "isa")) {
-			DBG("%s: no isa parent found\n", np->full_name);
-			continue;
-		}
-		
-		/* Now look for an "ibm,aix-loc" property that gives us ordering
-		 * if any...
-		 */
-	 	typep = (char *)get_property(np, "ibm,aix-loc", NULL);
-
-		/* Get the ISA port number */
-		reg = (struct isa_reg_property *)get_property(np, "reg", NULL);	
-		if (reg == NULL)
-			goto next_port;
-		/* We assume the interrupt number isn't translated ... */
-		interrupts = (u32 *)get_property(np, "interrupts", NULL);
-		/* get clock freq. if present */
-		clk = (u32 *)get_property(np, "clock-frequency", NULL);
-		/* get default speed if present */
-		spd = (u32 *)get_property(np, "current-speed", NULL);
-		/* Default to locate at end of array */
-		index = old_serial_count; /* end of the array by default */
-
-		/* If we have a location index, then use it */
-		if (typep && *typep == 'S') {
-			index = simple_strtol(typep+1, NULL, 0) - 1;
-			/* if index is out of range, use end of array instead */
-			if (index >= MAX_LEGACY_SERIAL_PORTS)
-				index = old_serial_count;
-			/* if our index is still out of range, that mean that
-			 * array is full, we could scan for a free slot but that
-			 * make little sense to bother, just skip the port
-			 */
-			if (index >= MAX_LEGACY_SERIAL_PORTS)
-				goto next_port;
-			if (index >= old_serial_count)
-				old_serial_count = index + 1;
-			/* Check if there is a port who already claimed our slot */
-			if (serial_ports[index].iobase != 0) {
-				/* if we still have some room, move it, else override */
-				if (old_serial_count < MAX_LEGACY_SERIAL_PORTS) {
-					DBG("Moved legacy port %d -> %d\n", index,
-					    old_serial_count);
-					serial_ports[old_serial_count++] =
-						serial_ports[index];
-				} else {
-					DBG("Replacing legacy port %d\n", index);
-				}
-			}
-		}
-		if (index >= MAX_LEGACY_SERIAL_PORTS)
-			goto next_port;
-		if (index >= old_serial_count)
-			old_serial_count = index + 1;
-
-		/* Now fill the entry */
-		memset(&serial_ports[index], 0, sizeof(struct plat_serial8250_port));
-		serial_ports[index].uartclk = clk ? *clk : BASE_BAUD * 16;
-		serial_ports[index].iobase = reg->address;
-		serial_ports[index].irq = interrupts ? interrupts[0] : 0;
-		serial_ports[index].flags = ASYNC_BOOT_AUTOCONF;
-
-		DBG("Added legacy port, index: %d, port: %x, irq: %d, clk: %d\n",
-		    index,
-		    serial_ports[index].iobase,
-		    serial_ports[index].irq,
-		    serial_ports[index].uartclk);
-
-		/* Get phys address of IO reg for port 1 */
-		if (index != 0)
-			goto next_port;
-
-		pci = of_get_parent(isa);
-		if (!pci) {
-			DBG("%s: no pci parent found\n", np->full_name);
-			goto next_port;
-		}
-
-		rangesp = (u32 *)get_property(pci, "ranges", &rlen);
-		if (rangesp == NULL) {
-			of_node_put(pci);
-			goto next_port;
-		}
-		rlen /= 4;
-
-		/* we need the #size-cells of the PCI bridge node itself */
-		phys_size = 1;
-		sizeprop = (u32 *)get_property(pci, "#size-cells", NULL);
-		if (sizeprop != NULL)
-			phys_size = *sizeprop;
-		/* we need the parent #addr-cells */
-		addr_size = prom_n_addr_cells(pci);
-		rentsize = 3 + addr_size + phys_size;
-		io_base = 0;
-		for (;rlen >= rentsize; rlen -= rentsize,rangesp += rentsize) {
-			if (((rangesp[0] >> 24) & 0x3) != 1)
-				continue; /* not IO space */
-			io_base = rangesp[3];
-			if (addr_size == 2)
-				io_base = (io_base << 32) | rangesp[4];
-		}
-		if (io_base != 0) {
-			*physport = io_base + reg->address;
-			if (default_speed && spd)
-				*default_speed = *spd;
-		}
-		of_node_put(pci);
-	next_port:
-		of_node_put(isa);
-	}
-
-	DBG(" <- generic_find_legacy_serial_port()\n");
-}
-
-static struct platform_device serial_device = {
-	.name	= "serial8250",
-	.id	= PLAT8250_DEV_PLATFORM,
-	.dev	= {
-		.platform_data = serial_ports,
-	},
-};
-
-static int __init serial_dev_init(void)
-{
-	return platform_device_register(&serial_device);
-}
-arch_initcall(serial_dev_init);
-
-#endif /* CONFIG_PPC_ISERIES */
-
 int check_legacy_ioport(unsigned long base_port)
 {
 	if (ppc_md.check_legacy_ioport == NULL)
diff --git a/arch/powerpc/kernel/udbg.c b/arch/powerpc/kernel/udbg.c
index 0d878e72fc44..2e372477d22a 100644
--- a/arch/powerpc/kernel/udbg.c
+++ b/arch/powerpc/kernel/udbg.c
@@ -99,7 +99,7 @@ static void udbg_console_write(struct console *con, const char *s,
 static struct console udbg_console = {
 	.name	= "udbg",
 	.write	= udbg_console_write,
-	.flags	= CON_PRINTBUFFER,
+	.flags	= CON_PRINTBUFFER | CON_ENABLED,
 	.index	= -1,
 };
 
diff --git a/arch/powerpc/kernel/udbg_16550.c b/arch/powerpc/kernel/udbg_16550.c
index 9313574ab935..50fd376446c9 100644
--- a/arch/powerpc/kernel/udbg_16550.c
+++ b/arch/powerpc/kernel/udbg_16550.c
@@ -43,6 +43,8 @@ struct NS16550 {
 #define LSR_TEMT 0x40  /* Xmitter empty */
 #define LSR_ERR  0x80  /* Error */
 
+#define LCR_DLAB 0x80
+
 static volatile struct NS16550 __iomem *udbg_comport;
 
 static void udbg_550_putc(unsigned char c)
@@ -77,29 +79,70 @@ static unsigned char udbg_550_getc(void)
 	return 0;
 }
 
-void udbg_init_uart(void __iomem *comport, unsigned int speed)
+void udbg_init_uart(void __iomem *comport, unsigned int speed,
+		    unsigned int clock)
 {
-	u16 dll = speed ? (115200 / speed) : 12;
+	unsigned int dll, base_bauds = clock / 16;
+
+	if (speed == 0)
+		speed = 9600;
+	dll = base_bauds / speed;
 
 	if (comport) {
 		udbg_comport = (struct NS16550 __iomem *)comport;
 		out_8(&udbg_comport->lcr, 0x00);
 		out_8(&udbg_comport->ier, 0xff);
 		out_8(&udbg_comport->ier, 0x00);
-		out_8(&udbg_comport->lcr, 0x80);	/* Access baud rate */
-		out_8(&udbg_comport->dll, dll & 0xff);	/* 1 = 115200,  2 = 57600,
-							   3 = 38400, 12 = 9600 baud */
-		out_8(&udbg_comport->dlm, dll >> 8);	/* dll >> 8 which should be zero
-							   for fast rates; */
-		out_8(&udbg_comport->lcr, 0x03);	/* 8 data, 1 stop, no parity */
-		out_8(&udbg_comport->mcr, 0x03);	/* RTS/DTR */
-		out_8(&udbg_comport->fcr ,0x07);	/* Clear & enable FIFOs */
+		out_8(&udbg_comport->lcr, LCR_DLAB);
+		out_8(&udbg_comport->dll, dll & 0xff);
+		out_8(&udbg_comport->dlm, dll >> 8);
+		/* 8 data, 1 stop, no parity */
+		out_8(&udbg_comport->lcr, 0x03);
+		/* RTS/DTR */
+		out_8(&udbg_comport->mcr, 0x03);
+		/* Clear & enable FIFOs */
+		out_8(&udbg_comport->fcr ,0x07);
 		udbg_putc = udbg_550_putc;
 		udbg_getc = udbg_550_getc;
 		udbg_getc_poll = udbg_550_getc_poll;
 	}
 }
 
+unsigned int udbg_probe_uart_speed(void __iomem *comport, unsigned int clock)
+{
+	unsigned int dll, dlm, divisor, prescaler, speed;
+	u8 old_lcr;
+	volatile struct NS16550 __iomem *port = comport;
+
+	old_lcr = in_8(&port->lcr);
+
+	/* select divisor latch registers.  */
+	out_8(&port->lcr, LCR_DLAB);
+
+	/* now, read the divisor */
+	dll = in_8(&port->dll);
+	dlm = in_8(&port->dlm);
+	divisor = dlm << 8 | dll;
+
+	/* check prescaling */
+	if (in_8(&port->mcr) & 0x80)
+		prescaler = 4;
+	else
+		prescaler = 1;
+
+	/* restore the LCR */
+	out_8(&port->lcr, old_lcr);
+
+	/* calculate speed */
+	speed = (clock / prescaler) / (divisor * 16);
+
+	/* sanity check */
+	if (speed < 9600 || speed > 115200)
+		speed = 9600;
+
+	return speed;
+}
+
 #ifdef CONFIG_PPC_MAPLE
 void udbg_maple_real_putc(unsigned char c)
 {
diff --git a/arch/powerpc/platforms/maple/setup.c b/arch/powerpc/platforms/maple/setup.c
index 95b2352655fe..8724e031e965 100644
--- a/arch/powerpc/platforms/maple/setup.c
+++ b/arch/powerpc/platforms/maple/setup.c
@@ -192,20 +192,6 @@ static void __init maple_init_early(void)
 	 */
 	hpte_init_native();
 
-	/* Find the serial port */
-	generic_find_legacy_serial_ports(&physport, &default_speed);
-
-	DBG("phys port addr: %lx\n", (long)physport);
-
-	if (physport) {
-		void *comport;
-		/* Map the uart for udbg. */
-		comport = (void *)ioremap(physport, 16);
-		udbg_init_uart(comport, default_speed);
-
-		DBG("Hello World !\n");
-	}
-
 	/* Setup interrupt mapping options */
 	ppc64_interrupt_controller = IC_OPEN_PIC;
 
diff --git a/arch/powerpc/platforms/pseries/lpar.c b/arch/powerpc/platforms/pseries/lpar.c
index cf1bc11b3346..cc0939d4cad1 100644
--- a/arch/powerpc/platforms/pseries/lpar.c
+++ b/arch/powerpc/platforms/pseries/lpar.c
@@ -24,6 +24,7 @@
 #include <linux/config.h>
 #include <linux/kernel.h>
 #include <linux/dma-mapping.h>
+#include <linux/console.h>
 #include <asm/processor.h>
 #include <asm/mmu.h>
 #include <asm/page.h>
@@ -191,7 +192,7 @@ static unsigned char udbg_getcLP(void)
 /* call this from early_init() for a working debug console on
  * vterm capable LPAR machines
  */
-void udbg_init_debug_lpar(void)
+void __init udbg_init_debug_lpar(void)
 {
 	vtermno = 0;
 	udbg_putc = udbg_putcLP;
@@ -200,63 +201,54 @@ void udbg_init_debug_lpar(void)
 }
 
 /* returns 0 if couldn't find or use /chosen/stdout as console */
-int find_udbg_vterm(void)
+void __init find_udbg_vterm(void)
 {
 	struct device_node *stdout_node;
 	u32 *termno;
 	char *name;
-	int found = 0;
+	int add_console;
 
 	/* find the boot console from /chosen/stdout */
 	if (!of_chosen)
-		return 0;
+		return;
 	name = (char *)get_property(of_chosen, "linux,stdout-path", NULL);
 	if (name == NULL)
-		return 0;
+		return;
 	stdout_node = of_find_node_by_path(name);
 	if (!stdout_node)
-		return 0;
-
-	/* now we have the stdout node; figure out what type of device it is. */
+		return;
 	name = (char *)get_property(stdout_node, "name", NULL);
 	if (!name) {
 		printk(KERN_WARNING "stdout node missing 'name' property!\n");
 		goto out;
 	}
+	/* The user has requested a console so this is already set up. */
+	add_console = !strstr(cmd_line, "console=");
 
-	if (strncmp(name, "vty", 3) == 0) {
-		if (device_is_compatible(stdout_node, "hvterm1")) {
-			termno = (u32 *)get_property(stdout_node, "reg", NULL);
-			if (termno) {
-				vtermno = termno[0];
-				udbg_putc = udbg_putcLP;
-				udbg_getc = udbg_getcLP;
-				udbg_getc_poll = udbg_getc_pollLP;
-				found = 1;
-			}
-		} else if (device_is_compatible(stdout_node, "hvterm-protocol")) {
-			termno = (u32 *)get_property(stdout_node, "reg", NULL);
-			if (termno) {
-				vtermno = termno[0];
-				udbg_putc = udbg_hvsi_putc;
-				udbg_getc = udbg_hvsi_getc;
-				udbg_getc_poll = udbg_hvsi_getc_poll;
-				found = 1;
-			}
-		}
-	} else if (strncmp(name, "serial", 6)) {
-		/* XXX fix ISA serial console */
-		printk(KERN_WARNING "serial stdout on LPAR ('%s')! "
-				"can't print udbg messages\n",
-		       stdout_node->full_name);
-	} else {
-		printk(KERN_WARNING "don't know how to print to stdout '%s'\n",
-		       stdout_node->full_name);
+	/* Check if it's a virtual terminal */
+	if (strncmp(name, "vty", 3) != 0)
+		goto out;
+	termno = (u32 *)get_property(stdout_node, "reg", NULL);
+	if (termno == NULL)
+		goto out;
+	vtermno = termno[0];
+
+	if (device_is_compatible(stdout_node, "hvterm1")) {
+		udbg_putc = udbg_putcLP;
+		udbg_getc = udbg_getcLP;
+		udbg_getc_poll = udbg_getc_pollLP;
+		if (add_console)
+			add_preferred_console("hvc", termno[0] & 0xff, NULL);
+	} else if (device_is_compatible(stdout_node, "hvterm-protocol")) {
+		vtermno = termno[0];
+		udbg_putc = udbg_hvsi_putc;
+		udbg_getc = udbg_hvsi_getc;
+		udbg_getc_poll = udbg_hvsi_getc_poll;
+		if (add_console)
+			add_preferred_console("hvsi", termno[0] & 0xff, NULL);
 	}
-
 out:
 	of_node_put(stdout_node);
-	return found;
 }
 
 void vpa_init(int cpu)
diff --git a/arch/powerpc/platforms/pseries/setup.c b/arch/powerpc/platforms/pseries/setup.c
index 8a4238a3757f..8828dc378c3e 100644
--- a/arch/powerpc/platforms/pseries/setup.c
+++ b/arch/powerpc/platforms/pseries/setup.c
@@ -79,8 +79,6 @@
 extern void find_udbg_vterm(void);
 extern void system_reset_fwnmi(void);	/* from head.S */
 extern void machine_check_fwnmi(void);	/* from head.S */
-extern void generic_find_legacy_serial_ports(u64 *physport,
-		unsigned int *default_speed);
 
 int fwnmi_active;  /* TRUE if an FWNMI handler is present */
 
@@ -366,10 +364,7 @@ static int pseries_set_xdabr(unsigned long dabr)
  */
 static void __init pSeries_init_early(void)
 {
-	void *comport;
 	int iommu_off = 0;
-	unsigned int default_speed;
-	u64 physport;
 
 	DBG(" -> pSeries_init_early()\n");
 
@@ -383,17 +378,8 @@ static void __init pSeries_init_early(void)
 			     get_property(of_chosen, "linux,iommu-off", NULL));
 	}
 
-	generic_find_legacy_serial_ports(&physport, &default_speed);
-
 	if (platform_is_lpar())
 		find_udbg_vterm();
-	else if (physport) {
-		/* Map the uart for udbg. */
-		comport = (void *)ioremap(physport, 16);
-		udbg_init_uart(comport, default_speed);
-
-		DBG("Hello World !\n");
-	}
 
 	if (firmware_has_feature(FW_FEATURE_DABR))
 		ppc_md.set_dabr = pseries_set_dabr;
diff --git a/arch/ppc/kernel/pci.c b/arch/ppc/kernel/pci.c
index f7fae5f153b2..0aa184112fb1 100644
--- a/arch/ppc/kernel/pci.c
+++ b/arch/ppc/kernel/pci.c
@@ -815,8 +815,7 @@ EXPORT_SYMBOL(pci_device_to_OF_node);
  * to set pci_assign_all_buses to 1 and still use RTAS for PCI
  * config cycles.
  */
-struct pci_controller*
-pci_find_hose_for_OF_device(struct device_node* node)
+struct pci_controller* pci_find_hose_for_OF_device(struct device_node* node)
 {
 	if (!have_of)
 		return NULL;
diff --git a/include/asm-powerpc/pci-bridge.h b/include/asm-powerpc/pci-bridge.h
index 223ec7bd81da..89e73fcd58bb 100644
--- a/include/asm-powerpc/pci-bridge.h
+++ b/include/asm-powerpc/pci-bridge.h
@@ -140,6 +140,9 @@ static inline struct pci_controller *pci_bus_to_host(struct pci_bus *bus)
 	return PCI_DN(busdn)->phb;
 }
 
+extern struct pci_controller*
+pci_find_hose_for_OF_device(struct device_node* node);
+
 extern struct pci_controller *
 pcibios_alloc_controller(struct device_node *dev);
 extern void pcibios_free_controller(struct pci_controller *phb);
diff --git a/include/asm-powerpc/serial.h b/include/asm-powerpc/serial.h
index b273d630b32f..6dc9546d6908 100644
--- a/include/asm-powerpc/serial.h
+++ b/include/asm-powerpc/serial.h
@@ -15,4 +15,6 @@
 /* Default baud base if not found in device-tree */
 #define BASE_BAUD ( 1843200 / 16 )
 
+extern void find_legacy_serial_ports(void);
+
 #endif /* _PPC64_SERIAL_H */
diff --git a/include/asm-powerpc/udbg.h b/include/asm-powerpc/udbg.h
index a383383bc4d4..4049a96dc43d 100644
--- a/include/asm-powerpc/udbg.h
+++ b/include/asm-powerpc/udbg.h
@@ -24,7 +24,10 @@ extern int udbg_read(char *buf, int buflen);
 extern void register_early_udbg_console(void);
 extern void udbg_printf(const char *fmt, ...);
 
-extern void udbg_init_uart(void __iomem *comport, unsigned int speed);
+extern void udbg_init_uart(void __iomem *comport, unsigned int speed,
+			   unsigned int clock);
+extern unsigned int udbg_probe_uart_speed(void __iomem *comport,
+					  unsigned int clock);
 
 struct device_node;
 extern void udbg_init_scc(struct device_node *np);