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   {
  35     uint16_t ax = 0x0e00|(ch&0xff);
  36     asm volatile("movw $0x0007,%%bx ; "
  37                  "int $0x10"
  38                  : "+a" (ax)
  39                  :: "ebx", "ecx", "edx", "esi", "edi", "ebp");
  40   }
  41
  42   return ch;
  43 }
  44
  45 int puts(const char *s)
  46 {
  47   int count = 0;
  48
  49   while ( *s ) {
  50     putchar(*s);
  51     count++;
  52     s++;
  53   }
  54
  55   return count;
  56 }
  57
  58 /*
  59  * Oh, it's a waste of space, but oh-so-yummy for debugging.  It's just
  60  * initialization code anyway, so it doesn't take up space when we're
  61  * actually running.  This version of printf() does not include 64-bit
  62  * support.  "Live with it."
  63  *
  64  * Most of this code was shamelessly snarfed from the Linux kernel, then
  65  * modified.
  66  */
  67
  68 static inline int
  69 isdigit(int ch)
  70 {
  71   return (ch >= '0') && (ch <= '9');
  72 }
  73
  74 static int skip_atoi(const char **s)
  75 {
  76   int i=0;
  77
  78   while (isdigit(**s))
  79     i = i*10 + *((*s)++) - '0';
  80   return i;
  81 }
  82
  83 unsigned int atou(const char *s)
  84 {
  85   unsigned int i = 0;
  86   while (isdigit(*s))
  87     i = i*10 + (*s++ - '0');
  88   return i;
  89 }
  90
  91 static int strnlen(const char *s, int maxlen)
  92 {
  93   const char *es = s;
  94   while ( *es && maxlen ) {
  95     es++; maxlen--;
  96   }
  97
  98   return (es-s);
  99 }
 100
 101 #define ZEROPAD 1               /* pad with zero */
 102 #define SIGN    2               /* unsigned/signed long */
 103 #define PLUS    4               /* show plus */
 104 #define SPACE   8               /* space if plus */
 105 #define LEFT    16              /* left justified */
 106 #define SPECIAL 32              /* 0x */
 107 #define LARGE   64              /* use 'ABCDEF' instead of 'abcdef' */
 108
 109 #define do_div(n,base) ({ \
 110 int __res; \
 111 __res = ((unsigned long) n) % (unsigned) base; \
 112 n = ((unsigned long) n) / (unsigned) base; \
 113 __res; })
 114
 115 static char * number(char * str, long num, int base, int size, int precision
 116         ,int type)
 117 {
 118   char c,sign,tmp[66];
 119   const char *digits="0123456789abcdefghijklmnopqrstuvwxyz";
 120   int i;
 121
 122   if (type & LARGE)
 123     digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
 124   if (type & LEFT)
 125     type &= ~ZEROPAD;
 126   if (base < 2 || base > 36)
 127     return 0;
 128   c = (type & ZEROPAD) ? '0' : ' ';
 129   sign = 0;
 130   if (type & SIGN) {
 131     if (num < 0) {
 132       sign = '-';
 133       num = -num;
 134       size--;
 135     } else if (type & PLUS) {
 136       sign = '+';
 137       size--;
 138     } else if (type & SPACE) {
 139       sign = ' ';
 140       size--;
 141     }
 142   }
 143   if (type & SPECIAL) {
 144     if (base == 16)
 145       size -= 2;
 146     else if (base == 8)
 147       size--;
 148   }
 149   i = 0;
 150   if (num == 0)
 151     tmp[i++]='0';
 152   else while (num != 0)
 153     tmp[i++] = digits[do_div(num,base)];
 154   if (i > precision)
 155     precision = i;
 156   size -= precision;
 157   if (!(type&(ZEROPAD+LEFT)))
 158     while(size-->0)
 159       *str++ = ' ';
 160   if (sign)
 161     *str++ = sign;
 162   if (type & SPECIAL) {
 163     if (base==8)
 164       *str++ = '0';
 165     else if (base==16) {
 166       *str++ = '0';
 167       *str++ = digits[33];
 168     }
 169   }
 170   if (!(type & LEFT))
 171     while (size-- > 0)
 172       *str++ = c;
 173   while (i < precision--)
 174     *str++ = '0';
 175   while (i-- > 0)
 176     *str++ = tmp[i];
 177   while (size-- > 0)
 178     *str++ = ' ';
 179   return str;
 180 }
 181
 182 /* Forward decl. needed for IP address printing stuff... */
 183 int sprintf(char * buf, const char *fmt, ...);
 184
 185 int vsprintf(char *buf, const char *fmt, va_list args)
 186 {
 187   int len;
 188   unsigned long num;
 189   int i, base;
 190   char * str;
 191   const char *s;
 192
 193   int flags;            /* flags to number() */
 194
 195   int field_width;      /* width of output field */
 196   int precision;                /* min. # of digits for integers; max
 197                                    number of chars for from string */
 198   int qualifier;                /* 'h', 'l', or 'L' for integer fields */
 199
 200   for (str=buf ; *fmt ; ++fmt) {
 201     if (*fmt != '%') {
 202       *str++ = *fmt;
 203       continue;
 204     }
 205
 206     /* process flags */
 207     flags = 0;
 208   repeat:
 209     ++fmt;              /* this also skips first '%' */
 210     switch (*fmt) {
 211     case '-': flags |= LEFT; goto repeat;
 212     case '+': flags |= PLUS; goto repeat;
 213     case ' ': flags |= SPACE; goto repeat;
 214     case '#': flags |= SPECIAL; goto repeat;
 215     case '0': flags |= ZEROPAD; goto repeat;
 216     }
 217
 218     /* get field width */
 219     field_width = -1;
 220     if (isdigit(*fmt))
 221       field_width = skip_atoi(&fmt);
 222     else if (*fmt == '*') {
 223       ++fmt;
 224       /* it's the next argument */
 225       field_width = va_arg(args, int);
 226       if (field_width < 0) {
 227         field_width = -field_width;
 228         flags |= LEFT;
 229       }
 230     }
 231
 232     /* get the precision */
 233     precision = -1;
 234     if (*fmt == '.') {
 235       ++fmt;
 236       if (isdigit(*fmt))
 237         precision = skip_atoi(&fmt);
 238       else if (*fmt == '*') {
 239         ++fmt;
 240         /* it's the next argument */
 241         precision = va_arg(args, int);
 242       }
 243       if (precision < 0)
 244         precision = 0;
 245     }
 246
 247     /* get the conversion qualifier */
 248     qualifier = -1;
 249     if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') {
 250       qualifier = *fmt;
 251       ++fmt;
 252     }
 253
 254     /* default base */
 255     base = 10;
 256
 257     switch (*fmt) {
 258     case 'c':
 259       if (!(flags & LEFT))
 260         while (--field_width > 0)
 261           *str++ = ' ';
 262       *str++ = (unsigned char) va_arg(args, int);
 263       while (--field_width > 0)
 264         *str++ = ' ';
 265       continue;
 266
 267     case 's':
 268       s = va_arg(args, char *);
 269       len = strnlen(s, precision);
 270
 271       if (!(flags & LEFT))
 272         while (len < field_width--)
 273           *str++ = ' ';
 274       for (i = 0; i < len; ++i)
 275         *str++ = *s++;
 276       while (len < field_width--)
 277         *str++ = ' ';
 278       continue;
 279
 280     case 'p':
 281       if (field_width == -1) {
 282         field_width = 2*sizeof(void *);
 283         flags |= ZEROPAD;
 284       }
 285       str = number(str,
 286                    (unsigned long) va_arg(args, void *), 16,
 287                    field_width, precision, flags);
 288       continue;
 289
 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 int sprintf(char * buf, const char *fmt, ...)
 347 {
 348   va_list args;
 349   int i;
 350
 351   va_start(args, fmt);
 352   i=vsprintf(buf,fmt,args);
 353   va_end(args);
 354   return i;
 355 }
 356
 357 int printf(const char *fmt, ...)
 358 {
 359   char printf_buf[1024];
 360   va_list args;
 361   int printed;
 362
 363   va_start(args, fmt);
 364   printed = vsprintf(printf_buf, fmt, args);
 365   va_end(args);
 366
 367   puts(printf_buf);
 368
 369   return printed;
 370 }
 371