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