00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00038 #include <printf/printf_core.h>
00039 #include <putchar.h>
00040 #include <print.h>
00041 #include <synch/spinlock.h>
00042 #include <arch/arg.h>
00043 #include <arch/asm.h>
00044
00045 #include <arch.h>
00046
00047 SPINLOCK_INITIALIZE(printflock);
00049 #define __PRINTF_FLAG_PREFIX 0x00000001
00050 #define __PRINTF_FLAG_SIGNED 0x00000002
00051 #define __PRINTF_FLAG_ZEROPADDED 0x00000004
00052 #define __PRINTF_FLAG_LEFTALIGNED 0x00000010
00053 #define __PRINTF_FLAG_SHOWPLUS 0x00000020
00054 #define __PRINTF_FLAG_SPACESIGN 0x00000040
00055 #define __PRINTF_FLAG_BIGCHARS 0x00000080
00056 #define __PRINTF_FLAG_NEGATIVE 0x00000100
00058 #define PRINT_NUMBER_BUFFER_SIZE (64+5)
00065 typedef enum {
00066 PrintfQualifierByte = 0,
00067 PrintfQualifierShort,
00068 PrintfQualifierInt,
00069 PrintfQualifierLong,
00070 PrintfQualifierLongLong,
00071 PrintfQualifierNative,
00072 PrintfQualifierPointer
00073 } qualifier_t;
00074
00075 static char digits_small[] = "0123456789abcdef";
00076 static char digits_big[] = "0123456789ABCDEF";
00082 static inline int isdigit(int c)
00083 {
00084 return ((c >= '0' )&&( c <= '9'));
00085 }
00086
00091 static __native strlen(const char *str)
00092 {
00093 __native counter = 0;
00094
00095 while (str[counter] != 0) {
00096 counter++;
00097 }
00098
00099 return counter;
00100 }
00101
00108 static int printf_putnchars(const char * buf, size_t count, struct printf_spec *ps)
00109 {
00110 return ps->write((void *)buf, count, ps->data);
00111 }
00112
00118 static int printf_putstr(const char * str, struct printf_spec *ps)
00119 {
00120 size_t count;
00121
00122 if (str == NULL) {
00123 return printf_putnchars("(NULL)", 6, ps);
00124 }
00125
00126 count = strlen(str);
00127
00128 return ps->write((void *) str, count, ps->data);
00129 }
00130
00136 static int printf_putchar(int c, struct printf_spec *ps)
00137 {
00138 unsigned char ch = c;
00139
00140 return ps->write((void *) &ch, 1, ps->data);
00141 }
00142
00149 static int print_char(char c, int width, __u64 flags, struct printf_spec *ps)
00150 {
00151 int counter = 0;
00152
00153 if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
00154 while (--width > 0) {
00155 if (printf_putchar(' ', ps) > 0)
00156 ++counter;
00157 }
00158 }
00159
00160 if (printf_putchar(c, ps) > 0)
00161 counter++;
00162
00163 while (--width > 0) {
00164 if (printf_putchar(' ', ps) > 0)
00165 ++counter;
00166 }
00167
00168 return ++counter;
00169 }
00170
00179 static int print_string(char *s, int width, int precision, __u64 flags, struct printf_spec *ps)
00180 {
00181 int counter = 0;
00182 size_t size;
00183 int retval;
00184
00185 if (s == NULL) {
00186 return printf_putstr("(NULL)", ps);
00187 }
00188
00189 size = strlen(s);
00190
00191
00192
00193 if (precision == 0)
00194 precision = size;
00195
00196 width -= precision;
00197
00198 if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
00199 while (width-- > 0) {
00200 if (printf_putchar(' ', ps) == 1)
00201 counter++;
00202 }
00203 }
00204
00205 while (precision > size) {
00206 precision--;
00207 if (printf_putchar(' ', ps) == 1)
00208 ++counter;
00209 }
00210
00211 if ((retval = printf_putnchars(s, precision, ps)) < 0) {
00212 return -counter;
00213 }
00214 counter += retval;
00215
00216 while (width-- > 0) {
00217 if (printf_putchar(' ', ps) == 1)
00218 ++counter;
00219 }
00220
00221 return counter;
00222 }
00223
00224
00239 static int print_number(__u64 num, int width, int precision, int base , __u64 flags, struct printf_spec *ps)
00240 {
00241 char *digits = digits_small;
00242 char d[PRINT_NUMBER_BUFFER_SIZE];
00243 char *ptr = &d[PRINT_NUMBER_BUFFER_SIZE - 1];
00244 int size = 0;
00245 int number_size;
00246 char sgn;
00247 int retval;
00248 int counter = 0;
00249
00250 if (flags & __PRINTF_FLAG_BIGCHARS)
00251 digits = digits_big;
00252
00253 *ptr-- = 0;
00254
00255 if (num == 0) {
00256 *ptr-- = '0';
00257 size++;
00258 } else {
00259 do {
00260 *ptr-- = digits[num % base];
00261 size++;
00262 } while (num /= base);
00263 }
00264
00265 number_size = size;
00266
00267
00268 if (flags & __PRINTF_FLAG_PREFIX) {
00269 switch(base) {
00270 case 2:
00271 size += 2;
00272 break;
00273 case 8:
00274 size++;
00275 break;
00276 case 16:
00277 size += 2;
00278 break;
00279 }
00280 }
00281
00282 sgn = 0;
00283 if (flags & __PRINTF_FLAG_SIGNED) {
00284 if (flags & __PRINTF_FLAG_NEGATIVE) {
00285 sgn = '-';
00286 size++;
00287 } else if (flags & __PRINTF_FLAG_SHOWPLUS) {
00288 sgn = '+';
00289 size++;
00290 } else if (flags & __PRINTF_FLAG_SPACESIGN) {
00291 sgn = ' ';
00292 size++;
00293 }
00294 }
00295
00296 if (flags & __PRINTF_FLAG_LEFTALIGNED) {
00297 flags &= ~__PRINTF_FLAG_ZEROPADDED;
00298 }
00299
00300
00301 if (flags & __PRINTF_FLAG_ZEROPADDED) {
00302 if ((precision == 0) && (width > size)) {
00303 precision = width - size + number_size;
00304 }
00305 }
00306
00307
00308 if (number_size > precision)
00309 precision = number_size;
00310
00311 width -= precision + size - number_size;
00312
00313 if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
00314 while (width-- > 0) {
00315 if (printf_putchar(' ', ps) == 1)
00316 counter++;
00317 }
00318 }
00319
00320
00321
00322 if (sgn) {
00323 if (printf_putchar(sgn, ps) == 1)
00324 counter++;
00325 }
00326
00327
00328
00329 if (flags & __PRINTF_FLAG_PREFIX) {
00330 switch(base) {
00331 case 2:
00332 if (printf_putchar('0', ps) == 1)
00333 counter++;
00334 if (flags & __PRINTF_FLAG_BIGCHARS) {
00335 if (printf_putchar('B', ps) == 1)
00336 counter++;
00337 } else {
00338 if (printf_putchar('b', ps) == 1)
00339 counter++;
00340 }
00341 break;
00342 case 8:
00343 if (printf_putchar('o', ps) == 1)
00344 counter++;
00345 break;
00346 case 16:
00347 if (printf_putchar('0', ps) == 1)
00348 counter++;
00349 if (flags & __PRINTF_FLAG_BIGCHARS) {
00350 if (printf_putchar('X', ps) == 1)
00351 counter++;
00352 } else {
00353 if (printf_putchar('x', ps) == 1)
00354 counter++;
00355 }
00356 break;
00357 }
00358 }
00359
00360
00361 precision -= number_size;
00362 while (precision-- > 0) {
00363 if (printf_putchar('0', ps) == 1)
00364 counter++;
00365 }
00366
00367
00368
00369
00370 if ((retval = printf_putstr(++ptr, ps)) > 0) {
00371 counter += retval;
00372 }
00373
00374
00375
00376 while (width-- > 0) {
00377 if (printf_putchar(' ', ps) == 1)
00378 counter++;
00379 }
00380
00381 return counter;
00382 }
00383
00384
00459 int printf_core(const char *fmt, struct printf_spec *ps, va_list ap)
00460 {
00461 int irqpri;
00462 int i = 0, j = 0;
00463 int end;
00464 int counter;
00465 int retval;
00466 char c;
00467 qualifier_t qualifier;
00468 int base;
00469 __u64 number;
00470 size_t size;
00471 int width, precision;
00472 __u64 flags;
00473
00474 counter = 0;
00475
00476 irqpri = interrupts_disable();
00477 spinlock_lock(&printflock);
00478
00479 while ((c = fmt[i])) {
00480
00481 if (c == '%' ) {
00482
00483 if (i > j) {
00484 if ((retval = printf_putnchars(&fmt[j], (size_t)(i - j), ps)) < 0) {
00485 counter = -counter;
00486 goto out;
00487 }
00488 counter += retval;
00489 }
00490
00491 j = i;
00492
00493 flags = 0;
00494 end = 0;
00495
00496 do {
00497 ++i;
00498 switch (c = fmt[i]) {
00499 case '#': flags |= __PRINTF_FLAG_PREFIX; break;
00500 case '-': flags |= __PRINTF_FLAG_LEFTALIGNED; break;
00501 case '+': flags |= __PRINTF_FLAG_SHOWPLUS; break;
00502 case ' ': flags |= __PRINTF_FLAG_SPACESIGN; break;
00503 case '0': flags |= __PRINTF_FLAG_ZEROPADDED; break;
00504 default: end = 1;
00505 };
00506
00507 } while (end == 0);
00508
00509
00510 width = 0;
00511 if (isdigit(fmt[i])) {
00512 while (isdigit(fmt[i])) {
00513 width *= 10;
00514 width += fmt[i++] - '0';
00515 }
00516 } else if (fmt[i] == '*') {
00517
00518 i++;
00519 width = (int)va_arg(ap, int);
00520 if (width < 0) {
00521
00522 width *= -1;
00523 flags |= __PRINTF_FLAG_LEFTALIGNED;
00524 }
00525 }
00526
00527
00528 precision = 0;
00529 if (fmt[i] == '.') {
00530 ++i;
00531 if (isdigit(fmt[i])) {
00532 while (isdigit(fmt[i])) {
00533 precision *= 10;
00534 precision += fmt[i++] - '0';
00535 }
00536 } else if (fmt[i] == '*') {
00537
00538 i++;
00539 precision = (int)va_arg(ap, int);
00540 if (precision < 0) {
00541
00542 precision = 0;
00543 }
00544 }
00545 }
00546
00547 switch (fmt[i++]) {
00551 case 'h':
00552 qualifier = PrintfQualifierShort;
00553 if (fmt[i] == 'h') {
00554 i++;
00555 qualifier = PrintfQualifierByte;
00556 }
00557 break;
00558 case 'l':
00559 qualifier = PrintfQualifierLong;
00560 if (fmt[i] == 'l') {
00561 i++;
00562 qualifier = PrintfQualifierLongLong;
00563 }
00564 break;
00565 case 'z':
00566 qualifier = PrintfQualifierNative;
00567 break;
00568 default:
00569 qualifier = PrintfQualifierInt;
00570 --i;
00571 }
00572
00573 base = 10;
00574
00575 switch (c = fmt[i]) {
00576
00577
00578
00579
00580 case 's':
00581 if ((retval = print_string(va_arg(ap, char*), width, precision, flags, ps)) < 0) {
00582 counter = -counter;
00583 goto out;
00584 };
00585
00586 counter += retval;
00587 j = i + 1;
00588 goto next_char;
00589 case 'c':
00590 c = va_arg(ap, unsigned int);
00591 if ((retval = print_char(c, width, flags, ps)) < 0) {
00592 counter = -counter;
00593 goto out;
00594 };
00595
00596 counter += retval;
00597 j = i + 1;
00598 goto next_char;
00599
00600
00601
00602
00603 case 'P':
00604 flags |= __PRINTF_FLAG_BIGCHARS;
00605 case 'p':
00606 flags |= __PRINTF_FLAG_PREFIX;
00607 base = 16;
00608 qualifier = PrintfQualifierPointer;
00609 break;
00610 case 'b':
00611 base = 2;
00612 break;
00613 case 'o':
00614 base = 8;
00615 break;
00616 case 'd':
00617 case 'i':
00618 flags |= __PRINTF_FLAG_SIGNED;
00619 case 'u':
00620 break;
00621 case 'X':
00622 flags |= __PRINTF_FLAG_BIGCHARS;
00623 case 'x':
00624 base = 16;
00625 break;
00626
00627 case '%':
00628 j = i;
00629 goto next_char;
00630
00631
00632
00633 default:
00634
00635
00636
00637
00638 goto next_char;
00639 }
00640
00641
00642
00643
00644 switch (qualifier) {
00645 case PrintfQualifierByte:
00646 size = sizeof(unsigned char);
00647 number = (__u64)va_arg(ap, unsigned int);
00648 break;
00649 case PrintfQualifierShort:
00650 size = sizeof(unsigned short);
00651 number = (__u64)va_arg(ap, unsigned int);
00652 break;
00653 case PrintfQualifierInt:
00654 size = sizeof(unsigned int);
00655 number = (__u64)va_arg(ap, unsigned int);
00656 break;
00657 case PrintfQualifierLong:
00658 size = sizeof(unsigned long);
00659 number = (__u64)va_arg(ap, unsigned long);
00660 break;
00661 case PrintfQualifierLongLong:
00662 size = sizeof(unsigned long long);
00663 number = (__u64)va_arg(ap, unsigned long long);
00664 break;
00665 case PrintfQualifierPointer:
00666 size = sizeof(void *);
00667 number = (__u64)(unsigned long)va_arg(ap, void *);
00668 break;
00669 case PrintfQualifierNative:
00670 size = sizeof(__native);
00671 number = (__u64)va_arg(ap, __native);
00672 break;
00673 default:
00674 counter = -counter;
00675 goto out;
00676 }
00677
00678 if (flags & __PRINTF_FLAG_SIGNED) {
00679 if (number & (0x1 << (size*8 - 1))) {
00680 flags |= __PRINTF_FLAG_NEGATIVE;
00681
00682 if (size == sizeof(__u64)) {
00683 number = -((__s64)number);
00684 } else {
00685 number = ~number;
00686 number &= (~((0xFFFFFFFFFFFFFFFFll) << (size * 8)));
00687 number++;
00688 }
00689 }
00690 }
00691
00692 if ((retval = print_number(number, width, precision, base, flags, ps)) < 0) {
00693 counter = -counter;
00694 goto out;
00695 };
00696
00697 counter += retval;
00698 j = i + 1;
00699 }
00700 next_char:
00701
00702 ++i;
00703 }
00704
00705 if (i > j) {
00706 if ((retval = printf_putnchars(&fmt[j], (__native)(i - j), ps)) < 0) {
00707 counter = -counter;
00708 goto out;
00709
00710 }
00711 counter += retval;
00712 }
00713
00714 out:
00715 spinlock_unlock(&printflock);
00716 interrupts_restore(irqpri);
00717
00718 return counter;
00719 }
00720
00721