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);