memdisk/conio.c

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