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     syscall(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 = "0123456789abcdefghijklmnopqrstuvwxyz";
 113     int i;
 114
 115     if (type & LARGE)
 116         digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
 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 /* Forward decl. needed for IP address printing stuff... */
 177 int sprintf(char *buf, const char *fmt, ...);
 178
 179 int vsprintf(char *buf, const char *fmt, va_list args)
 180 {
 181     int len;
 182     unsigned long num;
 183     int i, base;
 184     char *str;
 185     const char *s;
 186
 187     int flags;                  /* flags to number() */
 188
 189     int field_width;            /* width of output field */
 190     int precision;              /* min. # of digits for integers; max
 191                                    number of chars for from string */
 192     int qualifier;              /* 'h', 'l', or 'L' for integer fields */
 193
 194     for (str = buf; *fmt; ++fmt) {
 195         if (*fmt != '%') {
 196             *str++ = *fmt;
 197             continue;
 198         }
 199
 200         /* process flags */
 201         flags = 0;
 202 repeat:
 203         ++fmt;                  /* this also skips first '%' */
 204         switch (*fmt) {
 205         case '-':
 206             flags |= LEFT;
 207             goto repeat;
 208         case '+':
 209             flags |= PLUS;
 210             goto repeat;
 211         case ' ':
 212             flags |= SPACE;
 213             goto repeat;
 214         case '#':
 215             flags |= SPECIAL;
 216             goto repeat;
 217         case '0':
 218             flags |= ZEROPAD;
 219             goto repeat;
 220         }
 221
 222         /* get field width */
 223         field_width = -1;
 224         if (isdigit(*fmt))
 225             field_width = skip_atoi(&fmt);
 226         else if (*fmt == '*') {
 227             ++fmt;
 228             /* it's the next argument */
 229             field_width = va_arg(args, int);
 230             if (field_width < 0) {
 231                 field_width = -field_width;
 232                 flags |= LEFT;
 233             }
 234         }
 235
 236         /* get the precision */
 237         precision = -1;
 238         if (*fmt == '.') {
 239             ++fmt;
 240             if (isdigit(*fmt))
 241                 precision = skip_atoi(&fmt);
 242             else if (*fmt == '*') {
 243                 ++fmt;
 244                 /* it's the next argument */
 245                 precision = va_arg(args, int);
 246             }
 247             if (precision < 0)
 248                 precision = 0;
 249         }
 250
 251         /* get the conversion qualifier */
 252         qualifier = -1;
 253         if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') {
 254             qualifier = *fmt;
 255             ++fmt;
 256         }
 257
 258         /* default base */
 259         base = 10;
 260
 261         switch (*fmt) {
 262         case 'c':
 263             if (!(flags & LEFT))
 264                 while (--field_width > 0)
 265                     *str++ = ' ';
 266             *str++ = (unsigned char)va_arg(args, int);
 267             while (--field_width > 0)
 268                 *str++ = ' ';
 269             continue;
 270
 271         case 's':
 272             s = va_arg(args, char *);
 273             len = strnlen(s, precision);
 274
 275             if (!(flags & LEFT))
 276                 while (len < field_width--)
 277                     *str++ = ' ';
 278             for (i = 0; i < len; ++i)
 279                 *str++ = *s++;
 280             while (len < field_width--)
 281                 *str++ = ' ';
 282             continue;
 283
 284         case 'p':
 285             if (field_width == -1) {
 286                 field_width = 2 * sizeof(void *);
 287                 flags |= ZEROPAD;
 288             }
 289             str = number(str,
 290                          (unsigned long)va_arg(args, void *), 16,
 291                          field_width, precision, flags);
 292             continue;
 293
 294         case 'n':
 295             if (qualifier == 'l') {
 296                 long *ip = va_arg(args, long *);
 297                 *ip = (str - buf);
 298             } else {
 299                 int *ip = va_arg(args, int *);
 300                 *ip = (str - buf);
 301             }
 302             continue;
 303
 304         case '%':
 305             *str++ = '%';
 306             continue;
 307
 308             /* integer number formats - set up the flags and "break" */
 309         case 'o':
 310             base = 8;
 311             break;
 312
 313         case 'X':
 314             flags |= LARGE;
 315         case 'x':
 316             base = 16;
 317             break;
 318
 319         case 'd':
 320         case 'i':
 321             flags |= SIGN;
 322         case 'u':
 323             break;
 324
 325         default:
 326             *str++ = '%';
 327             if (*fmt)
 328                 *str++ = *fmt;
 329             else
 330                 --fmt;
 331             continue;
 332         }
 333         if (qualifier == 'l')
 334             num = va_arg(args, unsigned long);
 335         else if (qualifier == 'h') {
 336             num = (unsigned short)va_arg(args, int);
 337             if (flags & SIGN)
 338                 num = (short)num;
 339         } else if (flags & SIGN)
 340             num = va_arg(args, int);
 341         else
 342             num = va_arg(args, unsigned int);
 343         str = number(str, num, base, field_width, precision, flags);
 344     }
 345     *str = '\0';
 346     return str - buf;
 347 }
 348
 349 int sprintf(char *buf, const char *fmt, ...)
 350 {
 351     va_list args;
 352     int i;
 353
 354     va_start(args, fmt);
 355     i = vsprintf(buf, fmt, args);
 356     va_end(args);
 357     return i;
 358 }
 359
 360 int printf(const char *fmt, ...)
 361 {
 362     char printf_buf[1024];
 363     va_list args;
 364     int printed;
 365
 366     va_start(args, fmt);
 367     printed = vsprintf(printf_buf, fmt, args);
 368     va_end(args);
 369
 370     puts(printf_buf);
 371
 372     return printed;
 373 }