3 files changed, 5 insertions, 192 deletions

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