loader/console.c

   1 // Copyright 2009  Segher Boessenkool  <segher@kernel.crashing.org>
   2 // This code is licensed to you under the terms of the GNU GPL, version 2;
   3 // see file COPYING or http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
   4
   5
   6 #include <stdarg.h>
   7
   8 #include "loader.h"
   9
  10
  11 static void put(char c)
  12 {
  13         fb_putc(c);
  14         usbgecko_console_putc(c);
  15 }
  16
  17
  18 // __umoddi3() and friends are very big, and more general than we need:
  19 // radix is always (very) small, so we can work by much bigger chunks
  20 // than single bits, always.
  21 static int extract_dig(u64 *x, u32 radix)
  22 {
  23         u32 hi = *x >> 32;
  24         u32 lo = *x;
  25         u32 mod = hi % radix;
  26         hi /= radix;
  27         u32 n = (mod << 16) | (lo >> 16);
  28         mod = n % radix;
  29         n /= radix;
  30         lo = (mod << 16) | (lo & 0xffff);
  31         mod = lo % radix;
  32         lo /= radix;
  33         lo |= (n << 16);
  34         *x = ((u64)hi << 32) | lo;
  35         return mod;
  36 }
  37
  38
  39 // This implements conversions %{0}{number}{l,ll}[%cdsux] only.
  40 // Field length is obeyed for numbers only.
  41 // Always returns 0.
  42
  43 int printf(const char *restrict format, ...)
  44 {
  45         va_list ap;
  46
  47         va_start(ap, format);
  48
  49         while (*format) {
  50                 if (*format != '%') {
  51                         put(*format++);
  52                         continue;
  53                 }
  54                 format++;
  55
  56                 int zero = 0;
  57                 int prec = 0;
  58
  59                 if (*format == '0') {
  60                         zero = 1;
  61                         format++;
  62                 }
  63
  64                 while (*format >= '0' && *format <= '9')
  65                         prec = 10*prec + (*format++ - '0');
  66
  67                 int ll = 0;
  68                 while (*format == 'l') {
  69                         ll++;
  70                         format++;
  71                 }
  72
  73                 int radix = 10;
  74                 int is_signed = 1;
  75
  76                 switch (*format++) {
  77                 case '%':
  78                         put('%');
  79                         break;
  80
  81                 case 'c':
  82                         put(va_arg(ap, int));
  83                         break;
  84
  85                 case 's':
  86                         ;
  87                         char *s = va_arg(ap, char *);
  88                         while (*s)
  89                                 put(*s++);
  90                         break;
  91
  92                 case 'x':
  93                         radix = 16;
  94
  95                 case 'u':
  96                         is_signed = 0;
  97
  98                 case 'd':
  99                         ;
 100                         u64 x;
 101                         if (is_signed) {
 102                                 if (ll == 0)
 103                                         x = va_arg(ap, int);
 104                                 else if (ll == 1)
 105                                         x = va_arg(ap, long);
 106                                 else
 107                                         x = va_arg(ap, long long);
 108                         } else {
 109                                 if (ll == 0)
 110                                         x = va_arg(ap, unsigned int);
 111                                 else if (ll == 1)
 112                                         x = va_arg(ap, unsigned long);
 113                                 else
 114                                         x = va_arg(ap, unsigned long long);
 115                         }
 116
 117                         if (is_signed) {
 118                                 if ((long long)x < 0)
 119                                         x = -x;
 120                                 else
 121                                         is_signed = 0;
 122                         }
 123
 124                         char hold[22];
 125                         char *hld = &hold[sizeof hold];
 126                         *--hld = 0;
 127
 128                         int len = 0;
 129                         do {
 130                                 int dig = extract_dig(&x, radix);
 131                                 if (dig >= 10)
 132                                         dig += 'a' - 10;
 133                                 else
 134                                         dig += '0';
 135                                 *--hld = dig;
 136                                 len++;
 137                         } while (x);
 138                         if (is_signed)
 139                                 *--hld = '-';
 140
 141                         while (len < prec) {
 142                                 put(zero ? '0' : ' ');
 143                                 len++;
 144                         }
 145                         while (*hld)
 146                                 put(*hld++);
 147                 }
 148         }
 149
 150         va_end(ap);
 151
 152         return 0;
 153 }