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_div <<= 1;
  26                         tmp_res <<= 1;
  27                 }
  28                 dividend -= divisor * tmp_res;
  29                 result += tmp_res;
  30         }
  31         return result;
  32 }
  33
  34 #else /* BITS_PER_LONG >= 64 */
  35
  36 static inline unsigned long long div_u64_u64(unsigned long long dividend,
  37                                              unsigned long long divisor)
  38 {
  39         return dividend / divisor;
  40 }
  41
  42 #endif /* BITS_PER_LONG >= 64 */
  43
  44 static char *uint2str(unsigned long long value, char *buf)
  45 {
  46         unsigned long long digit, divisor = 10000000000000000000ULL;
  47         int first_digit = 1;
  48
  49         while (divisor > 0) {
  50                 digit = div_u64_u64(value, divisor);
  51                 value -= digit * divisor;
  52                 if (!first_digit || digit > 0 || divisor == 1) {
  53                         *buf++ = '0' + digit;
  54                         first_digit = 0;
  55                 }
  56                 divisor = div_u64_u64(divisor, 10);
  57         }
  58
  59         return buf;
  60 }
  61
  62 static char *int2str(long long value, char *buf)
  63 {
  64         if (value < 0) {
  65                 *buf++ = '-';
  66                 value = -value;
  67         }
  68         return uint2str(value, buf);
  69 }
  70
  71 static char *hex2str(unsigned long long value, char *buf,
  72                      unsigned long long leading_zero_mask)
  73 {
  74         const char hexdigit[] = "0123456789abcdef";
  75         unsigned long long digit, divisor = 0x1000000000000000ULL;
  76         int first_digit = 1;
  77
  78         while (divisor > 0) {
  79                 digit = div_u64_u64(value, divisor);
  80                 value -= digit * divisor;
  81                 if (!first_digit || digit > 0 || divisor == 1 ||
  82                     divisor & leading_zero_mask) {
  83                         *buf++ = hexdigit[digit];
  84                         first_digit = 0;
  85                 }
  86                 divisor >>= 4;
  87         }
  88
  89         return buf;
  90 }
  91
  92 static char *align(char *p1, char *p0, unsigned long width)
  93 {
  94         unsigned int n;
  95
  96         if (p1 - p0 >= width)
  97                 return p1;
  98
  99         for (n = 1; p1 - n >= p0; n++)
 100                 *(p0 + width - n) = *(p1 - n);
 101         memset(p0, ' ', width - (p1 - p0));
 102         return p0 + width;
 103 }
 104
 105 static void __vprintk(const char *fmt, va_list ap)
 106 {
 107         char buf[128];
 108         char *p, *p0;
 109         char c;
 110         unsigned long long v;
 111         unsigned int width;
 112         bool longmode;
 113
 114         p = buf;
 115
 116         while (1) {
 117                 c = *fmt++;
 118                 if (c == 0)
 119                         break;
 120                 else if (c == '%') {
 121                         *p = 0;
 122                         console_write(buf);
 123                         p = buf;
 124
 125                         c = *fmt++;
 126
 127                         width = 0;
 128                         p0 = p;
 129                         while (c >= '0' && c <= '9') {
 130                                 width = width * 10 + c - '0';
 131                                 c = *fmt++;
 132                                 if (width >= sizeof(buf) - 1)
 133                                         width = 0;
 134                         }
 135
 136                         longmode = false;
 137                         if (c == 'l') {
 138                                 longmode = true;
 139                                 c = *fmt++;
 140                         }
 141
 142                         switch (c) {
 143                         case 'd':
 144                                 if (longmode)
 145                                         v = va_arg(ap, long);
 146                                 else
 147                                         v = va_arg(ap, int);
 148                                 p = int2str(v, p);
 149                                 p = align(p, p0, width);
 150                                 break;
 151                         case 'p':
 152                                 *p++ = '0';
 153                                 *p++ = 'x';
 154                                 v = va_arg(ap, unsigned long);
 155                                 p = hex2str(v, p, (unsigned long)-1);
 156                                 break;
 157                         case 's':
 158                                 console_write(va_arg(ap, const char *));
 159                                 break;
 160                         case 'u':
 161                                 if (longmode)
 162                                         v = va_arg(ap, unsigned long);
 163                                 else
 164                                         v = va_arg(ap, unsigned int);
 165                                 p = uint2str(v, p);
 166                                 p = align(p, p0, width);
 167                                 break;
 168                         case 'x':
 169                                 if (longmode)
 170                                         v = va_arg(ap, unsigned long);
 171                                 else
 172                                         v = va_arg(ap, unsigned int);
 173                                 p = hex2str(v, p, 0);
 174                                 p = align(p, p0, width);
 175                                 break;
 176                         default:
 177                                 *p++ = '%';
 178                                 *p++ = c;
 179                                 break;
 180                         }
 181                 } else if (c == '\n') {
 182                         *p++ = c;
 183                         *p = 0;
 184                         console_write(buf);
 185                         p = buf;
 186                         *p++ = '\r';
 187                 } else
 188                         *p++ = c;
 189
 190                 if (p >= &buf[sizeof(buf) - 1]) {
 191                         *p = 0;
 192                         console_write(buf);
 193                         p = buf;
 194                 }
 195         }
 196
 197         *p = 0;
 198         console_write(buf);
 199 }