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
 186   va_start (args, format);
 187   vsnprintf (buffer, buf_size, format, args);
 188   va_end (args);
 189 }
 190 \f
 191 /* printf() and friends internals.  You do not need to understand
 192    this code. */
 193
 194 struct printf_conversion
 195   {
 196     enum
 197       {
 198         MINUS = 1 << 0,
 199         PLUS = 1 << 1,
 200         SPACE = 1 << 2,
 201         POUND = 1 << 3,
 202         ZERO = 1 << 4
 203       }
 204     flags;
 205
 206     int width;
 207     int precision;
 208
 209     enum
 210       {
 211         CHAR = 1,
 212         SHORT = 2,
 213         INT = 3,
 214         INTMAX = 4,
 215         LONG = 5,
 216         LONGLONG = 6,
 217         PTRDIFFT = 7,
 218         SIZET = 8
 219       }
 220     type;
 221   };
 222
 223 static const char *
 224 parse_conversion (const char *format, struct printf_conversion *c,
 225                   va_list *args)
 226 {
 227   /* Parse flag characters. */
 228   c->flags = 0;
 229   for (;;)
 230     {
 231       switch (*format++)
 232         {
 233         case '-':
 234           c->flags |= MINUS;
 235           break;
 236         case '+':
 237           c->flags |= PLUS;
 238           break;
 239         case ' ':
 240           c->flags |= SPACE;
 241           break;
 242         case '#':
 243           c->flags |= POUND;
 244           break;
 245         case '0':
 246           c->flags |= ZERO;
 247           break;
 248         default:
 249           format--;
 250           goto not_a_flag;
 251         }
 252     }
 253  not_a_flag:
 254   if (c->flags & MINUS)
 255     c->flags &= ~ZERO;
 256   if (c->flags & PLUS)
 257     c->flags &= ~SPACE;
 258
 259   /* Parse field width. */
 260   c->width = 0;
 261   if (*format == '*')
 262     {
 263       format++;
 264       c->width = va_arg (*args, int);
 265     }
 266   else
 267     {
 268       for (; isdigit (*format); format++)
 269         c->width = c->width * 10 + *format - '0';
 270     }
 271   if (c->width < 0)
 272     {
 273       c->width = -c->width;
 274       c->flags |= MINUS;
 275     }
 276
 277   /* Parse precision. */
 278   c->precision = -1;
 279   if (*format == '.')
 280     {
 281       format++;
 282       if (*format == '*')
 283         {
 284           format++;
 285           c->precision = va_arg (*args, int);
 286         }
 287       else
 288         {
 289           c->precision = 0;
 290           for (; isdigit (*format); format++)
 291             c->precision = c->precision * 10 + *format - '0';
 292         }
 293       if (c->precision < 0)
 294         c->precision = -1;
 295     }
 296   if (c->precision >= 0)
 297     c->flags &= ~ZERO;
 298
 299   /* Parse type. */
 300   c->type = INT;
 301   switch (*format++)
 302     {
 303     case 'h':
 304       if (*format == 'h')
 305         {
 306           format++;
 307           c->type = CHAR;
 308         }
 309       else
 310         c->type = SHORT;
 311       break;
 312
 313     case 'j':
 314       c->type = INTMAX;
 315       break;
 316
 317     case 'l':
 318       if (*format == 'l')
 319         {
 320           format++;
 321           c->type = LONGLONG;
 322         }
 323       else
 324         c->type = LONG;
 325       break;
 326
 327     case 'L':
 328     case 'q':
 329       c->type = LONGLONG;
 330       break;
 331
 332     case 't':
 333       c->type = PTRDIFFT;
 334       break;
 335
 336     case 'z':
 337     case 'Z':
 338       c->type = SIZET;
 339       break;
 340
 341     default:
 342       format--;
 343       break;
 344     }
 345
 346   return format;
 347 }
 348
 349 static void
 350 output_dup (char ch, size_t cnt, void (*output) (char, void *), void *aux)
 351 {
 352   while (cnt-- > 0)
 353     output (ch, aux);
 354 }
 355
 356 static void
 357 printf_integer (uintmax_t value, bool negative, const char *digits,
 358                 struct printf_conversion *c,
 359                 void (*output) (char, void *), void *aux)
 360
 361 {
 362   char buf[64], *cp;
 363   int base;
 364   const char *base_name;
 365   int pad_cnt;
 366
 367   base = strlen (digits);
 368
 369   /* Accumulate digits into buffer.
 370      This algorithm produces digits in reverse order, so later we
 371      will output the buffer's content in reverse.  This is also
 372      the reason that later we append zeros and the sign. */
 373   cp = buf;
 374   while (value > 0)
 375     {
 376       *cp++ = digits[value % base];
 377       value /= base;
 378     }
 379
 380   /* Append enough zeros to match precision.
 381      If precision is 0, then a value of zero is rendered as a
 382      null string.  Otherwise at least one digit is presented. */
 383   if (c->precision < 0)
 384     c->precision = 1;
 385   while (cp - buf < c->precision && cp - buf < (int) sizeof buf - 8)
 386     *cp++ = '0';
 387
 388   /* Append sign. */
 389   if (c->flags & PLUS)
 390     *cp++ = negative ? '-' : '+';
 391   else if (c->flags & SPACE)
 392     *cp++ = negative ? '-' : ' ';
 393   else if (negative)
 394     *cp++ = '-';
 395
 396   /* Get name of base. */
 397   base_name = "";
 398   if (c->flags & POUND)
 399     {
 400       if (base == 8)
 401         base_name = "0";
 402       else if (base == 16)
 403         base_name = digits[0xa] == 'a' ? "0x" : "0X";
 404     }
 405
 406   /* Calculate number of pad characters to fill field width. */
 407   pad_cnt = c->width - (cp - buf) - strlen (base_name);
 408   if (pad_cnt < 0)
 409     pad_cnt = 0;
 410
 411   /* Do output. */
 412   if ((c->flags & (MINUS | ZERO)) == 0)
 413     output_dup (' ', pad_cnt, output, aux);
 414   while (*base_name != '\0')
 415     output (*base_name++, aux);
 416   if (c->flags & ZERO)
 417     output_dup ('0', pad_cnt, output, aux);
 418   while (cp > buf)
 419     output (*--cp, aux);
 420   if (c->flags & MINUS)
 421     output_dup (' ', pad_cnt, output, aux);
 422 }
 423
 424 static void
 425 printf_string (const char *string, size_t length,
 426                struct printf_conversion *c,
 427                void (*output) (char, void *), void *aux)
 428 {
 429   if (c->width > 1 && (c->flags & MINUS) == 0)
 430     output_dup (' ', c->width - 1, output, aux);
 431   while (length-- > 0)
 432     output (*string++, aux);
 433   if (c->width > 1 && (c->flags & MINUS) != 0)
 434     output_dup (' ', c->width - 1, output, aux);
 435 }
 436
 437 static void
 438 printf_core (const char *format,
 439              void (*output) (char, void *), void *aux, ...)
 440 {
 441   va_list args;
 442
 443   va_start (args, aux);
 444   vprintf_core (format, args, output, aux);
 445   va_end (args);
 446 }
 447
 448 static void
 449 vprintf_core (const char *format, va_list args,
 450               void (*output) (char, void *), void *aux)
 451 {
 452   for (; *format != '\0'; format++)
 453     {
 454       struct printf_conversion c;
 455
 456       /* Literally copy non-conversions to output. */
 457       if (*format != '%')
 458         {
 459           output (*format, aux);
 460           continue;
 461         }
 462       format++;
 463
 464       /* %% => %. */
 465       if (*format == '%')
 466         {
 467           output ('%', aux);
 468           continue;
 469         }
 470
 471       format = parse_conversion (format, &c, &args);
 472       switch (*format)
 473         {
 474         case 'd':
 475         case 'i':
 476           {
 477             intmax_t value;
 478             uintmax_t abs_value;
 479             bool negative = false;
 480
 481             switch (c.type)
 482               {
 483               case CHAR:
 484                 value = (signed char) va_arg (args, int);
 485                 break;
 486               case SHORT:
 487                 value = (short) va_arg (args, int);
 488                 break;
 489               case INT:
 490                 value = va_arg (args, int);
 491                 break;
 492               case LONG:
 493                 value = va_arg (args, long);
 494                 break;
 495               case LONGLONG:
 496                 value = va_arg (args, long long);
 497                 break;
 498               case PTRDIFFT:
 499                 value = va_arg (args, ptrdiff_t);
 500                 break;
 501               case SIZET:
 502                 value = va_arg (args, size_t);
 503                 break;
 504               default:
 505                 NOT_REACHED ();
 506               }
 507
 508             if (value < 0)
 509               {
 510                 negative = true;
 511                 abs_value = -value;
 512               }
 513             else
 514               abs_value = value;
 515
 516             printf_integer (abs_value, negative, "0123456789",
 517                             &c, output, aux);
 518           }
 519           break;
 520
 521         case 'o':
 522         case 'u':
 523         case 'x':
 524         case 'X':
 525           {
 526             uintmax_t value;
 527             const char *digits;
 528
 529             switch (c.type)
 530               {
 531               case CHAR:
 532                 value = (unsigned char) va_arg (args, unsigned);
 533                 break;
 534               case SHORT:
 535                 value = (unsigned short) va_arg (args, unsigned);
 536                 break;
 537               case INT:
 538                 value = va_arg (args, unsigned);
 539                 break;
 540               case LONG:
 541                 value = va_arg (args, unsigned long);
 542                 break;
 543               case LONGLONG:
 544                 value = va_arg (args, unsigned long long);
 545                 break;
 546               case PTRDIFFT:
 547                 value = va_arg (args, ptrdiff_t);
 548                 break;
 549               case SIZET:
 550                 value = va_arg (args, size_t);
 551                 break;
 552               default:
 553                 NOT_REACHED ();
 554               }
 555
 556             switch (*format)
 557               {
 558               case 'o':
 559                 digits = "01234567";
 560                 break;
 561               case 'u':
 562                 digits = "0123456789";
 563                 break;
 564               case 'x':
 565                 digits = "0123456789abcdef";
 566                 break;
 567               case 'X':
 568                 digits = "0123456789ABCDEF";
 569                 break;
 570               default:
 571                 NOT_REACHED ();
 572               }
 573
 574             printf_integer (value, false, digits, &c, output, aux);
 575           }
 576           break;
 577
 578         case 'c':
 579           {
 580             char ch = va_arg (args, int);
 581             printf_string (&ch, 1, &c, output, aux);
 582           }
 583           break;
 584
 585         case 's':
 586           {
 587             const char *s;
 588             size_t length;
 589
 590             s = va_arg (args, char *);
 591             if (s == NULL)
 592               s = "(null)";
 593
 594             if (c.precision >= 0)
 595               {
 596                 const char *zero = memchr (s, '\0', c.precision);
 597                 if (zero != NULL)
 598                   length = zero - s;
 599                 else
 600                   length = c.precision;
 601               }
 602             else
 603               length = strlen (s);
 604
 605             printf_string (s, length, &c, output, aux);
 606           }
 607           break;
 608
 609         case 'p':
 610           {
 611             void *p = va_arg (args, void *);
 612
 613             c.flags = POUND;
 614             if (p != NULL)
 615               printf_integer ((uintptr_t) p,
 616                               false, "0123456789abcdef", &c,
 617                               output, aux);
 618             else
 619               printf_string ("(nil)", 5, &c, output, aux);
 620           }
 621           break;
 622
 623         case 'f':
 624         case 'e':
 625         case 'E':
 626         case 'g':
 627         case 'G':
 628         case 'n':
 629           printf_core ("<<no %%%c in kernel>>", output, aux, *format);
 630           break;
 631
 632         default:
 633           printf_core ("<<no %%%c conversion>>", output, aux, *format);
 634           break;
 635         }
 636     }
 637 }