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