hypervisor/printk-core.c

   1 /*
   2  * Jailhouse, a Linux-based partitioning hypervisor
   3  *
   4  * Copyright (c) Siemens AG, 2013
   5  *
   6  * Authors:
   7  *  Jan Kiszka <jan.kiszka@siemens.com>
   8  *
   9  * This work is licensed under the terms of the GNU GPL, version 2.  See
  10  * the COPYING file in the top-level directory.
  11  */
  12
  13 #if BITS_PER_LONG < 64
  14
  15 static unsigned long long div_u64_u64(unsigned long long dividend,
  16                                       unsigned long long divisor)
  17 {
  18         unsigned long long result = 0;
  19         unsigned long long tmp_res, tmp_div;
  20
  21         while (dividend >= divisor) {
  22                 tmp_div = divisor << 1;
  23                 tmp_res = 1;
  24                 while (dividend >= tmp_div) {
  25                         tmp_res <<= 1;
  26                         if (tmp_div & (1ULL << 63))
  27                                 break;
  28                         tmp_div <<= 1;
  29                 }
  30                 dividend -= divisor * tmp_res;
  31                 result += tmp_res;
  32         }
  33         return result;
  34 }
  35
  36 #else /* BITS_PER_LONG >= 64 */
  37
  38 static inline unsigned long long div_u64_u64(unsigned long long dividend,
  39                                              unsigned long long divisor)
  40 {
  41         return dividend / divisor;
  42 }
  43
  44 #endif /* BITS_PER_LONG >= 64 */
  45
  46 static char *uint2str(unsigned long long value, char *buf)
  47 {
  48         unsigned long long digit, divisor = 10000000000000000000ULL;
  49         int first_digit = 1;
  50
  51         while (divisor > 0) {
  52                 digit = div_u64_u64(value, divisor);
  53                 value -= digit * divisor;
  54                 if (!first_digit || digit > 0 || divisor == 1) {
  55                         *buf++ = '0' + digit;
  56                         first_digit = 0;
  57                 }
  58                 divisor = div_u64_u64(divisor, 10);
  59         }
  60
  61         return buf;
  62 }
  63
  64 static char *int2str(long long value, char *buf)
  65 {
  66         if (value < 0) {
  67                 *buf++ = '-';
  68                 value = -value;
  69         }
  70         return uint2str(value, buf);
  71 }
  72
  73 static char *hex2str(unsigned long long value, char *buf,
  74                      unsigned long long leading_zero_mask)
  75 {
  76         static const char hexdigit[] = "0123456789abcdef";
  77         unsigned long long digit, divisor = 0x1000000000000000ULL;
  78         int first_digit = 1;
  79
  80         while (divisor > 0) {
  81                 digit = div_u64_u64(value, divisor);
  82                 value -= digit * divisor;
  83                 if (!first_digit || digit > 0 || divisor == 1 ||
  84                     divisor & leading_zero_mask) {
  85                         *buf++ = hexdigit[digit];
  86                         first_digit = 0;
  87                 }
  88                 divisor >>= 4;
  89         }
  90
  91         return buf;
  92 }
  93
  94 static char *align(char *p1, char *p0, unsigned long width, char fill)
  95 {
  96         unsigned int n;
  97
  98         if (p1 - p0 >= width)
  99                 return p1;
 100
 101         for (n = 1; p1 - n >= p0; n++)
 102                 *(p0 + width - n) = *(p1 - n);
 103         memset(p0, fill, width - (p1 - p0));
 104         return p0 + width;
 105 }
 106
 107 static void __vprintk(const char *fmt, va_list ap)
 108 {
 109         char buf[128];
 110         char *p, *p0;
 111         char c, fill;
 112         unsigned long long v;
 113         unsigned int width;
 114         bool longmode;
 115
 116         p = buf;
 117
 118         while (1) {
 119                 c = *fmt++;
 120                 if (c == 0) {
 121                         break;
 122                 } else if (c == '%') {
 123                         *p = 0;
 124                         console_write(buf);
 125                         p = buf;
 126
 127                         c = *fmt++;
 128
 129                         width = 0;
 130                         p0 = p;
 131                         fill = (c == '0') ? '0' : ' ';
 132                         while (c >= '0' && c <= '9') {
 133                                 width = width * 10 + c - '0';
 134                                 c = *fmt++;
 135                                 if (width >= sizeof(buf) - 1)
 136                                         width = 0;
 137                         }
 138
 139                         longmode = false;
 140                         if (c == 'l') {
 141                                 longmode = true;
 142                                 c = *fmt++;
 143                         }
 144
 145                         switch (c) {
 146                         case 'd':
 147                                 if (longmode)
 148                                         v = va_arg(ap, long);
 149                                 else
 150                                         v = va_arg(ap, int);
 151                                 p = int2str(v, p);
 152                                 p = align(p, p0, width, fill);
 153                                 break;
 154                         case 'p':
 155                                 *p++ = '0';
 156                                 *p++ = 'x';
 157                                 v = va_arg(ap, unsigned long);
 158                                 p = hex2str(v, p, (unsigned long)-1);
 159                                 break;
 160                         case 's':
 161                                 console_write(va_arg(ap, const char *));
 162                                 break;
 163                         case 'u':
 164                                 if (longmode)
 165                                         v = va_arg(ap, unsigned long);
 166                                 else
 167                                         v = va_arg(ap, unsigned int);
 168                                 p = uint2str(v, p);
 169                                 p = align(p, p0, width, fill);
 170                                 break;
 171                         case 'x':
 172                                 if (longmode)
 173                                         v = va_arg(ap, unsigned long);
 174                                 else
 175                                         v = va_arg(ap, unsigned int);
 176                                 p = hex2str(v, p, 0);
 177                                 p = align(p, p0, width, fill);
 178                                 break;
 179                         default:
 180                                 *p++ = '%';
 181                                 *p++ = c;
 182                                 break;
 183                         }
 184                 } else {
 185                         *p++ = c;
 186                 }
 187                 if (p >= &buf[sizeof(buf) - 1]) {
 188                         *p = 0;
 189                         console_write(buf);
 190                         p = buf;
 191                 }
 192         }
 193
 194         *p = 0;
 195         console_write(buf);
 196 }