From 45a30cd864c313b353f47c8186b2b932069af4a1 Mon Sep 17 00:00:00 2001
From: Richard Henderson <rth@twiddle.net>
Date: Fri, 6 May 2011 15:50:07 -0700
Subject: Handle padding in printf numerical output.

---
 printf.c | 85 ++++++++++++++++++++++++++++++++++++++++++++--------------------
 protos.h |  1 +
 strlen.S | 59 ++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 119 insertions(+), 26 deletions(-)
 create mode 100644 strlen.S

diff --git a/printf.c b/printf.c
index f5c7545..469b82c 100644
--- a/printf.c
+++ b/printf.c
@@ -20,14 +20,38 @@
 
 #include <stdarg.h>
 #include <stdbool.h>
-#include "uart.h"
+#include <string.h>
+#include "console.h"
 
-static int print_decimal(unsigned long val)
+static int print_buf_pad(char *buf, int buflen, char *p, int width, int pad)
+{
+  int len = buf + buflen - p;
+  int r = 0;
+
+  if (width > len)
+    {
+      *--p = pad;
+      len++;
+
+      while (width > buflen)
+	{
+	  crb_puts(0, p, 1);
+	  width--;
+	  r++;
+	}
+      while (width > len)
+	*--p = pad, len++;
+    }
+
+  crb_puts(0, p, len);
+  return r + len;
+}
+
+static int print_decimal(unsigned long val, int width, int pad)
 {
   char buf[32];
-  char *p = buf+31;
+  char *p = buf + sizeof(buf);
 
-  *p = 0;
   if (val == 0)
     *--p = '0';
   else
@@ -48,16 +72,14 @@ static int print_decimal(unsigned long val)
       while (val);
     }
 
-  uart_puts(COM1, p+1);
-  return sizeof(buf) - (p - buf);
+  return print_buf_pad(buf, sizeof(buf), p, width, pad);
 }
 
-static int print_hex(unsigned long val)
+static int print_hex(unsigned long val, int width, char pad)
 {
   char buf[32];
-  char *p = buf+31;
+  char *p = buf + sizeof(buf);
 
-  *p = 0;
   if (val == 0)
     *--p = '0';
   else
@@ -71,8 +93,7 @@ static int print_hex(unsigned long val)
       while (val);
     }
 
-  uart_puts(COM1, p+1);
-  return sizeof(buf) - (p - buf);
+  return print_buf_pad(buf, sizeof(buf), p, width, pad);
 }
 
 int printf(const char *fmt, ...)
@@ -86,18 +107,21 @@ int printf(const char *fmt, ...)
   for (; *fmt ; fmt++)
     if (*fmt != '%')
       {
-        uart_putchar(COM1, *fmt);
+        crb_puts(0, fmt, 1);
 	r++;
       }
     else
       {
+        const char *percent = fmt;
 	bool is_long = false;
+        char pad = ' ';
+        int width = 0;
 
       restart:
         switch (*++fmt)
 	  {
 	  case '%':
-	    uart_putchar(COM1, '%');
+	    crb_puts(0, "%", 1);
 	    r++;
 	    break;
 
@@ -111,7 +135,7 @@ int printf(const char *fmt, ...)
 		long d = va_arg (args, long);
 		if (d < 0)
 		  {
-		    uart_putchar(COM1, '-');
+		    crb_puts(0, "-", 1);
 		    d = -d;
 		  }
 		val = d;
@@ -121,7 +145,7 @@ int printf(const char *fmt, ...)
 		int d = va_arg (args, int);
 		if (d < 0)
 		  {
-		    uart_putchar(COM1, '-');
+		    crb_puts(0, "-", 1);
 		    d = -d;
 		    r++;
 		  }
@@ -136,7 +160,7 @@ int printf(const char *fmt, ...)
 	      val = va_arg (args, unsigned int);
 
 	  do_unsigned:
-	    r += print_decimal (val);
+	    r += print_decimal (val, width, pad);
 	    break;
 
 	  case 'x':
@@ -144,23 +168,32 @@ int printf(const char *fmt, ...)
 	      val = va_arg (args, unsigned long);
 	    else
 	      val = va_arg (args, unsigned int);
-	    r += print_hex (val);
+	    r += print_hex (val, width, pad);
+	    break;
 
 	  case 's':
 	    {
 	      const char *s = va_arg (args, const char *);
-	      while (*s)
-		{
-		  uart_putchar(COM1, *s++);
-		  r++;
-		}
-	      break;
+	      int len = strlen(s);
+	      crb_puts(0, s, len);
+	      r += len;
 	    }
+	    break;
+
+	  case '0':
+	    pad = '0';
+          case '1' ... '9':
+	    width = *fmt - '0';
+	    while (fmt[1] >= '0' && fmt[1] <= '9')
+	      width = width * 10 + *++fmt - '0';
+	    goto restart;
 
 	  default:
-	    uart_putchar(COM1, '%');
-	    uart_putchar(COM1, *fmt);
-	    r += 2;
+	    {
+	      int len = fmt - percent;
+	      crb_puts(0, percent, len);
+	      r += len;
+	    }
 	    break;
 	  }
       }
diff --git a/protos.h b/protos.h
index b765083..bebd50a 100644
--- a/protos.h
+++ b/protos.h
@@ -176,6 +176,7 @@ extern void entInt(void);
  * Utils
  */
 
+extern int printf(const char *, ...);
 extern void ndelay(unsigned long nsec);
 
 static inline void udelay(unsigned long msec)
diff --git a/strlen.S b/strlen.S
new file mode 100644
index 0000000..5a77d72
--- /dev/null
+++ b/strlen.S
@@ -0,0 +1,59 @@
+/*
+ * strlen.S (c) 1995 David Mosberger (davidm@cs.arizona.edu)
+ *
+ * Finds length of a 0-terminated string.  Optimized for the
+ * Alpha architecture:
+ *
+ *	- memory accessed as aligned quadwords only
+ *	- uses bcmpge to compare 8 bytes in parallel
+ *	- does binary search to find 0 byte in last
+ *	  quadword (HAKMEM needed 12 instructions to
+ *	  do this instead of the 9 instructions that
+ *	  binary search needs).
+ */
+
+	.set noreorder
+	.set noat
+
+	.align 3
+
+	.globl	strlen
+	.ent	strlen
+strlen:
+	.frame	$sp, 0, $26, 0
+	.prologue 0
+
+	ldq_u	$1, 0($16)	# load first quadword ($16  may be misaligned)
+	lda	$2, -1($31)
+	insqh	$2, $16, $2
+	andnot	$16, 7, $0
+	or	$2, $1, $1
+	cmpbge	$31, $1, $2	# $2  <- bitmask: bit i == 1 <==> i-th byte == 0
+	bne	$2, found
+
+loop:	ldq	$1, 8($0)
+	addq	$0, 8, $0	# addr += 8
+	nop			# helps dual issue last two insns
+	cmpbge	$31, $1, $2
+	beq	$2, loop
+
+found:	blbs	$2, done	# make aligned case fast
+	negq	$2, $3
+	and	$2, $3, $2
+
+	and	$2, 0x0f, $1
+	addq	$0, 4, $3
+	cmoveq	$1, $3, $0
+
+	and	$2, 0x33, $1
+	addq	$0, 2, $3
+	cmoveq	$1, $3, $0
+
+	and	$2, 0x55, $1
+	addq	$0, 1, $3
+	cmoveq	$1, $3, $0
+
+done:	subq	$0, $16, $0
+	ret	$31, ($26)
+
+	.end	strlen
-- 
cgit v1.1