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 /* Sets the SIZE bytes in DST to VALUE. */
  12 void *
  13 memset (void *dst_, int value, size_t size)
  14 {
  15   unsigned char *dst = dst_;
  16
  17   ASSERT (dst != NULL || size == 0);
  18
  19   while (size-- > 0)
  20     *dst++ = value;
  21
  22   return dst_;
  23 }
  24
  25 /* Copies SIZE bytes from SRC to DST, which must not overlap.
  26    Returns DST. */
  27 void *
  28 memcpy (void *dst_, const void *src_, size_t size)
  29 {
  30   unsigned char *dst = dst_;
  31   const unsigned char *src = src_;
  32
  33   ASSERT (dst != NULL || size == 0);
  34   ASSERT (src != NULL || size == 0);
  35
  36   while (size-- > 0)
  37     *dst++ = *src++;
  38
  39   return dst_;
  40 }
  41
  42 /* Copies SIZE bytes from SRC to DST, which are allowed to
  43    overlap.  Returns DST. */
  44 void *
  45 memmove (void *dst_, const void *src_, size_t size)
  46 {
  47   unsigned char *dst = dst_;
  48   const unsigned char *src = src_;
  49
  50   ASSERT (dst != NULL || size == 0);
  51   ASSERT (src != NULL || size == 0);
  52
  53   if (dst < src)
  54     {
  55       while (size-- > 0)
  56         *dst++ = *src++;
  57     }
  58   else
  59     {
  60       dst += size;
  61       src += size;
  62       while (size-- > 0)
  63         *--dst = *--src;
  64     }
  65
  66   return dst;
  67 }
  68
  69 /* Returns a pointer to the first occurrence of CH in the first
  70    SIZE bytes starting at BLOCK.  Returns a null pointer if CH
  71    does not occur in BLOCK. */
  72 void *
  73 memchr (const void *block_, int ch_, size_t size)
  74 {
  75   const unsigned char *block = block_;
  76   unsigned char ch = ch_;
  77
  78   ASSERT (block != NULL || size == 0);
  79
  80   for (; size-- > 0; block++)
  81     if (*block == ch)
  82       return (void *) block;
  83
  84   return NULL;
  85 }
  86
  87 /* Find the first differing byte in the two blocks of SIZE bytes
  88    at A and B.  Returns a positive value if the byte in A is
  89    greater, a negative value if the byte in B is greater, or zero
  90    if blocks A and B are equal. */
  91 int
  92 memcmp (const void *a_, const void *b_, size_t size)
  93 {
  94   const unsigned char *a = a_;
  95   const unsigned char *b = b_;
  96
  97   ASSERT (a != NULL || size == 0);
  98   ASSERT (b != NULL || size == 0);
  99
 100   for (; size-- > 0; a++, b++)
 101     if (*a != *b)
 102       return *a > *b ? +1 : -1;
 103   return 0;
 104 }
 105
 106 /* Copies string SRC to DST.  If SRC is longer than SIZE - 1
 107    characters, only SIZE - 1 characters are copied.  A null
 108    terminator is always written to DST, unless SIZE is 0.
 109    Returns the length of SRC. */
 110 size_t
 111 strlcpy (char *dst, const char *src, size_t size)
 112 {
 113   size_t src_len;
 114
 115   ASSERT (dst != NULL);
 116   ASSERT (src != NULL);
 117
 118   src_len = strlen (src);
 119   if (size > 0)
 120     {
 121       size_t dst_len = size - 1;
 122       if (src_len < dst_len)
 123         src_len = dst_len;
 124       memcpy (dst, src, dst_len);
 125       dst[dst_len] = '\0';
 126     }
 127   return src_len;
 128 }
 129
 130 /* Returns the length of STRING. */
 131 size_t
 132 strlen (const char *string)
 133 {
 134   const char *p;
 135
 136   ASSERT (string != NULL);
 137
 138   for (p = string; *p != '\0'; p++)
 139     continue;
 140   return p - string;
 141 }
 142
 143 /* Finds and returns the first occurrence of C in STRING, or a
 144    null pointer if C does not appear in STRING.  If C == '\0'
 145    then returns a pointer to the null terminator at the end of
 146    STRING. */
 147 char *
 148 strchr (const char *string, int c_)
 149 {
 150   char c = c_;
 151
 152   ASSERT (string != NULL);
 153
 154   for (;;)
 155     if (*string == c)
 156       return (char *) string;
 157     else if (*string == '\0')
 158       return NULL;
 159     else
 160       string++;
 161 }
 162
 163 /* Finds the first differing characters in strings A and B.
 164    Returns a positive value if the character in A (as an unsigned
 165    char) is greater, a negative value if the character in B (as
 166    an unsigned char) is greater, or zero if strings A and B are
 167    equal. */
 168 int
 169 strcmp (const char *a_, const char *b_)
 170 {
 171   const unsigned char *a = (const unsigned char *) a_;
 172   const unsigned char *b = (const unsigned char *) b_;
 173
 174   ASSERT (a != NULL);
 175   ASSERT (b != NULL);
 176
 177   while (*a != '\0' && *a == *b)
 178     {
 179       a++;
 180       b++;
 181     }
 182
 183   return *a < *b ? -1 : *a > *b;
 184 }
 185
 186 char *
 187 strtok_r (char *s, const char *delimiters, char **save_ptr)
 188 {
 189   char *token;
 190
 191   ASSERT (delimiters != NULL);
 192   ASSERT (save_ptr != NULL);
 193
 194   /* If S is nonnull, start from it.
 195      If S is null, start from saved position. */
 196   if (s == NULL)
 197     s = *save_ptr;
 198   ASSERT (s != NULL);
 199
 200   /* Skip any DELIMITERS at our current position. */
 201   while (strchr (delimiters, *s) != NULL)
 202     {
 203       /* strchr() will always return nonnull if we're searching
 204          for a null byte, because every string contains a null
 205          byte (at the end). */
 206       if (*s == '\0')
 207         {
 208           *save_ptr = s;
 209           return NULL;
 210         }
 211
 212       s++;
 213     }
 214
 215   /* Skip any non-DELIMITERS up to the end of the string. */
 216   token = s;
 217   while (strchr (delimiters, *s) == NULL)
 218     s++;
 219   if (*s != '\0')
 220     {
 221       *s = '\0';
 222       *save_ptr = s + 1;
 223     }
 224   else
 225     *save_ptr = s;
 226   return token;
 227 }
 228
 229 int
 230 atoi (const char *s)
 231 {
 232   bool negative;
 233   int value;
 234
 235   /* Skip white space. */
 236   while (isspace (*s))
 237     s++;
 238
 239   /* Parse sign. */
 240   negative = false;
 241   if (*s == '+')
 242     s++;
 243   else if (*s == '-')
 244     {
 245       negative = true;
 246       s++;
 247     }
 248
 249   /* Parse digits.  We always initially parse the value as
 250      negative, and then make it positive later, because the
 251      negative range of an int is bigger than the positive range
 252      on a 2's complement system. */
 253   for (value = 0; isdigit (*s); s++)
 254     value = value * 10 - (*s - '0');
 255   if (!negative)
 256     value = -value;
 257
 258   return value;
 259 }
 260 \f
 261 static void
 262 vprintf_core (const char *format, va_list args,
 263               void (*output) (char, void *), void *aux);
 264
 265 static void
 266 vprintk_helper (char ch, void *aux UNUSED)
 267 {
 268   vga_putc (ch);
 269   serial_outb (ch);
 270 }
 271
 272 void
 273 vprintk (const char *format, va_list args)
 274 {
 275   enum intr_level old_level = intr_disable ();
 276   vprintf_core (format, args, vprintk_helper, NULL);
 277   intr_set_level (old_level);
 278 }
 279
 280 void
 281 printk (const char *format, ...)
 282 {
 283   va_list args;
 284
 285   va_start (args, format);
 286   vprintk (format, args);
 287   va_end (args);
 288 }
 289 \f
 290 struct vsnprintf_aux
 291   {
 292     char *p;
 293     int length;
 294     int max_length;
 295   };
 296
 297 static void
 298 vsnprintf_helper (char ch, void *aux_)
 299 {
 300   struct vsnprintf_aux *aux = aux_;
 301
 302   if (aux->length++ < aux->max_length)
 303     *aux->p++ = ch;
 304 }
 305
 306 int
 307 vsnprintf (char *buffer, size_t buf_size,
 308            const char *format, va_list args)
 309 {
 310   struct vsnprintf_aux aux;
 311   aux.p = buffer;
 312   aux.length = 0;
 313   aux.max_length = buf_size > 0 ? buf_size - 1 : 0;
 314
 315   vprintf_core (format, args, vsnprintf_helper, &aux);
 316
 317   if (buf_size > 0)
 318     *aux.p = '\0';
 319
 320   return aux.length;
 321 }
 322
 323 int
 324 snprintf (char *buffer, size_t buf_size,
 325           const char *format, ...)
 326 {
 327   va_list args;
 328   int retval;
 329
 330   va_start (args, format);
 331   retval = vsnprintf (buffer, buf_size, format, args);
 332   va_end (args);
 333
 334   return retval;
 335 }
 336 \f
 337 /* printf() and friends internals.  You do not need to understand
 338    this code. */
 339
 340 struct printf_conversion
 341   {
 342     enum
 343       {
 344         MINUS = 1 << 0,
 345         PLUS = 1 << 1,
 346         SPACE = 1 << 2,
 347         POUND = 1 << 3,
 348         ZERO = 1 << 4,
 349         GROUP = 1 << 5
 350       }
 351     flags;
 352
 353     int width;
 354     int precision;
 355
 356     enum
 357       {
 358         CHAR = 1,
 359         SHORT = 2,
 360         INT = 3,
 361         INTMAX = 4,
 362         LONG = 5,
 363         LONGLONG = 6,
 364         PTRDIFFT = 7,
 365         SIZET = 8
 366       }
 367     type;
 368   };
 369
 370 static const char *
 371 parse_conversion (const char *format, struct printf_conversion *c,
 372                   va_list *args)
 373 {
 374   /* Parse flag characters. */
 375   c->flags = 0;
 376   for (;;)
 377     {
 378       switch (*format++)
 379         {
 380         case '-':
 381           c->flags |= MINUS;
 382           break;
 383         case '+':
 384           c->flags |= PLUS;
 385           break;
 386         case ' ':
 387           c->flags |= SPACE;
 388           break;
 389         case '#':
 390           c->flags |= POUND;
 391           break;
 392         case '0':
 393           c->flags |= ZERO;
 394           break;
 395         case '\'':
 396           c->flags |= GROUP;
 397           break;
 398         default:
 399           format--;
 400           goto not_a_flag;
 401         }
 402     }
 403  not_a_flag:
 404   if (c->flags & MINUS)
 405     c->flags &= ~ZERO;
 406   if (c->flags & PLUS)
 407     c->flags &= ~SPACE;
 408
 409   /* Parse field width. */
 410   c->width = 0;
 411   if (*format == '*')
 412     {
 413       format++;
 414       c->width = va_arg (*args, int);
 415     }
 416   else
 417     {
 418       for (; isdigit (*format); format++)
 419         c->width = c->width * 10 + *format - '0';
 420     }
 421   if (c->width < 0)
 422     {
 423       c->width = -c->width;
 424       c->flags |= MINUS;
 425     }
 426
 427   /* Parse precision. */
 428   c->precision = -1;
 429   if (*format == '.')
 430     {
 431       format++;
 432       if (*format == '*')
 433         {
 434           format++;
 435           c->precision = va_arg (*args, int);
 436         }
 437       else
 438         {
 439           c->precision = 0;
 440           for (; isdigit (*format); format++)
 441             c->precision = c->precision * 10 + *format - '0';
 442         }
 443       if (c->precision < 0)
 444         c->precision = -1;
 445     }
 446   if (c->precision >= 0)
 447     c->flags &= ~ZERO;
 448
 449   /* Parse type. */
 450   c->type = INT;
 451   switch (*format++)
 452     {
 453     case 'h':
 454       if (*format == 'h')
 455         {
 456           format++;
 457           c->type = CHAR;
 458         }
 459       else
 460         c->type = SHORT;
 461       break;
 462
 463     case 'j':
 464       c->type = INTMAX;
 465       break;
 466
 467     case 'l':
 468       if (*format == 'l')
 469         {
 470           format++;
 471           c->type = LONGLONG;
 472         }
 473       else
 474         c->type = LONG;
 475       break;
 476
 477     case 'L':
 478     case 'q':
 479       c->type = LONGLONG;
 480       break;
 481
 482     case 't':
 483       c->type = PTRDIFFT;
 484       break;
 485
 486     case 'z':
 487     case 'Z':
 488       c->type = SIZET;
 489       break;
 490
 491     default:
 492       format--;
 493       break;
 494     }
 495
 496   return format;
 497 }
 498
 499 static void
 500 output_dup (char ch, size_t cnt, void (*output) (char, void *), void *aux)
 501 {
 502   while (cnt-- > 0)
 503     output (ch, aux);
 504 }
 505
 506 static void
 507 printf_integer (uintmax_t value, bool negative, const char *digits,
 508                 struct printf_conversion *c,
 509                 void (*output) (char, void *), void *aux)
 510
 511 {
 512   char buf[64], *cp;
 513   int base;
 514   const char *base_name;
 515   int pad_cnt, group_cnt;
 516
 517   base = strlen (digits);
 518
 519   /* Accumulate digits into buffer.
 520      This algorithm produces digits in reverse order, so later we
 521      will output the buffer's content in reverse.  This is also
 522      the reason that later we append zeros and the sign. */
 523   cp = buf;
 524   group_cnt = 0;
 525   while (value > 0)
 526     {
 527       if ((c->flags & GROUP) && group_cnt++ == 3)
 528         {
 529           *cp++ = ',';
 530           group_cnt = 0;
 531         }
 532       *cp++ = digits[value % base];
 533       value /= base;
 534     }
 535
 536   /* Append enough zeros to match precision.
 537      If precision is 0, then a value of zero is rendered as a
 538      null string.  Otherwise at least one digit is presented. */
 539   if (c->precision < 0)
 540     c->precision = 1;
 541   while (cp - buf < c->precision && cp - buf < (int) sizeof buf - 8)
 542     *cp++ = '0';
 543
 544   /* Append sign. */
 545   if (c->flags & PLUS)
 546     *cp++ = negative ? '-' : '+';
 547   else if (c->flags & SPACE)
 548     *cp++ = negative ? '-' : ' ';
 549   else if (negative)
 550     *cp++ = '-';
 551
 552   /* Get name of base. */
 553   base_name = "";
 554   if (c->flags & POUND)
 555     {
 556       if (base == 8)
 557         base_name = "0";
 558       else if (base == 16)
 559         base_name = digits[0xa] == 'a' ? "0x" : "0X";
 560     }
 561
 562   /* Calculate number of pad characters to fill field width. */
 563   pad_cnt = c->width - (cp - buf) - strlen (base_name);
 564   if (pad_cnt < 0)
 565     pad_cnt = 0;
 566
 567   /* Do output. */
 568   if ((c->flags & (MINUS | ZERO)) == 0)
 569     output_dup (' ', pad_cnt, output, aux);
 570   while (*base_name != '\0')
 571     output (*base_name++, aux);
 572   if (c->flags & ZERO)
 573     output_dup ('0', pad_cnt, output, aux);
 574   while (cp > buf)
 575     output (*--cp, aux);
 576   if (c->flags & MINUS)
 577     output_dup (' ', pad_cnt, output, aux);
 578 }
 579
 580 static void
 581 printf_string (const char *string, size_t length,
 582                struct printf_conversion *c,
 583                void (*output) (char, void *), void *aux)
 584 {
 585   if (c->width > 1 && (c->flags & MINUS) == 0)
 586     output_dup (' ', c->width - 1, output, aux);
 587   while (length-- > 0)
 588     output (*string++, aux);
 589   if (c->width > 1 && (c->flags & MINUS) != 0)
 590     output_dup (' ', c->width - 1, output, aux);
 591 }
 592
 593 static void
 594 printf_core (const char *format,
 595              void (*output) (char, void *), void *aux, ...)
 596 {
 597   va_list args;
 598
 599   va_start (args, aux);
 600   vprintf_core (format, args, output, aux);
 601   va_end (args);
 602 }
 603
 604 static void
 605 vprintf_core (const char *format, va_list args,
 606               void (*output) (char, void *), void *aux)
 607 {
 608   for (; *format != '\0'; format++)
 609     {
 610       struct printf_conversion c;
 611
 612       /* Literally copy non-conversions to output. */
 613       if (*format != '%')
 614         {
 615           output (*format, aux);
 616           continue;
 617         }
 618       format++;
 619
 620       /* %% => %. */
 621       if (*format == '%')
 622         {
 623           output ('%', aux);
 624           continue;
 625         }
 626
 627       format = parse_conversion (format, &c, &args);
 628       switch (*format)
 629         {
 630         case 'd':
 631         case 'i':
 632           {
 633             intmax_t value;
 634             uintmax_t abs_value;
 635             bool negative = false;
 636
 637             switch (c.type)
 638               {
 639               case CHAR:
 640                 value = (signed char) va_arg (args, int);
 641                 break;
 642               case SHORT:
 643                 value = (short) va_arg (args, int);
 644                 break;
 645               case INT:
 646                 value = va_arg (args, int);
 647                 break;
 648               case LONG:
 649                 value = va_arg (args, long);
 650                 break;
 651               case LONGLONG:
 652                 value = va_arg (args, long long);
 653                 break;
 654               case PTRDIFFT:
 655                 value = va_arg (args, ptrdiff_t);
 656                 break;
 657               case SIZET:
 658                 value = va_arg (args, size_t);
 659                 break;
 660               default:
 661                 NOT_REACHED ();
 662               }
 663
 664             if (value < 0)
 665               {
 666                 negative = true;
 667                 abs_value = -value;
 668               }
 669             else
 670               abs_value = value;
 671
 672             printf_integer (abs_value, negative, "0123456789",
 673                             &c, output, aux);
 674           }
 675           break;
 676
 677         case 'o':
 678         case 'u':
 679         case 'x':
 680         case 'X':
 681           {
 682             uintmax_t value;
 683             const char *digits;
 684
 685             switch (c.type)
 686               {
 687               case CHAR:
 688                 value = (unsigned char) va_arg (args, unsigned);
 689                 break;
 690               case SHORT:
 691                 value = (unsigned short) va_arg (args, unsigned);
 692                 break;
 693               case INT:
 694                 value = va_arg (args, unsigned);
 695                 break;
 696               case LONG:
 697                 value = va_arg (args, unsigned long);
 698                 break;
 699               case LONGLONG:
 700                 value = va_arg (args, unsigned long long);
 701                 break;
 702               case PTRDIFFT:
 703                 value = va_arg (args, ptrdiff_t);
 704                 break;
 705               case SIZET:
 706                 value = va_arg (args, size_t);
 707                 break;
 708               default:
 709                 NOT_REACHED ();
 710               }
 711
 712             switch (*format)
 713               {
 714               case 'o':
 715                 digits = "01234567";
 716                 break;
 717               case 'u':
 718                 digits = "0123456789";
 719                 break;
 720               case 'x':
 721                 digits = "0123456789abcdef";
 722                 break;
 723               case 'X':
 724                 digits = "0123456789ABCDEF";
 725                 break;
 726               default:
 727                 NOT_REACHED ();
 728               }
 729
 730             printf_integer (value, false, digits, &c, output, aux);
 731           }
 732           break;
 733
 734         case 'c':
 735           {
 736             char ch = va_arg (args, int);
 737             printf_string (&ch, 1, &c, output, aux);
 738           }
 739           break;
 740
 741         case 's':
 742           {
 743             const char *s;
 744             size_t length;
 745
 746             s = va_arg (args, char *);
 747             if (s == NULL)
 748               s = "(null)";
 749
 750             if (c.precision >= 0)
 751               {
 752                 const char *zero = memchr (s, '\0', c.precision);
 753                 if (zero != NULL)
 754                   length = zero - s;
 755                 else
 756                   length = c.precision;
 757               }
 758             else
 759               length = strlen (s);
 760
 761             printf_string (s, length, &c, output, aux);
 762           }
 763           break;
 764
 765         case 'p':
 766           {
 767             void *p = va_arg (args, void *);
 768
 769             c.flags = POUND;
 770             if (p != NULL)
 771               printf_integer ((uintptr_t) p,
 772                               false, "0123456789abcdef", &c,
 773                               output, aux);
 774             else
 775               printf_string ("(nil)", 5, &c, output, aux);
 776           }
 777           break;
 778
 779         case 'f':
 780         case 'e':
 781         case 'E':
 782         case 'g':
 783         case 'G':
 784         case 'n':
 785           printf_core ("<<no %%%c in kernel>>", output, aux, *format);
 786           break;
 787
 788         default:
 789           printf_core ("<<no %%%c conversion>>", output, aux, *format);
 790           break;
 791         }
 792     }
 793 }
 794 \f
 795 void
 796 hex_dump (const void *buffer, size_t size, bool ascii)
 797 {
 798   const size_t n_per_line = 16;
 799   const uint8_t *p = buffer;
 800   size_t ofs = 0;
 801
 802   while (size > 0)
 803     {
 804       size_t n, i;
 805
 806       printk ("%08zx", ofs);
 807       n = size >= n_per_line ? n_per_line : size;
 808       for (i = 0; i < n; i++)
 809         printk ("%c%02x", i == n_per_line / 2 ? '-' : ' ', (unsigned) p[i]);
 810
 811       if (ascii)
 812         {
 813           for (; i < n_per_line; i++)
 814             printk ("   ");
 815           printk (" |");
 816           for (i = 0; i < n; i++)
 817             printk ("%c", isprint (p[i]) ? p[i] : '.');
 818           for (; i < n_per_line; i++)
 819             printk (" ");
 820           printk ("|");
 821         }
 822       printk ("\n");
 823
 824       p += n;
 825       ofs += n;
 826       size -= n;
 827     }
 828 }