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