pintos-os.org Git - pintos-anon/blob - src/lib/lib.c

   1 #include <stdint.h>
   2 #include <stdarg.h>
   3 #include <stdbool.h>
   4 #include <stddef.h>
   5 #include "debug.h"
   6 #include "lib.h"
   7 #include "serial.h"
   8 #include "vga.h"
   9
  10 void *
  11 memset (void *dst_, int value, size_t cnt)
  12 {
  13   uint8_t *dst = dst_;
  14
  15   while (cnt-- > 0)
  16     *dst++ = value;
  17
  18   return dst_;
  19 }
  20
  21 void *
  22 memcpy (void *dst_, const void *src_, size_t cnt)
  23 {
  24   uint8_t *dst = dst_;
  25   const uint8_t *src = src_;
  26
  27   while (cnt-- > 0)
  28     *dst++ = *src++;
  29
  30   return dst_;
  31 }
  32
  33 void *
  34 memmove (void *dst_, const void *src_, size_t cnt)
  35 {
  36   uint8_t *dst = dst_;
  37   const uint8_t *src = src_;
  38
  39   if (dst < src)
  40     {
  41       while (cnt-- > 0)
  42         *dst++ = *src++;
  43     }
  44   else
  45     {
  46       dst += cnt;
  47       src += cnt;
  48       while (cnt-- > 0)
  49         *--dst = *--src;
  50     }
  51
  52   return dst;
  53 }
  54
  55 void *
  56 memchr (const void *block_, int ch_, size_t size)
  57 {
  58   const uint8_t *block = block_;
  59   uint8_t ch = ch_;
  60
  61   for (; size-- > 0; block++)
  62     if (*block == ch)
  63       return (void *) block;
  64
  65   return NULL;
  66 }
  67
  68 size_t
  69 strlcpy (char *dst, const char *src, size_t size)
  70 {
  71   size_t src_len = strlen (src);
  72   if (size > 0)
  73     {
  74       size_t dst_len_max = size - 1;
  75       size_t dst_len = src_len < dst_len_max ? src_len : dst_len_max;
  76       memcpy (dst, src, dst_len);
  77       dst[dst_len] = '\0';
  78     }
  79   return src_len;
  80 }
  81
  82 size_t
  83 strlen (const char *string)
  84 {
  85   const char *p;
  86
  87   for (p = string; *p != '\0'; p++)
  88     continue;
  89   return p - string;
  90 }
  91
  92 char *
  93 strchr (const char *string, int c_)
  94 {
  95   char c = c_;
  96
  97   for (;;)
  98     if (*string == c)
  99       return (char *) string;
 100     else if (*string == '\0')
 101       return NULL;
 102     else string++;
 103 }
 104
 105 static void
 106 vprintf_core (const char *format, va_list args,
 107               void (*output) (char, void *), void *aux);
 108
 109 static void
 110 output_console (char ch, void *aux __attribute__ ((unused)))
 111 {
 112   vga_putc (ch);
 113   serial_outb (ch);
 114 }
 115
 116 void
 117 vprintk (const char *format, va_list args)
 118 {
 119   vprintf_core (format, args, output_console, NULL);
 120 }
 121
 122 void
 123 printk (const char *format, ...)
 124 {
 125   va_list args;
 126
 127   va_start (args, format);
 128   vprintk (format, args);
 129   va_end (args);
 130 }
 131 \f
 132 /* printf() and friends internals.  You do not need to understand
 133    this code. */
 134
 135 struct printf_conversion
 136   {
 137     enum
 138       {
 139         MINUS = 1 << 0,
 140         PLUS = 1 << 1,
 141         SPACE = 1 << 2,
 142         POUND = 1 << 3,
 143         ZERO = 1 << 4
 144       }
 145     flags;
 146
 147     int width;
 148     int precision;
 149
 150     enum
 151       {
 152         CHAR = 1,
 153         SHORT = 2,
 154         INT = 3,
 155         INTMAX = 4,
 156         LONG = 5,
 157         LONGLONG = 6,
 158         PTRDIFFT = 7,
 159         SIZET = 8
 160       }
 161     type;
 162   };
 163
 164 static const char *
 165 parse_conversion (const char *format, struct printf_conversion *c, va_list *args)
 166 {
 167   /* Parse flag characters. */
 168   c->flags = 0;
 169   for (;;)
 170     {
 171       switch (*format++)
 172         {
 173         case '-':
 174           c->flags |= MINUS;
 175           break;
 176         case '+':
 177           c->flags |= PLUS;
 178           break;
 179         case ' ':
 180           c->flags |= SPACE;
 181           break;
 182         case '#':
 183           c->flags |= POUND;
 184           break;
 185         case '0':
 186           c->flags |= ZERO;
 187           break;
 188         default:
 189           format--;
 190           goto not_a_flag;
 191         }
 192     }
 193  not_a_flag:
 194   if (c->flags & MINUS)
 195     c->flags &= ~ZERO;
 196   if (c->flags & PLUS)
 197     c->flags &= ~SPACE;
 198
 199   /* Parse field width. */
 200   c->width = 0;
 201   if (*format == '*')
 202     {
 203       format++;
 204       c->width = va_arg (*args, int);
 205     }
 206   else
 207     {
 208       for (; isdigit (*format); format++)
 209         c->width = c->width * 10 + *format - '0';
 210     }
 211   if (c->width < 0)
 212     {
 213       c->width = -c->width;
 214       c->flags |= MINUS;
 215     }
 216
 217   /* Parse precision. */
 218   c->precision = -1;
 219   if (*format == '.')
 220     {
 221       format++;
 222       if (*format == '*')
 223         {
 224           format++;
 225           c->precision = va_arg (*args, int);
 226         }
 227       else
 228         {
 229           c->precision = 0;
 230           for (; isdigit (*format); format++)
 231             c->precision = c->precision * 10 + *format - '0';
 232         }
 233       if (c->precision < 0)
 234         c->precision = -1;
 235     }
 236   if (c->precision >= 0)
 237     c->flags &= ~ZERO;
 238
 239   /* Parse type. */
 240   c->type = INT;
 241   switch (*format++)
 242     {
 243     case 'h':
 244       if (*format == 'h')
 245         {
 246           format++;
 247           c->type = CHAR;
 248         }
 249       else
 250         c->type = SHORT;
 251       break;
 252
 253     case 'j':
 254       c->type = INTMAX;
 255       break;
 256
 257     case 'l':
 258       if (*format == 'l')
 259         {
 260           format++;
 261           c->type = LONGLONG;
 262         }
 263       else
 264         c->type = LONG;
 265       break;
 266
 267     case 'L':
 268     case 'q':
 269       c->type = LONGLONG;
 270       break;
 271
 272     case 't':
 273       c->type = PTRDIFFT;
 274       break;
 275
 276     case 'z':
 277     case 'Z':
 278       c->type = SIZET;
 279       break;
 280
 281     default:
 282       format--;
 283       break;
 284     }
 285
 286   return format;
 287 }
 288
 289 static void
 290 output_dup (char ch, size_t cnt, void (*output) (char, void *), void *aux)
 291 {
 292   while (cnt-- > 0)
 293     output (ch, aux);
 294 }
 295
 296 static void
 297 printf_integer (uintmax_t value, bool negative, const char *digits,
 298                 struct printf_conversion *c,
 299                 void (*output) (char, void *), void *aux)
 300
 301 {
 302   int base = strlen (digits);
 303
 304   char buf[64];
 305   char *const buf_end = buf + sizeof buf;
 306   char *bp;
 307
 308   int pad;
 309
 310   /* Accumulate digits into buffer.
 311      This algorithm produces digits in reverse order, so we count
 312      backward from the end of the array. */
 313   bp = buf_end;
 314   while (value > 0)
 315     {
 316       *--bp = digits[value % base];
 317       value /= base;
 318     }
 319
 320   /* Prepend enough zeros to match precision.
 321      If precision is 0, then a value of zero is rendered as a
 322      null string.  Otherwise at least one digit is presented. */
 323   if (c->precision < 0)
 324     c->precision = 1;
 325   while (buf_end - bp < c->precision && bp > buf + 3)
 326     *--bp = '0';
 327
 328   /* Prepend sign. */
 329   if (c->flags & PLUS)
 330     *--bp = negative ? '-' : '+';
 331   else if (c->flags & SPACE)
 332     *--bp = negative ? '-' : ' ';
 333   else if (negative)
 334     *--bp = '-';
 335
 336   /* Prepend base. */
 337   if ((c->flags & POUND) && base != 10)
 338     {
 339       *--bp = digits[0xa] == 'a' ? 'x' : 'X';
 340       *--bp = '0';
 341     }
 342
 343   /* Calculate number of pad characters to fill field width. */
 344   pad = c->width - (buf_end - bp);
 345   if (pad < 0)
 346     pad = 0;
 347
 348   /* Do output. */
 349   if ((c->flags & (MINUS | ZERO)) == 0)
 350     output_dup (' ', pad, output, aux);
 351   if (bp < buf_end && strchr (digits, *bp) == NULL)
 352     output (*bp++, aux);
 353   if (c->flags & ZERO)
 354     output_dup ('0', pad, output, aux);
 355   while (bp < buf_end)
 356     output (*bp++, aux);
 357   if (c->flags & MINUS)
 358     output_dup (' ', pad, output, aux);
 359 }
 360
 361 static void
 362 printf_string (const char *string, size_t length,
 363                struct printf_conversion *c,
 364                void (*output) (char, void *), void *aux)
 365 {
 366   if (c->width > 1 && (c->flags & MINUS) == 0)
 367     output_dup (' ', c->width - 1, output, aux);
 368   while (length-- > 0)
 369     output (*string++, aux);
 370   if (c->width > 1 && (c->flags & MINUS) != 0)
 371     output_dup (' ', c->width - 1, output, aux);
 372 }
 373
 374 static void
 375 printf_core (const char *format,
 376              void (*output) (char, void *), void *aux, ...)
 377 {
 378   va_list args;
 379
 380   va_start (args, aux);
 381   vprintf_core (format, args, output, aux);
 382   va_end (args);
 383 }
 384
 385 static void
 386 vprintf_core (const char *format, va_list args,
 387               void (*output) (char, void *), void *aux)
 388 {
 389   for (; *format != '\0'; format++)
 390     {
 391       struct printf_conversion c;
 392
 393       /* Literally copy non-conversions to output. */
 394       if (*format != '%')
 395         {
 396           output (*format, aux);
 397           continue;
 398         }
 399       format++;
 400
 401       /* %% => %. */
 402       if (*format == '%')
 403         {
 404           output ('%', aux);
 405           continue;
 406         }
 407
 408       format = parse_conversion (format, &c, &args);
 409       switch (*format)
 410         {
 411         case 'd':
 412         case 'i':
 413           {
 414             intmax_t value;
 415             uintmax_t abs_value;
 416             bool negative = false;
 417
 418             switch (c.type)
 419               {
 420               case CHAR:
 421                 value = (signed char) va_arg (args, int);
 422                 break;
 423               case SHORT:
 424                 value = (short) va_arg (args, int);
 425                 break;
 426               case INT:
 427                 value = va_arg (args, int);
 428                 break;
 429               case LONG:
 430                 value = va_arg (args, long);
 431                 break;
 432               case LONGLONG:
 433                 value = va_arg (args, long long);
 434                 break;
 435               case PTRDIFFT:
 436                 value = va_arg (args, ptrdiff_t);
 437                 break;
 438               case SIZET:
 439                 value = va_arg (args, size_t);
 440                 break;
 441               default:
 442                 NOT_REACHED ();
 443               }
 444
 445             if (value < 0)
 446               {
 447                 negative = true;
 448                 abs_value = -value;
 449               }
 450             else
 451               abs_value = value;
 452
 453             printf_integer (abs_value, negative, "0123456789",
 454                             &c, output, aux);
 455           }
 456           break;
 457
 458         case 'o':
 459         case 'u':
 460         case 'x':
 461         case 'X':
 462           {
 463             uintmax_t value;
 464             const char *digits;
 465
 466             switch (c.type)
 467               {
 468               case CHAR:
 469                 value = (unsigned char) va_arg (args, unsigned);
 470                 break;
 471               case SHORT:
 472                 value = (unsigned short) va_arg (args, unsigned);
 473                 break;
 474               case INT:
 475                 value = va_arg (args, unsigned);
 476                 break;
 477               case LONG:
 478                 value = va_arg (args, unsigned long);
 479                 break;
 480               case LONGLONG:
 481                 value = va_arg (args, unsigned long long);
 482                 break;
 483               case PTRDIFFT:
 484                 value = va_arg (args, ptrdiff_t);
 485                 break;
 486               case SIZET:
 487                 value = va_arg (args, size_t);
 488                 break;
 489               default:
 490                 NOT_REACHED ();
 491               }
 492
 493             switch (*format)
 494               {
 495               case 'o':
 496                 digits = "01234567";
 497                 break;
 498               case 'u':
 499                 digits = "0123456789";
 500                 break;
 501               case 'x':
 502                 digits = "0123456789abcdef";
 503                 break;
 504               case 'X':
 505                 digits = "0123456789ABCDEF";
 506                 break;
 507               default:
 508                 NOT_REACHED ();
 509               }
 510
 511             printf_integer (value, false, digits, &c, output, aux);
 512           }
 513           break;
 514
 515         case 'c':
 516           {
 517             char ch = va_arg (args, int);
 518             printf_string (&ch, 1, &c, output, aux);
 519           }
 520           break;
 521
 522         case 's':
 523           {
 524             const char *s;
 525             size_t length;
 526
 527             s = va_arg (args, char *);
 528             if (s == NULL)
 529               s = "(null)";
 530
 531             if (c.precision >= 0)
 532               {
 533                 const char *zero = memchr (s, '\0', c.precision);
 534                 if (zero != NULL)
 535                   length = zero - s;
 536                 else
 537                   length = c.precision;
 538               }
 539             else
 540               length = strlen (s);
 541
 542             printf_string (s, length, &c, output, aux);
 543           }
 544           break;
 545
 546         case 'p':
 547           {
 548             void *p = va_arg (args, void *);
 549
 550             c.flags = POUND;
 551             if (p != NULL)
 552               printf_integer ((uintptr_t) p,
 553                               false, "0123456789abcdef", &c,
 554                               output, aux);
 555             else
 556               printf_string ("(nil)", 5, &c, output, aux);
 557           }
 558           break;
 559
 560         case 'f':
 561         case 'e':
 562         case 'E':
 563         case 'g':
 564         case 'G':
 565         case 'n':
 566           printf_core ("<<no %%%c in kernel>>", output, aux, *format);
 567           break;
 568
 569         default:
 570           printf_core ("<<no %%%c conversion>>", output, aux, *format);
 571           break;
 572         }
 573     }
 574 }