src/odometry/grubio.c

   1 /* Simple printf like functions taken from GRUB project */
   2
   3 /*
   4  *  GRUB  --  GRand Unified Bootloader
   5  *  Copyright (C) 1999,2000,2001,2002,2003,2004,2005,2006,2007  Free Software Foundation, Inc.
   6  *
   7  *  GRUB is free software: you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License as published by
   9  *  the Free Software Foundation, either version 3 of the License, or
  10  *  (at your option) any later version.
  11  *
  12  *  GRUB is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU General Public License
  18  *  along with GRUB.  If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "grubio.h"
  22 #include <string.h>
  23 #include <unistd.h>
  24 #include <stdlib.h>
  25 #include <ctype.h>
  26
  27 int
  28 grub_printf (const char *fmt, ...)
  29 {
  30   va_list ap;
  31   int ret;
  32
  33   va_start (ap, fmt);
  34   ret = grub_vprintf (fmt, ap);
  35   va_end (ap);
  36
  37   return ret;
  38 }
  39
  40 int
  41 grub_vprintf (const char *fmt, va_list args)
  42 {
  43   int ret;
  44
  45   ret = grub_vsprintf (0, fmt, args);
  46   return ret;
  47 }
  48
  49 unsigned long long
  50 grub_strtoull (const char *str, char **end, int base)
  51 {
  52   unsigned long long num = 0;
  53   int found = 0;
  54
  55   /* Skip white spaces.  */
  56   while (*str && isspace (*str))
  57     str++;
  58
  59   /* Guess the base, if not specified. The prefix `0x' means 16, and
  60      the prefix `0' means 8.  */
  61   if (base == 0 && str[0] == '0')
  62     {
  63       if (str[1] == 'x')
  64         {
  65           if (base == 0 || base == 16)
  66             {
  67               base = 16;
  68               str += 2;
  69             }
  70         }
  71       else if (str[1] >= '0' && str[1] <= '7')
  72         base = 8;
  73     }
  74
  75   if (base == 0)
  76     base = 10;
  77
  78   while (*str)
  79     {
  80       unsigned long digit;
  81
  82       digit = tolower (*str) - '0';
  83       if (digit > 9)
  84         {
  85           digit += '0' - 'a' + 10;
  86           if (digit >= (unsigned long) base)
  87             break;
  88         }
  89
  90       found = 1;
  91
  92       /* NUM * BASE + DIGIT > ~0ULL */
  93       if (num > grub_divmod64 (~0ULL - digit, base, 0))
  94         {
  95 /*        grub_error (GRUB_ERR_OUT_OF_RANGE, "overflow is detected"); */
  96           return ~0ULL;
  97         }
  98
  99       num = num * base + digit;
 100       str++;
 101     }
 102
 103   if (! found)
 104     {
 105 /*       grub_error (GRUB_ERR_BAD_NUMBER, "unrecognized number"); */
 106       return 0;
 107     }
 108
 109   if (end)
 110     *end = (char *) str;
 111
 112   return num;
 113 }
 114
 115 static inline void
 116 grub_reverse (char *str)
 117 {
 118   char *p = str + strlen (str) - 1;
 119
 120   while (str < p)
 121     {
 122       char tmp;
 123
 124       tmp = *str;
 125       *str = *p;
 126       *p = tmp;
 127       str++;
 128       p--;
 129     }
 130 }
 131
 132 static char *
 133 grub_itoa (char *str, int c, unsigned n)
 134 {
 135   unsigned base = (c == 'x') ? 16 : 10;
 136   char *p;
 137
 138   if ((int) n < 0 && c == 'd')
 139     {
 140       n = (unsigned) (-((int) n));
 141       *str++ = '-';
 142     }
 143
 144   p = str;
 145   do
 146     {
 147       unsigned d = n % base;
 148       *p++ = (d > 9) ? d + 'a' - 10 : d + '0';
 149     }
 150   while (n /= base);
 151   *p = 0;
 152
 153   grub_reverse (str);
 154   return p;
 155 }
 156
 157 /* Divide N by D, return the quotient, and store the remainder in *R.  */
 158 uint64_t
 159 grub_divmod64 (uint64_t n, uint32_t d, uint32_t *r)
 160 {
 161   /* This algorithm is typically implemented by hardware. The idea
 162      is to get the highest bit in N, 64 times, by keeping
 163      upper(N * 2^i) = upper((Q * 10 + M) * 2^i), where upper
 164      represents the high 64 bits in 128-bits space.  */
 165   unsigned bits = 64;
 166   unsigned long long q = 0;
 167   unsigned m = 0;
 168
 169   /* Skip the slow computation if 32-bit arithmetic is possible.  */
 170   if (n < 0xffffffff)
 171     {
 172       if (r)
 173         *r = ((uint32_t) n) % d;
 174
 175       return ((uint32_t) n) / d;
 176     }
 177
 178   while (bits--)
 179     {
 180       m <<= 1;
 181
 182       if (n & (1ULL << 63))
 183         m |= 1;
 184
 185       q <<= 1;
 186       n <<= 1;
 187
 188       if (m >= d)
 189         {
 190           q |= 1;
 191           m -= d;
 192         }
 193     }
 194
 195   if (r)
 196     *r = m;
 197
 198   return q;
 199 }
 200
 201 /* Convert a long long value to a string. This function avoids 64-bit
 202    modular arithmetic or divisions.  */
 203 static char *
 204 grub_lltoa (char *str, int c, unsigned long long n)
 205 {
 206   unsigned base = (c == 'x') ? 16 : 10;
 207   char *p;
 208
 209   if ((long long) n < 0 && c == 'd')
 210     {
 211       n = (unsigned long long) (-((long long) n));
 212       *str++ = '-';
 213     }
 214
 215   p = str;
 216
 217   if (base == 16)
 218     do
 219       {
 220         unsigned d = (unsigned) (n & 0xf);
 221         *p++ = (d > 9) ? d + 'a' - 10 : d + '0';
 222       }
 223     while (n >>= 4);
 224   else
 225     /* BASE == 10 */
 226     do
 227       {
 228         uint32_t m;
 229
 230         n = grub_divmod64 (n, 10, &m);
 231         *p++ = m + '0';
 232       }
 233     while (n);
 234
 235   *p = 0;
 236
 237   grub_reverse (str);
 238   return p;
 239 }
 240
 241 int
 242 grub_vsprintf (char *str, const char *fmt, va_list args)
 243 {
 244   char c;
 245   int count = 0;
 246   auto void write_char (unsigned char ch);
 247   auto void write_str (const char *s);
 248   auto void write_fill (const char ch, int n);
 249
 250   void write_char (unsigned char ch)
 251     {
 252       if (str)
 253         *str++ = ch;
 254       else
 255         write(1, &ch, 1);
 256
 257       count++;
 258     }
 259
 260   void write_str (const char *s)
 261     {
 262       while (*s)
 263         write_char (*s++);
 264     }
 265
 266   void write_fill (const char ch, int n)
 267     {
 268       int i;
 269       for (i = 0; i < n; i++)
 270         write_char (ch);
 271     }
 272
 273   while ((c = *fmt++) != 0)
 274     {
 275       if (c != '%')
 276         write_char (c);
 277       else
 278         {
 279           char tmp[32];
 280           char *p;
 281           unsigned int format1 = 0;
 282           unsigned int format2 = 3;
 283           char zerofill = ' ';
 284           int rightfill = 0;
 285           int n;
 286           int longfmt = 0;
 287           int longlongfmt = 0;
 288
 289           if (*fmt && *fmt =='-')
 290             {
 291               rightfill = 1;
 292               fmt++;
 293             }
 294
 295           p = (char *) fmt;
 296           /* Read formatting parameters.  */
 297           while (*p && isdigit (*p))
 298             p++;
 299
 300           if (p > fmt)
 301             {
 302               char s[p - fmt + 1];
 303               strncpy (s, fmt, p - fmt);
 304               s[p - fmt] = 0;
 305               if (s[0] == '0')
 306                 zerofill = '0';
 307               format1 = strtoul (s, 0, 10);
 308               fmt = p;
 309               if (*p && *p == '.')
 310                 {
 311                   p++;
 312                   fmt++;
 313                   while (*p && isdigit (*p))
 314                     p++;
 315
 316                   if (p > fmt)
 317                     {
 318                       char fstr[p - fmt];
 319                       strncpy (fstr, fmt, p - fmt);
 320                       format2 = strtoul (fstr, 0, 10);
 321                       fmt = p;
 322                     }
 323                 }
 324             }
 325
 326           c = *fmt++;
 327           if (c == 'l')
 328             {
 329               longfmt = 1;
 330               c = *fmt++;
 331               if (c == 'l')
 332                 {
 333                   longlongfmt = 1;
 334                   c = *fmt++;
 335                 }
 336             }
 337
 338           switch (c)
 339             {
 340             case 'p':
 341               write_str ("0x");
 342               c = 'x';
 343               longlongfmt |= (sizeof (void *) == sizeof (long long));
 344               /* fall through */
 345             case 'x':
 346             case 'u':
 347             case 'd':
 348               if (longlongfmt)
 349                 {
 350                   long long ll;
 351
 352                   ll = va_arg (args, long long);
 353                   grub_lltoa (tmp, c, ll);
 354                 }
 355               else
 356                 {
 357                   if (longfmt)
 358                     n = va_arg (args, long);
 359                   else
 360                     n = va_arg (args, int);
 361                   grub_itoa (tmp, c, n);
 362                 }
 363               if (! rightfill && strlen (tmp) < format1)
 364                 write_fill (zerofill, format1 - strlen (tmp));
 365               write_str (tmp);
 366               if (rightfill && strlen (tmp) < format1)
 367                 write_fill (zerofill, format1 - strlen (tmp));
 368               break;
 369
 370             case 'c':
 371               n = va_arg (args, int);
 372               write_char (n & 0xff);
 373               break;
 374
 375             case 'f':
 376               {
 377                 va_arg (args, double);
 378                 tmp[0] = '?'; /* Not implemented */
 379                 tmp[1] = 0;
 380                 if (!rightfill && strlen (tmp) < format1)
 381                   write_fill (zerofill, format1 - strlen (tmp));
 382                 write_str (tmp);
 383                 if (rightfill && strlen (tmp) < format1)
 384                   write_fill (zerofill, format1 - strlen (tmp));
 385                 break;
 386               }
 387
 388             case 'C':
 389               {
 390                 uint32_t code = va_arg (args, uint32_t);
 391                 int shift;
 392                 unsigned mask;
 393
 394                 if (code <= 0x7f)
 395                   {
 396                     shift = 0;
 397                     mask = 0;
 398                   }
 399                 else if (code <= 0x7ff)
 400                   {
 401                     shift = 6;
 402                     mask = 0xc0;
 403                   }
 404                 else if (code <= 0xffff)
 405                   {
 406                     shift = 12;
 407                     mask = 0xe0;
 408                   }
 409                 else if (code <= 0x1fffff)
 410                   {
 411                     shift = 18;
 412                     mask = 0xf0;
 413                   }
 414                 else if (code <= 0x3ffffff)
 415                   {
 416                     shift = 24;
 417                     mask = 0xf8;
 418                   }
 419                 else if (code <= 0x7fffffff)
 420                   {
 421                     shift = 30;
 422                     mask = 0xfc;
 423                   }
 424                 else
 425                   {
 426                     code = '?';
 427                     shift = 0;
 428                     mask = 0;
 429                   }
 430
 431                 write_char (mask | (code >> shift));
 432
 433                 for (shift -= 6; shift >= 0; shift -= 6)
 434                   write_char (0x80 | (0x3f & (code >> shift)));
 435               }
 436               break;
 437
 438             case 's':
 439               p = va_arg (args, char *);
 440               if (p)
 441                 {
 442                   if (!rightfill && strlen (p) < format1)
 443                     write_fill (zerofill, format1 - strlen (p));
 444
 445                   write_str (p);
 446
 447                   if (rightfill && strlen (p) < format1)
 448                     write_fill (zerofill, format1 - strlen (p));
 449                 }
 450               else
 451                 write_str ("(null)");
 452
 453               break;
 454
 455             default:
 456               write_char (c);
 457               break;
 458             }
 459         }
 460     }
 461
 462   if (str)
 463     *str = '\0';
 464
 465   return count;
 466 }
 467
 468 int
 469 grub_sprintf (char *str, const char *fmt, ...)
 470 {
 471   va_list ap;
 472   int ret;
 473
 474   va_start (ap, fmt);
 475   ret = grub_vsprintf (str, fmt, ap);
 476   va_end (ap);
 477
 478   return ret;
 479 }
 480