src/console/vtxprintf.c

   1 /*
   2  * This file is part of the coreboot project.
   3  *
   4  *  Copyright (C) 1991, 1992  Linus Torvalds
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; version 2 of the License.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * vtxprintf.c, originally from linux/lib/vsprintf.c
  16  */
  17
  18 #include <console/vtxprintf.h>
  19 #include <string.h>
  20
  21 #define call_tx(x) tx_byte(x, data)
  22
  23 #if !IS_ENABLED(CONFIG_ARCH_MIPS)
  24 #define SUPPORT_64BIT_INTS
  25 #endif
  26
  27 /* haha, don't need ctype.c */
  28 #define isdigit(c)      ((c) >= '0' && (c) <= '9')
  29 #define is_digit isdigit
  30 #define isxdigit(c)     (((c) >= '0' && (c) <= '9') || ((c) >= 'a' && (c) <= 'f') || ((c) >= 'A' && (c) <= 'F'))
  31
  32 static int skip_atoi(const char **s)
  33 {
  34         int i=0;
  35
  36         while (is_digit(**s))
  37                 i = i*10 + *((*s)++) - '0';
  38         return i;
  39 }
  40
  41 #define ZEROPAD 1               /* pad with zero */
  42 #define SIGN    2               /* unsigned/signed long */
  43 #define PLUS    4               /* show plus */
  44 #define SPACE   8               /* space if plus */
  45 #define LEFT    16              /* left justified */
  46 #define SPECIAL 32              /* 0x */
  47 #define LARGE   64              /* use 'ABCDEF' instead of 'abcdef' */
  48
  49 static int number(void (*tx_byte)(unsigned char byte, void *data),
  50         unsigned long long inum, int base, int size, int precision, int type,
  51         void *data)
  52 {
  53         char c,sign,tmp[66];
  54         const char *digits="0123456789abcdefghijklmnopqrstuvwxyz";
  55         int i;
  56         int count = 0;
  57 #ifdef SUPPORT_64BIT_INTS
  58         unsigned long long num = inum;
  59 #else
  60         unsigned long num = (long)inum;
  61
  62         if (num != inum) {
  63                 /* Alert user to an incorrect result by printing #^!. */
  64                 call_tx('#');
  65                 call_tx('^');
  66                 call_tx('!');
  67         }
  68 #endif
  69
  70         if (type & LARGE)
  71                 digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
  72         if (type & LEFT)
  73                 type &= ~ZEROPAD;
  74         if (base < 2 || base > 36)
  75                 return 0;
  76         c = (type & ZEROPAD) ? '0' : ' ';
  77         sign = 0;
  78         if (type & SIGN) {
  79                 if ((signed long long)num < 0) {
  80                         sign = '-';
  81                         num = -num;
  82                         size--;
  83                 } else if (type & PLUS) {
  84                         sign = '+';
  85                         size--;
  86                 } else if (type & SPACE) {
  87                         sign = ' ';
  88                         size--;
  89                 }
  90         }
  91         if (type & SPECIAL) {
  92                 if (base == 16)
  93                         size -= 2;
  94                 else if (base == 8)
  95                         size--;
  96         }
  97         i = 0;
  98         if (num == 0)
  99                 tmp[i++]='0';
 100         else while (num != 0){
 101                 tmp[i++] = digits[num % base];
 102                 num /= base;
 103         }
 104         if (i > precision)
 105                 precision = i;
 106         size -= precision;
 107         if (!(type&(ZEROPAD+LEFT)))
 108                 while (size-- > 0)
 109                         call_tx(' '), count++;
 110         if (sign)
 111                 call_tx(sign), count++;
 112         if (type & SPECIAL) {
 113                 if (base == 8)
 114                         call_tx('0'), count++;
 115                 else if (base == 16) {
 116                         call_tx('0'), count++;
 117                         call_tx(digits[33]), count++;
 118                 }
 119         }
 120         if (!(type & LEFT))
 121                 while (size-- > 0)
 122                         call_tx(c), count++;
 123         while (i < precision--)
 124                 call_tx('0'), count++;
 125         while (i-- > 0)
 126                 call_tx(tmp[i]), count++;
 127         while (size-- > 0)
 128                 call_tx(' '), count++;
 129         return count;
 130 }
 131
 132
 133 int vtxprintf(void (*tx_byte)(unsigned char byte, void *data),
 134                const char *fmt, va_list args, void *data)
 135 {
 136         int len;
 137         unsigned long long num;
 138         int i, base;
 139         const char *s;
 140
 141         int flags;              /* flags to number() */
 142
 143         int field_width;        /* width of output field */
 144         int precision;          /* min. # of digits for integers; max
 145                                    number of chars for from string */
 146         int qualifier;          /* 'h', 'H', 'l', or 'L' for integer fields */
 147
 148         int count;
 149
 150         for (count=0; *fmt ; ++fmt) {
 151                 if (*fmt != '%') {
 152                         call_tx(*fmt), count++;
 153                         continue;
 154                 }
 155
 156                 /* process flags */
 157                 flags = 0;
 158 repeat:
 159                         ++fmt;          /* this also skips first '%' */
 160                         switch (*fmt) {
 161                                 case '-': flags |= LEFT; goto repeat;
 162                                 case '+': flags |= PLUS; goto repeat;
 163                                 case ' ': flags |= SPACE; goto repeat;
 164                                 case '#': flags |= SPECIAL; goto repeat;
 165                                 case '0': flags |= ZEROPAD; goto repeat;
 166                                 }
 167
 168                 /* get field width */
 169                 field_width = -1;
 170                 if (is_digit(*fmt))
 171                         field_width = skip_atoi(&fmt);
 172                 else if (*fmt == '*') {
 173                         ++fmt;
 174                         /* it's the next argument */
 175                         field_width = va_arg(args, int);
 176                         if (field_width < 0) {
 177                                 field_width = -field_width;
 178                                 flags |= LEFT;
 179                         }
 180                 }
 181
 182                 /* get the precision */
 183                 precision = -1;
 184                 if (*fmt == '.') {
 185                         ++fmt;
 186                         if (is_digit(*fmt))
 187                                 precision = skip_atoi(&fmt);
 188                         else if (*fmt == '*') {
 189                                 ++fmt;
 190                                 /* it's the next argument */
 191                                 precision = va_arg(args, int);
 192                         }
 193                         if (precision < 0)
 194                                 precision = 0;
 195                 }
 196
 197                 /* get the conversion qualifier */
 198                 qualifier = -1;
 199                 if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' || *fmt == 'z') {
 200                         qualifier = *fmt;
 201                         ++fmt;
 202                         if (*fmt == 'l') {
 203                                 qualifier = 'L';
 204                                 ++fmt;
 205                         }
 206                         if (*fmt == 'h') {
 207                                 qualifier = 'H';
 208                                 ++fmt;
 209                         }
 210                 }
 211
 212                 /* default base */
 213                 base = 10;
 214
 215                 switch (*fmt) {
 216                 case 'c':
 217                         if (!(flags & LEFT))
 218                                 while (--field_width > 0)
 219                                         call_tx(' '), count++;
 220                         call_tx((unsigned char) va_arg(args, int)), count++;
 221                         while (--field_width > 0)
 222                                 call_tx(' '), count++;
 223                         continue;
 224
 225                 case 's':
 226                         s = va_arg(args, char *);
 227                         if (!s)
 228                                 s = "<NULL>";
 229
 230                         len = strnlen(s, (size_t)precision);
 231
 232                         if (!(flags & LEFT))
 233                                 while (len < field_width--)
 234                                         call_tx(' '), count++;
 235                         for (i = 0; i < len; ++i)
 236                                 call_tx(*s++), count++;
 237                         while (len < field_width--)
 238                                 call_tx(' '), count++;
 239                         continue;
 240
 241                 case 'p':
 242                         if (field_width == -1) {
 243                                 field_width = 2*sizeof(void *);
 244                                 flags |= ZEROPAD;
 245                         }
 246                         count += number(tx_byte,
 247                                 (unsigned long) va_arg(args, void *), 16,
 248                                 field_width, precision, flags, data);
 249                         continue;
 250
 251                 case 'n':
 252                         if (qualifier == 'L') {
 253                                 long long *ip = va_arg(args, long long *);
 254                                 *ip = count;
 255                         } else if (qualifier == 'l') {
 256                                 long * ip = va_arg(args, long *);
 257                                 *ip = count;
 258                         } else {
 259                                 int * ip = va_arg(args, int *);
 260                                 *ip = count;
 261                         }
 262                         continue;
 263
 264                 case '%':
 265                         call_tx('%'), count++;
 266                         continue;
 267
 268                 /* integer number formats - set up the flags and "break" */
 269                 case 'o':
 270                         base = 8;
 271                         break;
 272
 273                 case 'X':
 274                         flags |= LARGE;
 275                 case 'x':
 276                         base = 16;
 277                         break;
 278
 279                 case 'd':
 280                 case 'i':
 281                         flags |= SIGN;
 282                 case 'u':
 283                         break;
 284
 285                 default:
 286                         call_tx('%'), count++;
 287                         if (*fmt)
 288                                 call_tx(*fmt), count++;
 289                         else
 290                                 --fmt;
 291                         continue;
 292                 }
 293                 if (qualifier == 'L') {
 294                         num = va_arg(args, unsigned long long);
 295                 } else if (qualifier == 'l') {
 296                         num = va_arg(args, unsigned long);
 297                 } else if (qualifier == 'z') {
 298                         num = va_arg(args, size_t);
 299                 } else if (qualifier == 'h') {
 300                         num = (unsigned short) va_arg(args, int);
 301                         if (flags & SIGN)
 302                                 num = (short) num;
 303                 } else if (qualifier == 'H') {
 304                         num = (unsigned char) va_arg(args, int);
 305                         if (flags & SIGN)
 306                                 num = (signed char) num;
 307                 } else if (flags & SIGN) {
 308                         num = va_arg(args, int);
 309                 } else {
 310                         num = va_arg(args, unsigned int);
 311                 }
 312                 count += number(tx_byte, num, base, field_width, precision, flags, data);
 313         }
 314         return count;
 315 }