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;
 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   while (value > 0)
 382     {
 383       if ((c->flags & GROUP) && cp > buf && (cp - buf) % 3 == 0)
 384         *cp++ = ',';
 385       *cp++ = digits[value % base];
 386       value /= base;
 387     }
 388
 389   /* Append enough zeros to match precision.
 390      If precision is 0, then a value of zero is rendered as a
 391      null string.  Otherwise at least one digit is presented. */
 392   if (c->precision < 0)
 393     c->precision = 1;
 394   while (cp - buf < c->precision && cp - buf < (int) sizeof buf - 8)
 395     *cp++ = '0';
 396
 397   /* Append sign. */
 398   if (c->flags & PLUS)
 399     *cp++ = negative ? '-' : '+';
 400   else if (c->flags & SPACE)
 401     *cp++ = negative ? '-' : ' ';
 402   else if (negative)
 403     *cp++ = '-';
 404
 405   /* Get name of base. */
 406   base_name = "";
 407   if (c->flags & POUND)
 408     {
 409       if (base == 8)
 410         base_name = "0";
 411       else if (base == 16)
 412         base_name = digits[0xa] == 'a' ? "0x" : "0X";
 413     }
 414
 415   /* Calculate number of pad characters to fill field width. */
 416   pad_cnt = c->width - (cp - buf) - strlen (base_name);
 417   if (pad_cnt < 0)
 418     pad_cnt = 0;
 419
 420   /* Do output. */
 421   if ((c->flags & (MINUS | ZERO)) == 0)
 422     output_dup (' ', pad_cnt, output, aux);
 423   while (*base_name != '\0')
 424     output (*base_name++, aux);
 425   if (c->flags & ZERO)
 426     output_dup ('0', pad_cnt, output, aux);
 427   while (cp > buf)
 428     output (*--cp, aux);
 429   if (c->flags & MINUS)
 430     output_dup (' ', pad_cnt, output, aux);
 431 }
 432
 433 static void
 434 printf_string (const char *string, size_t length,
 435                struct printf_conversion *c,
 436                void (*output) (char, void *), void *aux)
 437 {
 438   if (c->width > 1 && (c->flags & MINUS) == 0)
 439     output_dup (' ', c->width - 1, output, aux);
 440   while (length-- > 0)
 441     output (*string++, aux);
 442   if (c->width > 1 && (c->flags & MINUS) != 0)
 443     output_dup (' ', c->width - 1, output, aux);
 444 }
 445
 446 static void
 447 printf_core (const char *format,
 448              void (*output) (char, void *), void *aux, ...)
 449 {
 450   va_list args;
 451
 452   va_start (args, aux);
 453   vprintf_core (format, args, output, aux);
 454   va_end (args);
 455 }
 456
 457 static void
 458 vprintf_core (const char *format, va_list args,
 459               void (*output) (char, void *), void *aux)
 460 {
 461   for (; *format != '\0'; format++)
 462     {
 463       struct printf_conversion c;
 464
 465       /* Literally copy non-conversions to output. */
 466       if (*format != '%')
 467         {
 468           output (*format, aux);
 469           continue;
 470         }
 471       format++;
 472
 473       /* %% => %. */
 474       if (*format == '%')
 475         {
 476           output ('%', aux);
 477           continue;
 478         }
 479
 480       format = parse_conversion (format, &c, &args);
 481       switch (*format)
 482         {
 483         case 'd':
 484         case 'i':
 485           {
 486             intmax_t value;
 487             uintmax_t abs_value;
 488             bool negative = false;
 489
 490             switch (c.type)
 491               {
 492               case CHAR:
 493                 value = (signed char) va_arg (args, int);
 494                 break;
 495               case SHORT:
 496                 value = (short) va_arg (args, int);
 497                 break;
 498               case INT:
 499                 value = va_arg (args, int);
 500                 break;
 501               case LONG:
 502                 value = va_arg (args, long);
 503                 break;
 504               case LONGLONG:
 505                 value = va_arg (args, long long);
 506                 break;
 507               case PTRDIFFT:
 508                 value = va_arg (args, ptrdiff_t);
 509                 break;
 510               case SIZET:
 511                 value = va_arg (args, size_t);
 512                 break;
 513               default:
 514                 NOT_REACHED ();
 515               }
 516
 517             if (value < 0)
 518               {
 519                 negative = true;
 520                 abs_value = -value;
 521               }
 522             else
 523               abs_value = value;
 524
 525             printf_integer (abs_value, negative, "0123456789",
 526                             &c, output, aux);
 527           }
 528           break;
 529
 530         case 'o':
 531         case 'u':
 532         case 'x':
 533         case 'X':
 534           {
 535             uintmax_t value;
 536             const char *digits;
 537
 538             switch (c.type)
 539               {
 540               case CHAR:
 541                 value = (unsigned char) va_arg (args, unsigned);
 542                 break;
 543               case SHORT:
 544                 value = (unsigned short) va_arg (args, unsigned);
 545                 break;
 546               case INT:
 547                 value = va_arg (args, unsigned);
 548                 break;
 549               case LONG:
 550                 value = va_arg (args, unsigned long);
 551                 break;
 552               case LONGLONG:
 553                 value = va_arg (args, unsigned long long);
 554                 break;
 555               case PTRDIFFT:
 556                 value = va_arg (args, ptrdiff_t);
 557                 break;
 558               case SIZET:
 559                 value = va_arg (args, size_t);
 560                 break;
 561               default:
 562                 NOT_REACHED ();
 563               }
 564
 565             switch (*format)
 566               {
 567               case 'o':
 568                 digits = "01234567";
 569                 break;
 570               case 'u':
 571                 digits = "0123456789";
 572                 break;
 573               case 'x':
 574                 digits = "0123456789abcdef";
 575                 break;
 576               case 'X':
 577                 digits = "0123456789ABCDEF";
 578                 break;
 579               default:
 580                 NOT_REACHED ();
 581               }
 582
 583             printf_integer (value, false, digits, &c, output, aux);
 584           }
 585           break;
 586
 587         case 'c':
 588           {
 589             char ch = va_arg (args, int);
 590             printf_string (&ch, 1, &c, output, aux);
 591           }
 592           break;
 593
 594         case 's':
 595           {
 596             const char *s;
 597             size_t length;
 598
 599             s = va_arg (args, char *);
 600             if (s == NULL)
 601               s = "(null)";
 602
 603             if (c.precision >= 0)
 604               {
 605                 const char *zero = memchr (s, '\0', c.precision);
 606                 if (zero != NULL)
 607                   length = zero - s;
 608                 else
 609                   length = c.precision;
 610               }
 611             else
 612               length = strlen (s);
 613
 614             printf_string (s, length, &c, output, aux);
 615           }
 616           break;
 617
 618         case 'p':
 619           {
 620             void *p = va_arg (args, void *);
 621
 622             c.flags = POUND;
 623             if (p != NULL)
 624               printf_integer ((uintptr_t) p,
 625                               false, "0123456789abcdef", &c,
 626                               output, aux);
 627             else
 628               printf_string ("(nil)", 5, &c, output, aux);
 629           }
 630           break;
 631
 632         case 'f':
 633         case 'e':
 634         case 'E':
 635         case 'g':
 636         case 'G':
 637         case 'n':
 638           printf_core ("<<no %%%c in kernel>>", output, aux, *format);
 639           break;
 640
 641         default:
 642           printf_core ("<<no %%%c conversion>>", output, aux, *format);
 643           break;
 644         }
 645     }
 646 }
 647 \f
 648 void
 649 hex_dump (const void *buffer, size_t size)
 650 {
 651   const size_t n_per_line = 16;
 652   const uint8_t *p = buffer;
 653   size_t ofs = 0;
 654
 655   while (size > 0)
 656     {
 657       size_t n, i;
 658
 659       printk ("%08zx", ofs);
 660       n = size >= n_per_line ? n_per_line : size;
 661       for (i = 0; i < n; i++)
 662         printk ("%c%02x", i == n / 2 ? '-' : ' ', (unsigned) p[i]);
 663       for (; i < n_per_line; i++)
 664         printk ("   ");
 665       printk (" |");
 666       for (i = 0; i < n; i++)
 667         printk ("%c", isprint (p[i]) ? p[i] : '.');
 668       for (; i < n_per_line; i++)
 669         printk (" ");
 670       printk ("|\n");
 671
 672       p += n;
 673       ofs += n;
 674       size -= n;
 675     }
 676 }