pd.if.org Git - zos/blob - klib/printk.c

   1 /* adapted from http://www.efgh.com/software/gprintf.htm */
   2 /* public domain */
   3
   4 #include <stdarg.h>
   5 #include <stdint.h>
   6 #include <stddef.h>
   7
   8 #include "kernel.h"
   9 #include "tty.h"
  10
  11 #define BITS_PER_BYTE 8
  12
  13 #define F_THOUSANDS   0x1
  14 #define F_LEFTJUSTIFY 0x2
  15 #define F_USESIGN     0x4
  16 #define F_SPACE       0x8
  17 #define F_ALTERNATE   0x10
  18 #define F_ZEROPAD     0x20
  19 #define F_CAPITAL     0x40
  20 /* F_PRECISION set if precision specified in format */
  21 #define F_PRECISION   0x80
  22
  23 #if 0
  24 static unsigned char digits[17]    = "01234567890abcdef";
  25 static unsigned char capdigits[17] = "01234567890ABCDEF";
  26 #endif
  27
  28 struct parameters {
  29         int n; /* number of output chars */
  30         unsigned flags;
  31         int width;
  32         int precision;
  33         int modifier;
  34
  35         short len;
  36         int (*output_function)(void *, int);
  37         void *output_pointer;
  38         char *str;
  39         char buf[BITS_PER_BYTE * sizeof(uintmax_t) + 1];
  40 };
  41
  42 static int strlenk(char *s) {
  43         int len = 0;
  44         while (*s++) {
  45                 len++;
  46         }
  47         return len;
  48 }
  49
  50 static void output(struct parameters *p, char *s) {
  51         int justify = p->width - p->len;
  52         char jc = ' ';
  53
  54         if (justify < 0) {
  55                 justify = 0;
  56         }
  57
  58         if (justify && p->flags & F_ZEROPAD) {
  59                 jc = '0';
  60         }
  61
  62         if (justify && !(p->flags & F_LEFTJUSTIFY)) {
  63                 while (justify--) {
  64                         terminal.putchar(jc);
  65                         p->n++;
  66                 }
  67         }
  68
  69         while (p->len--) {
  70                 terminal.putchar(*s++);
  71                 p->n++;
  72         }
  73
  74         while (justify-- > 0) {
  75                 terminal.putchar(' ');
  76                 p->n++;
  77         }
  78 }
  79
  80 static inline int get_decimal(const char *str, int *val) {
  81         int ans = 0;
  82         int used = 0;
  83         int neg = 0;
  84         char digit;
  85
  86         if (*str == '-') {
  87                 str++;
  88                 neg = 1;
  89                 used++;
  90         }
  91
  92         while (digit = *str++) {
  93                 if (digit >= '0' && digit <= '9') {
  94                         ans *= 10;
  95                         ans += (digit - '0');
  96                         used++;
  97                 } else {
  98                         break;
  99                 }
 100         }
 101         if (neg) {
 102                 ans = -ans;
 103         }
 104         *val = ans;
 105         return used;
 106 }
 107
 108 int printk(const char *fmt, ...) {
 109         va_list ap;
 110         int n;
 111         va_start(ap, fmt);
 112         n = printkv(fmt, ap);
 113         va_end(ap);
 114         return n;
 115 }
 116
 117 int printkv(const char *fmt, va_list ap) {
 118         struct parameters p = { 0 };
 119         struct parameters z = { 0 };
 120         unsigned char fc;
 121         int get_flags;
 122
 123         while (fc = *fmt++) {
 124                 if (fc != '%') {
 125                         terminal.putchar(fc);
 126                         p.n++;
 127                         continue;
 128                 }
 129                 p = z;
 130
 131                 /* it's a conversion, get flags */
 132                 get_flags = 1;
 133                 while (get_flags) {
 134                         fc = *fmt++;
 135                         switch (fc) {
 136                                 case 0:
 137                                         /* we're done */
 138                                         /* this is an error though */
 139                                         return p.n;
 140                                         break;
 141                                 case '#':
 142                                         p.flags |= F_ALTERNATE;
 143                                         break;
 144                                 case '0':
 145                                         p.flags |= F_ZEROPAD;
 146                                         break;
 147                                 case '-':
 148                                         p.flags |= F_LEFTJUSTIFY;
 149                                         break;
 150                                 case ' ':
 151                                         p.flags |= F_SPACE;
 152                                         break;
 153                                 case '+':
 154                                         p.flags |= F_USESIGN;
 155                                         break;
 156                                 /* posix, not C, so maybe need a define */
 157                                 case '.':
 158                                         p.flags |= F_THOUSANDS;
 159                                         break;
 160                                 default:
 161                                         fmt--;
 162                                         get_flags = 0;
 163                                         break;
 164                         }
 165                 }
 166
 167                 /* get minimum field width */
 168                 if (fc >= '1' && fc <= '9') {
 169                         fmt += get_decimal(fmt, &p.width);
 170                 } else if (*fmt == '*') {
 171                         fmt++;
 172                         if (*fmt >= '1' && *fmt <= '9') {
 173                                 int lenarg = 0;
 174                                 fmt += get_decimal(fmt, &lenarg);
 175                                 if (*fmt != '$') {
 176                                         /* TODO ERROR */
 177                                 }
 178                                 fmt++; /* skip past the $ */
 179                         } else {
 180                                 p.width = va_arg(ap, int);
 181                                 if (p.width < 0) {
 182                                         p.width = -p.width;
 183                                         p.flags |= F_LEFTJUSTIFY;
 184                                 }
 185                         }
 186                 }
 187
 188                 /* get precision */
 189                 if (*fmt == '.') {
 190                         fmt++;
 191                         p.precision = 0;
 192                         p.flags |= F_PRECISION;
 193
 194                         if (fc >= '0' && fc <= '9') {
 195                                 fmt += get_decimal(fmt, &p.width);
 196                         } else if (*fmt == '*') {
 197                                 fmt++;
 198                                 if (*fmt >= '1' && *fmt <= '9') {
 199                                         int arg = 0;
 200                                         fmt += get_decimal(fmt, &arg);
 201                                         if (*fmt != '$') {
 202                                                 /* TODO ERROR */
 203                                         }
 204                                         fmt++; /* skip past the $ */
 205                                 } else {
 206                                         p.precision = va_arg(ap, int);
 207                                         if (p.precision < 0) {
 208                                                 p.precision = 0;
 209                                                 p.flags &= ~F_PRECISION;
 210                                         }
 211                                 }
 212                         }
 213                 }
 214
 215                 /* get length modifier */
 216                 switch (*fmt) {
 217                         case 'L':
 218                         case 'j':
 219                         case 'z':
 220                         case 't':
 221                                 p.modifier = *fmt++;
 222                                 break;
 223                         case 'h':
 224                                 p.modifier = *fmt++;
 225                                 if (*fmt == 'h') {
 226                                         fmt++;
 227                                         p.modifier = 'H';
 228                                 }
 229                                 break;
 230                         case 'l':
 231                                 p.modifier = *fmt++;
 232                                 if (*fmt == 'l') {
 233                                         fmt++;
 234                                         p.modifier = 'L';
 235                                 }
 236                                 break;
 237                         default:
 238                                 break;
 239                 }
 240
 241                 /* conversion specifier */
 242                 int base = 2;
 243                 uintmax_t i;
 244                 union {
 245                         uintmax_t uim;
 246                         int i;
 247                         char c;
 248                         unsigned u;
 249                         unsigned char uc;
 250                         unsigned short us;
 251                         unsigned long ul;
 252                         unsigned long long ull;
 253                         size_t size;
 254                         ptrdiff_t pd;
 255                 } arg;
 256
 257                 switch (*fmt) {
 258                         case '%':
 259                                 terminal.putchar('%');
 260                                 p.n++;
 261                                 fmt++;
 262                                 break;
 263                         case 'c':
 264                                 if (p.modifier == 'l') {
 265                                         /* wide int wint_t */
 266                                         break;
 267                                 }
 268                                 arg.i = va_arg(ap, int);
 269                                 p.buf[0] = (unsigned char) arg.i;
 270                                 p.len = 1;
 271                                 output(&p, p.buf);
 272                                 fmt++;
 273                                 break;
 274                         case 's':
 275                                 p.str = va_arg(ap, char *);
 276                                 /* TODO may not be properly terminated if precision is given */
 277                                 p.len = strlenk(p.str);
 278                                 /* precision, if given, is maximum characters
 279                                  * for an 's' conversion */
 280                                 if (p.flags & F_PRECISION && p.len > p.precision) {
 281                                         p.len = p.precision;
 282                                 }
 283                                 /* TODO don't zero pad strings */
 284                                 output(&p, p.str);
 285                                 fmt++;
 286                                 break;
 287                         case 'X':
 288                                 p.flags |= F_CAPITAL;
 289                         case 'x':
 290                                 base += 6;
 291                         case 'u':
 292                                 base += 2;
 293                         case 'o':
 294                                 base += 6;
 295                                 switch (p.modifier) {
 296                                         case 'H':
 297                                                 arg.u = va_arg(ap, unsigned int);
 298                                                 i = (uintmax_t) (unsigned char) arg.uc;
 299                                                 break;
 300                                         case 'h':
 301                                                 arg.u = va_arg(ap, unsigned int);
 302                                                 i = (uintmax_t) (unsigned short) arg.us;
 303                                                 break;
 304                                         case 'l':
 305                                                 arg.ul = va_arg(ap, unsigned long);
 306                                                 i = (uintmax_t) arg.ul;
 307                                                 break;
 308                                         case 'L':
 309                                                 arg.ull = va_arg(ap, unsigned long long);
 310                                                 i = (uintmax_t) arg.ull;
 311                                                 break;
 312                                         case 'j':
 313                                                 i =  va_arg(ap, uintmax_t);
 314                                                 break;
 315                                         case 'z':
 316                                                 arg.size = va_arg(ap, size_t);
 317                                                 i = (uintmax_t) arg.size;
 318                                                 break;
 319                                         case 't':
 320                                                 arg.pd = va_arg(ap, ptrdiff_t);
 321                                                 i = (uintmax_t) arg.pd;
 322                                                 break;
 323                                         default:
 324                                                 arg.u = va_arg(ap, unsigned);
 325                                                 i = (uintmax_t) arg.u;
 326                                                 break;
 327                                 }
 328                                 p.str = p.buf + sizeof(p.buf) - 1;
 329                                 *p.str = 0;
 330                                 p.str--;
 331                                 do {
 332                                         int c;
 333                                         c = i % base + '0';
 334                                         if (c > '9') {
 335                                                 if (p.flags & F_CAPITAL)
 336                                                         c += 'A'-'9'-1;
 337                                                 else
 338                                                         c += 'a'-'9'-1;
 339                                         }
 340                                         *p.str-- = c;
 341                                         p.len++;
 342                                 } while ( i /= base);
 343                                 output(&p, p.str+1);
 344                                 fmt++;
 345                                 break;
 346                         case 'd':
 347                         case 'i':
 348                                 base = 10;
 349
 350                         default:
 351                                 break;
 352                 }
 353                 /* output number here ? */
 354
 355         }
 356         terminal.update_cursor();
 357
 358         return p.n;
 359 }