[POWERPC] 4xx: Add early udbg support for 40x processors

This adds some basic real mode based early udbg support for 40x
in order to debug things more easily

Signed-off-by: Benjamin Herrenschmidt <benh@kernel.crashing.org>
Signed-off-by: Josh Boyer <jwboyer@linux.vnet.ibm.com>

3 files changed, 75 insertions, 0 deletions

diff --git a/arch/powerpc/kernel/misc_32.S b/arch/powerpc/kernel/misc_32.S
index ea1137851a4a..be09f0d2d90b 100644
--- a/arch/powerpc/kernel/misc_32.S
+++ b/arch/powerpc/kernel/misc_32.S
@@ -206,6 +206,45 @@ _GLOBAL(_nmask_and_or_msr)
         isync
         blr                     /* Done */
 
+#ifdef CONFIG_40x
+
+/*
+ * Do an IO access in real mode
+ */
+_GLOBAL(real_readb)
+        mfmsr   r7
+        ori     r0,r7,MSR_DR
+        xori    r0,r0,MSR_DR
+        sync
+        mtmsr   r0
+        sync
+        isync
+        lbz     r3,0(r3)
+        sync
+        mtmsr   r7
+        sync
+        isync
+        blr
+
+        /*
+ * Do an IO access in real mode
+ */
+_GLOBAL(real_writeb)
+        mfmsr   r7
+        ori     r0,r7,MSR_DR
+        xori    r0,r0,MSR_DR
+        sync
+        mtmsr   r0
+        sync
+        isync
+        stb     r3,0(r4)
+        sync
+        mtmsr   r7
+        sync
+        isync
+        blr
+
+#endif /* CONFIG_40x */
 
 /*
  * Flush MMU TLB
diff --git a/arch/powerpc/kernel/udbg.c b/arch/powerpc/kernel/udbg.c
index eba148f2a31c..7aad6203e411 100644
--- a/arch/powerpc/kernel/udbg.c
+++ b/arch/powerpc/kernel/udbg.c
@@ -54,6 +54,9 @@ void __init udbg_early_init(void)
 #elif defined(CONFIG_PPC_EARLY_DEBUG_44x)
         /* PPC44x debug */
         udbg_init_44x_as1();
+#elif defined(CONFIG_PPC_EARLY_DEBUG_40x)
+        /* PPC40x debug */
+        udbg_init_40x_realmode();
 #elif defined(CONFIG_PPC_EARLY_DEBUG_CPM)
         udbg_init_cpm();
 #endif
diff --git a/arch/powerpc/kernel/udbg_16550.c b/arch/powerpc/kernel/udbg_16550.c
index df740eae77b8..cb01ebc59387 100644
--- a/arch/powerpc/kernel/udbg_16550.c
+++ b/arch/powerpc/kernel/udbg_16550.c
@@ -225,3 +225,36 @@ void __init udbg_init_44x_as1(void)
         udbg_getc = udbg_44x_as1_getc;
 }
 #endif /* CONFIG_PPC_EARLY_DEBUG_44x */
+
+#ifdef CONFIG_PPC_EARLY_DEBUG_40x
+static void udbg_40x_real_putc(char c)
+{
+        if (udbg_comport) {
+                while ((real_readb(&udbg_comport->lsr) & LSR_THRE) == 0)
+                        /* wait for idle */;
+                real_writeb(c, &udbg_comport->thr); eieio();
+                if (c == '\n')
+                        udbg_40x_real_putc('\r');
+        }
+}
+
+static int udbg_40x_real_getc(void)
+{
+        if (udbg_comport) {
+                while ((real_readb(&udbg_comport->lsr) & LSR_DR) == 0)
+                        ; /* wait for char */
+                return real_readb(&udbg_comport->rbr);
+        }
+        return -1;
+}
+
+void __init udbg_init_40x_realmode(void)
+{
+        udbg_comport = (struct NS16550 __iomem *)
+                CONFIG_PPC_EARLY_DEBUG_40x_PHYSADDR;
+
+        udbg_putc = udbg_40x_real_putc;
+        udbg_getc = udbg_40x_real_getc;
+        udbg_getc_poll = NULL;
+}
+#endif /* CONFIG_PPC_EARLY_DEBUG_40x */