memdisk/conio.c

   1 /* ----------------------------------------------------------------------- *
   2  *
   3  *   Copyright 2001-2008 H. Peter Anvin - All Rights Reserved
   4  *
   5  *   This program is free software; you can redistribute it and/or modify
   6  *   it under the terms of the GNU General Public License as published by
   7  *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
   8  *   Boston MA 02111-1307, USA; either version 2 of the License, or
   9  *   (at your option) any later version; incorporated herein by reference.
  10  *
  11  * ----------------------------------------------------------------------- */
  12
  13 /*
  14  * conio.c
  15  *
  16  * Output to the screen
  17  */
  18
  19 #include <stdint.h>
  20 #include "memdisk.h"
  21 #include "conio.h"
  22
  23 int putchar(int ch)
  24 {
  25     com32sys_t regs;
  26
  27     if (ch == '\n') {
  28         /* \n -> \r\n */
  29         putchar('\r');
  30     }
  31
  32     regs.eax.w[0] = 0x0e00 | (ch & 0xff);
  33     intcall(0x10, &regs, NULL);
  34
  35     return ch;
  36 }
  37
  38 int puts(const char *s)
  39 {
  40     int count = 0;
  41
  42     while (*s) {
  43         putchar(*s);
  44         count++;
  45         s++;
  46     }
  47
  48     return count;
  49 }
  50
  51 /*
  52  * Oh, it's a waste of space, but oh-so-yummy for debugging.  It's just
  53  * initialization code anyway, so it doesn't take up space when we're
  54  * actually running.  This version of printf() does not include 64-bit
  55  * support.  "Live with it."
  56  *
  57  * Most of this code was shamelessly snarfed from the Linux kernel, then
  58  * modified.
  59  */
  60
  61 static inline int isdigit(int ch)
  62 {
  63     return (ch >= '0') && (ch <= '9');
  64 }
  65
  66 static int skip_atoi(const char **s)
  67 {
  68     int i = 0;
  69
  70     while (isdigit(**s))
  71         i = i * 10 + *((*s)++) - '0';
  72     return i;
  73 }
  74
  75 unsigned int atou(const char *s)
  76 {
  77     unsigned int i = 0;
  78     while (isdigit(*s))
  79         i = i * 10 + (*s++ - '0');
  80     return i;
  81 }
  82
  83 static int strnlen(const char *s, int maxlen)
  84 {
  85     const char *es = s;
  86     while (*es && maxlen) {
  87         es++;
  88         maxlen--;
  89     }
  90
  91     return (es - s);
  92 }
  93
  94 #define ZEROPAD 1               /* pad with zero */
  95 #define SIGN    2               /* unsigned/signed long */
  96 #define PLUS    4               /* show plus */
  97 #define SPACE   8               /* space if plus */
  98 #define LEFT    16              /* left justified */
  99 #define SPECIAL 32              /* 0x */
 100 #define LARGE   64              /* use 'ABCDEF' instead of 'abcdef' */
 101
 102 #define do_div(n,base) ({ \
 103 int __res; \
 104 __res = ((unsigned long) n) % (unsigned) base; \
 105 n = ((unsigned long) n) / (unsigned) base; \
 106 __res; })
 107
 108 static char *number(char *str, long num, int base, int size, int precision,
 109                     int type)
 110 {
 111     char c, sign, tmp[66];
 112     const char *digits = "0123456789abcdef";
 113     int i;
 114
 115     if (type & LARGE)
 116         digits = "0123456789ABCDEF";
 117     if (type & LEFT)
 118         type &= ~ZEROPAD;
 119     if (base < 2 || base > 36)
 120         return 0;
 121     c = (type & ZEROPAD) ? '0' : ' ';
 122     sign = 0;
 123     if (type & SIGN) {
 124         if (num < 0) {
 125             sign = '-';
 126             num = -num;
 127             size--;
 128         } else if (type & PLUS) {
 129             sign = '+';
 130             size--;
 131         } else if (type & SPACE) {
 132             sign = ' ';
 133             size--;
 134         }
 135     }
 136     if (type & SPECIAL) {
 137         if (base == 16)
 138             size -= 2;
 139         else if (base == 8)
 140             size--;
 141     }
 142     i = 0;
 143     if (num == 0)
 144         tmp[i++] = '0';
 145     else
 146         while (num != 0)
 147             tmp[i++] = digits[do_div(num, base)];
 148     if (i > precision)
 149         precision = i;
 150     size -= precision;
 151     if (!(type & (ZEROPAD + LEFT)))
 152         while (size-- > 0)
 153             *str++ = ' ';
 154     if (sign)
 155         *str++ = sign;
 156     if (type & SPECIAL) {
 157         if (base == 8)
 158             *str++ = '0';
 159         else if (base == 16) {
 160             *str++ = '0';
 161             *str++ = digits[33];
 162         }
 163     }
 164     if (!(type & LEFT))
 165         while (size-- > 0)
 166             *str++ = c;
 167     while (i < precision--)
 168         *str++ = '0';
 169     while (i-- > 0)
 170         *str++ = tmp[i];
 171     while (size-- > 0)
 172         *str++ = ' ';
 173     return str;
 174 }
 175
 176 int vsprintf(char *buf, const char *fmt, va_list args)
 177 {
 178     int len;
 179     unsigned long num;
 180     int i, base;
 181     char *str;
 182     const char *s;
 183
 184     int flags;                  /* flags to number() */
 185
 186     int field_width;            /* width of output field */
 187     int precision;              /* min. # of digits for integers; max
 188                                    number of chars for from string */
 189     int qualifier;              /* 'h', 'l', or 'L' for integer fields */
 190
 191     for (str = buf; *fmt; ++fmt) {
 192         if (*fmt != '%') {
 193             *str++ = *fmt;
 194             continue;
 195         }
 196
 197         /* process flags */
 198         flags = 0;
 199 repeat:
 200         ++fmt;                  /* this also skips first '%' */
 201         switch (*fmt) {
 202         case '-':
 203             flags |= LEFT;
 204             goto repeat;
 205         case '+':
 206             flags |= PLUS;
 207             goto repeat;
 208         case ' ':
 209             flags |= SPACE;
 210             goto repeat;
 211         case '#':
 212             flags |= SPECIAL;
 213             goto repeat;
 214         case '0':
 215             flags |= ZEROPAD;
 216             goto repeat;
 217         }
 218
 219         /* get field width */
 220         field_width = -1;
 221         if (isdigit(*fmt))
 222             field_width = skip_atoi(&fmt);
 223         else if (*fmt == '*') {
 224             ++fmt;
 225             /* it's the next argument */
 226             field_width = va_arg(args, int);
 227             if (field_width < 0) {
 228                 field_width = -field_width;
 229                 flags |= LEFT;
 230             }
 231         }
 232
 233         /* get the precision */
 234         precision = -1;
 235         if (*fmt == '.') {
 236             ++fmt;
 237             if (isdigit(*fmt))
 238                 precision = skip_atoi(&fmt);
 239             else if (*fmt == '*') {
 240                 ++fmt;
 241                 /* it's the next argument */
 242                 precision = va_arg(args, int);
 243             }
 244             if (precision < 0)
 245                 precision = 0;
 246         }
 247
 248         /* get the conversion qualifier */
 249         qualifier = -1;
 250         if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') {
 251             qualifier = *fmt;
 252             ++fmt;
 253         }
 254
 255         /* default base */
 256         base = 10;
 257
 258         switch (*fmt) {
 259         case 'c':
 260             if (!(flags & LEFT))
 261                 while (--field_width > 0)
 262                     *str++ = ' ';
 263             *str++ = (unsigned char)va_arg(args, int);
 264             while (--field_width > 0)
 265                 *str++ = ' ';
 266             continue;
 267
 268         case 's':
 269             s = va_arg(args, char *);
 270             len = strnlen(s, precision);
 271
 272             if (!(flags & LEFT))
 273                 while (len < field_width--)
 274                     *str++ = ' ';
 275             for (i = 0; i < len; ++i)
 276                 *str++ = *s++;
 277             while (len < field_width--)
 278                 *str++ = ' ';
 279             continue;
 280
 281         case 'p':
 282             if (field_width == -1) {
 283                 field_width = 2 * sizeof(void *);
 284                 flags |= ZEROPAD;
 285             }
 286             str = number(str,
 287                          (unsigned long)va_arg(args, void *), 16,
 288                          field_width, precision, flags);
 289             continue;
 290
 291         case 'n':
 292             if (qualifier == 'l') {
 293                 long *ip = va_arg(args, long *);
 294                 *ip = (str - buf);
 295             } else {
 296                 int *ip = va_arg(args, int *);
 297                 *ip = (str - buf);
 298             }
 299             continue;
 300
 301         case '%':
 302             *str++ = '%';
 303             continue;
 304
 305             /* integer number formats - set up the flags and "break" */
 306         case 'o':
 307             base = 8;
 308             break;
 309
 310         case 'X':
 311             flags |= LARGE;
 312         case 'x':
 313             base = 16;
 314             break;
 315
 316         case 'd':
 317         case 'i':
 318             flags |= SIGN;
 319         case 'u':
 320             break;
 321
 322         default:
 323             *str++ = '%';
 324             if (*fmt)
 325                 *str++ = *fmt;
 326             else
 327                 --fmt;
 328             continue;
 329         }
 330         if (qualifier == 'l')
 331             num = va_arg(args, unsigned long);
 332         else if (qualifier == 'h') {
 333             num = (unsigned short)va_arg(args, int);
 334             if (flags & SIGN)
 335                 num = (short)num;
 336         } else if (flags & SIGN)
 337             num = va_arg(args, int);
 338         else
 339             num = va_arg(args, unsigned int);
 340         str = number(str, num, base, field_width, precision, flags);
 341     }
 342     *str = '\0';
 343     return str - buf;
 344 }
 345
 346 #if 0
 347 int sprintf(char *buf, const char *fmt, ...)
 348 {
 349     va_list args;
 350     int i;
 351
 352     va_start(args, fmt);
 353     i = vsprintf(buf, fmt, args);
 354     va_end(args);
 355     return i;
 356 }
 357 #endif
 358
 359 int vprintf(const char *fmt, va_list args)
 360 {
 361     char printf_buf[2048];
 362     int printed;
 363
 364     printed = vsprintf(printf_buf, fmt, args);
 365     puts(printf_buf);
 366     return printed;
 367 }
 368
 369 int printf(const char *fmt, ...)
 370 {
 371     va_list args;
 372     int printed;
 373
 374     va_start(args, fmt);
 375     printed = vprintf(fmt, args);
 376     va_end(args);
 377     return printed;
 378 }
 379
 380 /*
 381  * Jump here if all hope is gone...
 382  */
 383 void __attribute__ ((noreturn)) die(const char *fmt, ...)
 384 {
 385     va_list ap;
 386
 387     va_start(ap, fmt);
 388     vprintf(fmt, ap);
 389     va_end(ap);
 390
 391     sti();
 392     for (;;)
 393         asm volatile("hlt");
 394 }