dprintf: make usable on hardware

Actually configure the serial port used for dprintf, so we can
actually use it on real hardware.

Signed-off-by: H. Peter Anvin <hpa@zytor.com>

1 files changed, 71 insertions, 6 deletions

diff --git a/com32/lib/vdprintf.c b/com32/lib/vdprintf.c
index ea9e0488..330279b3 100644
--- a/com32/lib/vdprintf.c
+++ b/com32/lib/vdprintf.c
@@ -14,15 +14,41 @@
 #define DEBUG 1
 #include <dprintf.h>
 
-#define BUFFER_SIZE	32768
+#define BUFFER_SIZE	4096
 
+enum serial_port_regs {
+    THR = 0,
+    RBR = 0,
+    DLL = 0,
+    DLM = 1,
+    IER = 1,
+    IIR = 2,
+    FCR = 2,
+    LCR = 3,
+    MCR = 4,
+    LSR = 5,
+    MSR = 6,
+    SCR = 7,
+};
 static const uint16_t debug_base = 0x03f8; /* I/O base address */
 
+static void debug_putc(char c)
+{
+    if (c == '\n')
+	debug_putc('\r');
+
+    while ((inb(debug_base + LSR) & 0x20) == 0)
+	cpu_relax();
+    outb(c, debug_base + THR);
+}
+
 void vdprintf(const char *format, va_list ap)
 {
     int rv;
     char buffer[BUFFER_SIZE];
     char *p;
+    static bool debug_init = false;
+    static bool debug_ok   = false;
 
     rv = vsnprintf(buffer, BUFFER_SIZE, format, ap);
 
@@ -37,10 +63,49 @@ void vdprintf(const char *format, va_list ap)
      * if one is enabled or not (this means we don't have to enable the real
      * serial console and therefore get conflicting output.)
      */
-    p = buffer;
-    while (rv--) {
-	while ((inb(debug_base+5) & 0x20) == 0)
-	    cpu_relax();
-	outb(*p++, debug_base);
+    if (__unlikely(!debug_init)) {
+	uint8_t dll, dlm, lcr;
+
+	debug_init = true;
+
+	cli();
+
+	/* Initialize the serial port to 115200 n81 with FIFOs enabled */
+	outb(0x83, debug_base + LCR);
+	outb(0x01, debug_base + DLL);
+	outb(0x00, debug_base + DLM);
+	(void)inb(debug_base + IER);	/* Synchronize */
+	dll = inb(debug_base + DLL);
+	dlm = inb(debug_base + DLM);
+	lcr = inb(debug_base + LCR);
+	
+	outb(0x03, debug_base + LCR);
+	(void)inb(debug_base + IER);	/* Synchronize */
+
+	outb(0x00, debug_base + IER);
+	(void)inb(debug_base + IER);	/* Synchronize */
+
+	sti();
+
+	if (dll != 0x01 || dlm != 0x00 || lcr != 0x83) {
+	    /* No serial port present */
+	    return;
+	}
+
+	outb(0x01, debug_base + FCR);
+	(void)inb(debug_base + IER);	/* Synchronize */
+	if (inb(debug_base + IIR) < 0xc0) {
+	    outb(0x00, debug_base + FCR); /* Disable non-functional FIFOs */
+	    (void)inb(debug_base + IER);	/* Synchronize */
+	}
+
+	debug_ok = true;
     }
+
+    if (!debug_ok)
+	return;
+
+    p = buffer;
+    while (rv--)
+	debug_putc(*p++);
 }