kea128工程统一部份

/* * file printf.c */ #include <stdarg.h> #include "printf.h" /********************************************************************/ typedef struct { int dest; void (*func)(char); char *loc; } PRINTK_INFO; int printk(PRINTK_INFO *, const char *, va_list); /********************************************************************/ #define DEST_CONSOLE (1) #define DEST_STRING (2) #define FLAGS_MINUS (0x01) #define FLAGS_PLUS (0x02) #define FLAGS_SPACE (0x04) #define FLAGS_ZERO (0x08) #define FLAGS_POUND (0x10) #define IS_FLAG_MINUS(a) (a & FLAGS_MINUS) #define IS_FLAG_PLUS(a) (a & FLAGS_PLUS) #define IS_FLAG_SPACE(a) (a & FLAGS_SPACE) #define IS_FLAG_ZERO(a) (a & FLAGS_ZERO) #define IS_FLAG_POUND(a) (a & FLAGS_POUND) #define LENMOD_h (0x01) #define LENMOD_l (0x02) #define LENMOD_L (0x04) #define IS_LENMOD_h(a) (a & LENMOD_h) #define IS_LENMOD_l(a) (a & LENMOD_l) #define IS_LENMOD_L(a) (a & LENMOD_L) #define FMT_d (0x0001) #define FMT_o (0x0002) #define FMT_x (0x0004) #define FMT_X (0x0008) #define FMT_u (0x0010) #define FMT_c (0x0020) #define FMT_s (0x0040) #define FMT_p (0x0080) #define FMT_n (0x0100) #define IS_FMT_d(a) (a & FMT_d) #define IS_FMT_o(a) (a & FMT_o) #define IS_FMT_x(a) (a & FMT_x) #define IS_FMT_X(a) (a & FMT_X) #define IS_FMT_u(a) (a & FMT_u) #define IS_FMT_c(a) (a & FMT_c) #define IS_FMT_s(a) (a & FMT_s) #define IS_FMT_p(a) (a & FMT_p) #define IS_FMT_n(a) (a & FMT_n) /********************************************************************/ static void printk_putc(int c, int *count, PRINTK_INFO *info) { switch (info->dest) { case DEST_CONSOLE: info->func((char) c); break; case DEST_STRING: *(info->loc) = (unsigned char) c; ++(info->loc); break; default: break; } *count += 1; } /********************************************************************/ static int printk_mknumstr(char *numstr, void *nump, int neg, int radix) { int a, b, c; unsigned int ua, ub, uc; int nlen; char *nstrp; nlen = 0; nstrp = numstr; *nstrp++ = '\0'; if (neg) { a = *(int *) nump; if (a == 0) { *nstrp = '0'; ++nlen; goto done; } while (a != 0) { b = (int) a / (int) radix; c = (int) a - ((int) b * (int) radix); if (c < 0) { c = ~c + 1 + '0'; } else { c = c + '0'; } a = b; *nstrp++ = (char) c; ++nlen; } } else { ua = *(unsigned int *) nump; if (ua == 0) { *nstrp = '0'; ++nlen; goto done; } while (ua != 0) { ub = (unsigned int) ua / (unsigned int) radix; uc = (unsigned int) ua - ((unsigned int) ub * (unsigned int) radix); if (uc < 10) { uc = uc + '0'; } else { uc = uc - 10 + 'A'; } ua = ub; *nstrp++ = (char) uc; ++nlen; } } done: return nlen; } /********************************************************************/ static void printk_pad_zero(int curlen, int field_width, int *count, PRINTK_INFO *info) { int i; for (i = curlen; i < field_width; i++) { printk_putc('0', count, info); } } /********************************************************************/ static void printk_pad_space(int curlen, int field_width, int *count, PRINTK_INFO *info) { int i; for (i = curlen; i < field_width; i++) { printk_putc(' ', count, info); } } /********************************************************************/ int printk(PRINTK_INFO *info, const char *fmt, va_list ap) { /* va_list ap; */ char *p; int c; char vstr[33]; char *vstrp; int vlen; int done; int count = 0; int flags_used; int field_width; #if 0 int precision_used; int precision_width; int length_modifier; #endif int ival; int schar, dschar; int *ivalp; char *sval; int cval; unsigned int uval; /* * Start parsing apart the format string and display appropriate * formats and data. */ for (p = (char *) fmt; (c = *p) != 0; p++) { /* * All formats begin with a '%' marker. Special chars like * '\n' or '\t' are normally converted to the appropriate * character by the __compiler__. Thus, no need for this * routine to account for the '\' character. */ if (c != '%') { /* * This needs to be replaced with something like * 'out_char()' or call an OS routine. */ #ifndef UNIX_DEBUG if (c != '\n') { printk_putc(c, &count, info); } else { printk_putc(0x0D /* CR */, &count, info); printk_putc(0x0A /* LF */, &count, info); } #else printk_putc(c, &count, info); #endif /* * By using 'continue', the next iteration of the loop * is used, skipping the code that follows. */ continue; } /* * First check for specification modifier flags. */ flags_used = 0; done = FALSE; while (!done) { switch (/* c = */*++p) { case '-': flags_used |= FLAGS_MINUS; break; case '+': flags_used |= FLAGS_PLUS; break; case ' ': flags_used |= FLAGS_SPACE; break; case '0': flags_used |= FLAGS_ZERO; break; case '#': flags_used |= FLAGS_POUND; break; default: /* we've gone one char too far */ --p; done = TRUE; break; } } /* * Next check for minimum field width. */ field_width = 0; done = FALSE; while (!done) { switch (c = *++p) { case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': field_width = (field_width * 10) + (c - '0'); break; default: /* we've gone one char too far */ --p; done = TRUE; break; } } /* * Next check for the width and precision field separator. */ if (/* (c = *++p) */*++p == '.') { /* precision_used = TRUE; */ /* * Must get precision field width, if present. */ /* precision_width = 0; */ done = FALSE; while (!done) { switch (/* c = uncomment if used below */*++p) { case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': #if 0 precision_width = (precision_width * 10) + (c - '0'); #endif break; default: /* we've gone one char too far */ --p; done = TRUE; break; } } } else { /* we've gone one char too far */ --p; #if 0 precision_used = FALSE; precision_width = 0; #endif } /* * Check for the length modifier. */ /* length_modifier = 0; */ switch (/* c = */*++p) { case 'h': /* length_modifier |= LENMOD_h; */ break; case 'l': /* length_modifier |= LENMOD_l; */ break; case 'L': /* length_modifier |= LENMOD_L; */ break; default: /* we've gone one char too far */ --p; break; } /* * Now we're ready to examine the format. */ switch (c = *++p) { case 'd': case 'i': ival = (int) va_arg(ap, int); vlen = printk_mknumstr(vstr, &ival, TRUE, 10); vstrp = &vstr[vlen]; if (ival < 0) { schar = '-'; ++vlen; } else { if (IS_FLAG_PLUS(flags_used)) { schar = '+'; ++vlen; } else { if (IS_FLAG_SPACE(flags_used)) { schar = ' '; ++vlen; } else { schar = 0; } } } dschar = FALSE; /* * do the ZERO pad. */ if (IS_FLAG_ZERO(flags_used)) { if (schar) printk_putc(schar, &count, info); dschar = TRUE; printk_pad_zero(vlen, field_width, &count, info); vlen = field_width; } else { if (!IS_FLAG_MINUS(flags_used)) { printk_pad_space(vlen, field_width, &count, info); if (schar) printk_putc(schar, &count, info); dschar =