2 files changed, 96 insertions, 60 deletions

diff --git a/drivers/tty/hvc/hvc_console.c b/drivers/tty/hvc/hvc_console.c
index 4a652999380f..a5dec1ca1b82 100644
--- a/drivers/tty/hvc/hvc_console.c
+++ b/drivers/tty/hvc/hvc_console.c
@@ -245,6 +245,20 @@ static void hvc_port_destruct(struct tty_port *port)
         kfree(hp);
 }
 
+static void hvc_check_console(int index)
+{
+        /* Already enabled, bail out */
+        if (hvc_console.flags & CON_ENABLED)
+                return;
+
+        /* If this index is what the user requested, then register
+         * now (setup won't fail at this point).  It's ok to just
+         * call register again if previously .setup failed.
+         */
+        if (index == hvc_console.index)
+                register_console(&hvc_console);
+}
+
 /*
  * hvc_instantiate() is an early console discovery method which locates
  * consoles * prior to the vio subsystem discovering them.  Hotplugged
@@ -275,12 +289,8 @@ int hvc_instantiate(uint32_t vtermno, int index, const struct hv_ops *ops)
         if (last_hvc < index)
                 last_hvc = index;
 
-        /* if this index is what the user requested, then register
-         * now (setup won't fail at this point).  It's ok to just
-         * call register again if previously .setup failed.
-         */
-        if (index == hvc_console.index)
-                register_console(&hvc_console);
+        /* check if we need to re-register the kernel console */
+        hvc_check_console(index);
 
         return 0;
 }
@@ -877,10 +887,15 @@ struct hvc_struct *hvc_alloc(uint32_t vtermno, int data,
                 i = ++last_hvc;
 
         hp->index = i;
+        cons_ops[i] = ops;
+        vtermnos[i] = vtermno;
 
         list_add_tail(&(hp->next), &hvc_structs);
         spin_unlock(&hvc_structs_lock);
 
+        /* check if we need to re-register the kernel console */
+        hvc_check_console(i);
+
         return hp;
 }
 EXPORT_SYMBOL_GPL(hvc_alloc);
@@ -893,8 +908,12 @@ int hvc_remove(struct hvc_struct *hp)
         tty = tty_port_tty_get(&hp->port);
 
         spin_lock_irqsave(&hp->lock, flags);
-        if (hp->index < MAX_NR_HVC_CONSOLES)
+        if (hp->index < MAX_NR_HVC_CONSOLES) {
+                console_lock();
                 vtermnos[hp->index] = -1;
+                cons_ops[hp->index] = NULL;
+                console_unlock();
+        }
 
         /* Don't whack hp->irq because tty_hangup() will need to free the irq. */
 
diff --git a/drivers/tty/hvc/hvc_vio.c b/drivers/tty/hvc/hvc_vio.c
index ee307799271a..070c0ee68642 100644
--- a/drivers/tty/hvc/hvc_vio.c
+++ b/drivers/tty/hvc/hvc_vio.c
@@ -230,6 +230,69 @@ static const struct hv_ops hvterm_hvsi_ops = {
         .tiocmset = hvterm_hvsi_tiocmset,
 };
 
+static void udbg_hvc_putc(char c)
+{
+        int count = -1;
+
+        if (!hvterm_privs[0])
+                return;
+
+        if (c == '\n')
+                udbg_hvc_putc('\r');
+
+        do {
+                switch(hvterm_privs[0]->proto) {
+                case HV_PROTOCOL_RAW:
+                        count = hvterm_raw_put_chars(0, &c, 1);
+                        break;
+                case HV_PROTOCOL_HVSI:
+                        count = hvterm_hvsi_put_chars(0, &c, 1);
+                        break;
+                }
+        } while(count == 0);
+}
+
+static int udbg_hvc_getc_poll(void)
+{
+        int rc = 0;
+        char c;
+
+        if (!hvterm_privs[0])
+                return -1;
+
+        switch(hvterm_privs[0]->proto) {
+        case HV_PROTOCOL_RAW:
+                rc = hvterm_raw_get_chars(0, &c, 1);
+                break;
+        case HV_PROTOCOL_HVSI:
+                rc = hvterm_hvsi_get_chars(0, &c, 1);
+                break;
+        }
+        if (!rc)
+                return -1;
+        return c;
+}
+
+static int udbg_hvc_getc(void)
+{
+        int ch;
+
+        if (!hvterm_privs[0])
+                return -1;
+
+        for (;;) {
+                ch = udbg_hvc_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 int __devinit hvc_vio_probe(struct vio_dev *vdev,
                                    const struct vio_device_id *id)
 {
@@ -289,6 +352,13 @@ static int __devinit hvc_vio_probe(struct vio_dev *vdev,
                 return PTR_ERR(hp);
         dev_set_drvdata(&vdev->dev, hp);
 
+        /* register udbg if it's not there already for console 0 */
+        if (hp->index == 0 && !udbg_putc) {
+                udbg_putc = udbg_hvc_putc;
+                udbg_getc = udbg_hvc_getc;
+                udbg_getc_poll = udbg_hvc_getc_poll;
+        }
+
         return 0;
 }
 
@@ -331,59 +401,6 @@ static void __exit hvc_vio_exit(void)
 }
 module_exit(hvc_vio_exit);
 
-static void udbg_hvc_putc(char c)
-{
-        int count = -1;
-
-        if (c == '\n')
-                udbg_hvc_putc('\r');
-
-        do {
-                switch(hvterm_priv0.proto) {
-                case HV_PROTOCOL_RAW:
-                        count = hvterm_raw_put_chars(0, &c, 1);
-                        break;
-                case HV_PROTOCOL_HVSI:
-                        count = hvterm_hvsi_put_chars(0, &c, 1);
-                        break;
-                }
-        } while(count == 0);
-}
-
-static int udbg_hvc_getc_poll(void)
-{
-        int rc = 0;
-        char c;
-
-        switch(hvterm_priv0.proto) {
-        case HV_PROTOCOL_RAW:
-                rc = hvterm_raw_get_chars(0, &c, 1);
-                break;
-        case HV_PROTOCOL_HVSI:
-                rc = hvterm_hvsi_get_chars(0, &c, 1);
-                break;
-        }
-        if (!rc)
-                return -1;
-        return c;
-}
-
-static int udbg_hvc_getc(void)
-{
-        int ch;
-        for (;;) {
-                ch = udbg_hvc_getc_poll();
-                if (ch == -1) {
-                        /* This shouldn't be needed...but... */
-                        volatile unsigned long delay;
-                        for (delay=0; delay < 2000000; delay++)
-                                ;
-                } else {
-                        return ch;
-                }
-        }
-}
-
 void __init hvc_vio_init_early(void)
 {
         struct device_node *stdout_node;