LPLD_OSKinetis_V2.rar_V2

/* * File: printk.c * Purpose: The standard C library routine printf(), but without * all the baggage. */ #include "common.h" #include <stdarg.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)