6 files changed, 24 insertions, 192 deletions

diff --git a/arch/powerpc/Kconfig.debug b/arch/powerpc/Kconfig.debug
index e72dcf6a421d..067cb8480747 100644
--- a/arch/powerpc/Kconfig.debug
+++ b/arch/powerpc/Kconfig.debug
@@ -167,6 +167,13 @@ config PPC_EARLY_DEBUG_LPAR
           Select this to enable early debugging for a machine with a HVC
           console on vterm 0.
 
+config PPC_EARLY_DEBUG_LPAR_HVSI
+        bool "LPAR HVSI Console"
+        depends on PPC_PSERIES
+        help
+          Select this to enable early debugging for a machine with a HVSI
+          console on a specified vterm.
+
 config PPC_EARLY_DEBUG_G5
         bool "Apple G5"
         depends on PPC_PMAC64
@@ -253,6 +260,14 @@ config PPC_EARLY_DEBUG_WSP
 
 endchoice
 
+config PPC_EARLY_DEBUG_HVSI_VTERMNO
+        hex "vterm number to use with early debug HVSI"
+        depends on PPC_EARLY_DEBUG_LPAR_HVSI
+        default "0x30000000"
+        help
+          You probably want 0x30000000 for your first serial port and
+          0x30000001 for your second one
+
 config PPC_EARLY_DEBUG_44x_PHYSLOW
         hex "Low 32 bits of early debug UART physical address"
         depends on PPC_EARLY_DEBUG_44x
diff --git a/arch/powerpc/include/asm/udbg.h b/arch/powerpc/include/asm/udbg.h
index 58580e94a2bb..93e05d1b34b2 100644
--- a/arch/powerpc/include/asm/udbg.h
+++ b/arch/powerpc/include/asm/udbg.h
@@ -40,6 +40,7 @@ extern void udbg_adb_init_early(void);
 
 extern void __init udbg_early_init(void);
 extern void __init udbg_init_debug_lpar(void);
+extern void __init udbg_init_debug_lpar_hvsi(void);
 extern void __init udbg_init_pmac_realmode(void);
 extern void __init udbg_init_maple_realmode(void);
 extern void __init udbg_init_pas_realmode(void);
diff --git a/arch/powerpc/kernel/udbg.c b/arch/powerpc/kernel/udbg.c
index a57e61ea0558..faa82c1f3f68 100644
--- a/arch/powerpc/kernel/udbg.c
+++ b/arch/powerpc/kernel/udbg.c
@@ -31,6 +31,9 @@ void __init udbg_early_init(void)
 #if defined(CONFIG_PPC_EARLY_DEBUG_LPAR)
         /* For LPAR machines that have an HVC console on vterm 0 */
         udbg_init_debug_lpar();
+#elif defined(CONFIG_PPC_EARLY_DEBUG_LPAR_HVSI)
+        /* For LPAR machines that have an HVSI console on vterm 0 */
+        udbg_init_debug_lpar_hvsi();
 #elif defined(CONFIG_PPC_EARLY_DEBUG_G5)
         /* For use on Apple G5 machines */
         udbg_init_pmac_realmode();
diff --git a/arch/powerpc/platforms/pseries/lpar.c b/arch/powerpc/platforms/pseries/lpar.c
index e3a96c4348ab..f7205d344efd 100644
--- a/arch/powerpc/platforms/pseries/lpar.c
+++ b/arch/powerpc/platforms/pseries/lpar.c
@@ -52,195 +52,6 @@ EXPORT_SYMBOL(plpar_hcall_norets);
 
 extern void pSeries_find_serial_port(void);
 
-
-static int vtermno;     /* virtual terminal# for udbg  */
-
-#define __ALIGNED__ __attribute__((__aligned__(sizeof(long))))
-static void udbg_hvsi_putc(char c)
-{
-        /* packet's seqno isn't used anyways */
-        uint8_t packet[] __ALIGNED__ = { 0xff, 5, 0, 0, c };
-        int rc;
-
-        if (c == '\n')
-                udbg_hvsi_putc('\r');
-
-        do {
-                rc = plpar_put_term_char(vtermno, sizeof(packet), packet);
-        } while (rc == H_BUSY);
-}
-
-static long hvsi_udbg_buf_len;
-static uint8_t hvsi_udbg_buf[256];
-
-static int udbg_hvsi_getc_poll(void)
-{
-        unsigned char ch;
-        int rc, i;
-
-        if (hvsi_udbg_buf_len == 0) {
-                rc = plpar_get_term_char(vtermno, &hvsi_udbg_buf_len, hvsi_udbg_buf);
-                if (rc != H_SUCCESS || hvsi_udbg_buf[0] != 0xff) {
-                        /* bad read or non-data packet */
-                        hvsi_udbg_buf_len = 0;
-                } else {
-                        /* remove the packet header */
-                        for (i = 4; i < hvsi_udbg_buf_len; i++)
-                                hvsi_udbg_buf[i-4] = hvsi_udbg_buf[i];
-                        hvsi_udbg_buf_len -= 4;
-                }
-        }
-
-        if (hvsi_udbg_buf_len <= 0 || hvsi_udbg_buf_len > 256) {
-                /* no data ready */
-                hvsi_udbg_buf_len = 0;
-                return -1;
-        }
-
-        ch = hvsi_udbg_buf[0];
-        /* shift remaining data down */
-        for (i = 1; i < hvsi_udbg_buf_len; i++) {
-                hvsi_udbg_buf[i-1] = hvsi_udbg_buf[i];
-        }
-        hvsi_udbg_buf_len--;
-
-        return ch;
-}
-
-static int udbg_hvsi_getc(void)
-{
-        int ch;
-        for (;;) {
-                ch = udbg_hvsi_getc_poll();
-                if (ch == -1) {
-                        /* This shouldn't be needed...but... */
-                        volatile unsigned long delay;
-                        for (delay=0; delay < 2000000; delay++)
-                                ;
-                } else {
-                        return ch;
-                }
-        }
-}
-
-static void udbg_putcLP(char c)
-{
-        char buf[16];
-        unsigned long rc;
-
-        if (c == '\n')
-                udbg_putcLP('\r');
-
-        buf[0] = c;
-        do {
-                rc = plpar_put_term_char(vtermno, 1, buf);
-        } while(rc == H_BUSY);
-}
-
-/* Buffered chars getc */
-static long inbuflen;
-static long inbuf[2];   /* must be 2 longs */
-
-static int udbg_getc_pollLP(void)
-{
-        /* The interface is tricky because it may return up to 16 chars.
-         * We save them statically for future calls to udbg_getc().
-         */
-        char ch, *buf = (char *)inbuf;
-        int i;
-        long rc;
-        if (inbuflen == 0) {
-                /* get some more chars. */
-                inbuflen = 0;
-                rc = plpar_get_term_char(vtermno, &inbuflen, buf);
-                if (rc != H_SUCCESS)
-                        inbuflen = 0;   /* otherwise inbuflen is garbage */
-        }
-        if (inbuflen <= 0 || inbuflen > 16) {
-                /* Catch error case as well as other oddities (corruption) */
-                inbuflen = 0;
-                return -1;
-        }
-        ch = buf[0];
-        for (i = 1; i < inbuflen; i++)  /* shuffle them down. */
-                buf[i-1] = buf[i];
-        inbuflen--;
-        return ch;
-}
-
-static int udbg_getcLP(void)
-{
-        int ch;
-        for (;;) {
-                ch = udbg_getc_pollLP();
-                if (ch == -1) {
-                        /* This shouldn't be needed...but... */
-                        volatile unsigned long delay;
-                        for (delay=0; delay < 2000000; delay++)
-                                ;
-                } else {
-                        return ch;
-                }
-        }
-}
-
-/* call this from early_init() for a working debug console on
- * vterm capable LPAR machines
- */
-void __init udbg_init_debug_lpar(void)
-{
-        vtermno = 0;
-        udbg_putc = udbg_putcLP;
-        udbg_getc = udbg_getcLP;
-        udbg_getc_poll = udbg_getc_pollLP;
-}
-
-/* returns 0 if couldn't find or use /chosen/stdout as console */
-void __init find_udbg_vterm(void)
-{
-        struct device_node *stdout_node;
-        const u32 *termno;
-        const char *name;
-
-        /* find the boot console from /chosen/stdout */
-        if (!of_chosen)
-                return;
-        name = of_get_property(of_chosen, "linux,stdout-path", NULL);
-        if (name == NULL)
-                return;
-        stdout_node = of_find_node_by_path(name);
-        if (!stdout_node)
-                return;
-        name = of_get_property(stdout_node, "name", NULL);
-        if (!name) {
-                printk(KERN_WARNING "stdout node missing 'name' property!\n");
-                goto out;
-        }
-
-        /* Check if it's a virtual terminal */
-        if (strncmp(name, "vty", 3) != 0)
-                goto out;
-        termno = of_get_property(stdout_node, "reg", NULL);
-        if (termno == NULL)
-                goto out;
-        vtermno = termno[0];
-
-        if (of_device_is_compatible(stdout_node, "hvterm1")) {
-                udbg_putc = udbg_putcLP;
-                udbg_getc = udbg_getcLP;
-                udbg_getc_poll = udbg_getc_pollLP;
-                add_preferred_console("hvc", termno[0] & 0xff, NULL);
-        } else if (of_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;
-                add_preferred_console("hvsi", termno[0] & 0xff, NULL);
-        }
-out:
-        of_node_put(stdout_node);
-}
-
 void vpa_init(int cpu)
 {
         int hwcpu = get_hard_smp_processor_id(cpu);
diff --git a/arch/powerpc/platforms/pseries/pseries.h b/arch/powerpc/platforms/pseries/pseries.h
index e9f6d2859c3c..24c7162f11d9 100644
--- a/arch/powerpc/platforms/pseries/pseries.h
+++ b/arch/powerpc/platforms/pseries/pseries.h
@@ -47,7 +47,8 @@ extern void pSeries_final_fixup(void);
 /* Poweron flag used for enabling auto ups restart */
 extern unsigned long rtas_poweron_auto;
 
-extern void find_udbg_vterm(void);
+/* Provided by HVC VIO */
+extern void hvc_vio_init_early(void);
 
 /* Dynamic logical Partitioning/Mobility */
 extern void dlpar_free_cc_nodes(struct device_node *);
diff --git a/arch/powerpc/platforms/pseries/setup.c b/arch/powerpc/platforms/pseries/setup.c
index 593acceeff96..d00e52926b71 100644
--- a/arch/powerpc/platforms/pseries/setup.c
+++ b/arch/powerpc/platforms/pseries/setup.c
@@ -512,9 +512,10 @@ static void __init pSeries_init_early(void)
 {
         pr_debug(" -> pSeries_init_early()\n");
 
+#ifdef CONFIG_HVC_CONSOLE
         if (firmware_has_feature(FW_FEATURE_LPAR))
-                find_udbg_vterm();
-
+                hvc_vio_init_early();
+#endif
         if (firmware_has_feature(FW_FEATURE_DABR))
                 ppc_md.set_dabr = pseries_set_dabr;
         else if (firmware_has_feature(FW_FEATURE_XDABR))