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
00030
00031
00032
00033
00034 #include <coremark.h>
00035 #include <stdarg.h>
00036
00037 #define ZEROPAD (1<<0)
00038 #define SIGN (1<<1)
00039 #define PLUS (1<<2)
00040 #define SPACE (1<<3)
00041 #define LEFT (1<<4)
00042 #define HEX_PREP (1<<5)
00043 #define UPPERCASE (1<<6)
00044
00045 #define is_digit(c) ((c) >= '0' && (c) <= '9')
00046
00047 static char *digits = "0123456789abcdefghijklmnopqrstuvwxyz";
00048 static char *upper_digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
00049
00050 static size_t strnlen(const char *s, size_t count)
00051 {
00052 const char *sc;
00053 for (sc = s; *sc != '\0' && count--; ++sc);
00054 return sc - s;
00055 }
00056
00057 static int skip_atoi(const char **s)
00058 {
00059 int i = 0;
00060 while (is_digit(**s)) i = i*10 + *((*s)++) - '0';
00061 return i;
00062 }
00063
00064 static char *number(char *str, long num, int base, int size, int precision, int type)
00065 {
00066 char c, sign, tmp[66];
00067 char *dig = digits;
00068 int i;
00069
00070 if (type & UPPERCASE) dig = upper_digits;
00071 if (type & LEFT) type &= ~ZEROPAD;
00072 if (base < 2 || base > 36) return 0;
00073
00074 c = (type & ZEROPAD) ? '0' : ' ';
00075 sign = 0;
00076 if (type & SIGN)
00077 {
00078 if (num < 0)
00079 {
00080 sign = '-';
00081 num = -num;
00082 size--;
00083 }
00084 else if (type & PLUS)
00085 {
00086 sign = '+';
00087 size--;
00088 }
00089 else if (type & SPACE)
00090 {
00091 sign = ' ';
00092 size--;
00093 }
00094 }
00095
00096 if (type & HEX_PREP)
00097 {
00098 if (base == 16)
00099 size -= 2;
00100 else if (base == 8)
00101 size--;
00102 }
00103
00104 i = 0;
00105
00106 if (num == 0)
00107 tmp[i++] = '0';
00108 else
00109 {
00110 while (num != 0)
00111 {
00112 tmp[i++] = dig[((unsigned long) num) % (unsigned) base];
00113 num = ((unsigned long) num) / (unsigned) base;
00114 }
00115 }
00116
00117 if (i > precision) precision = i;
00118 size -= precision;
00119 if (!(type & (ZEROPAD | LEFT))) while (size-- > 0) *str++ = ' ';
00120 if (sign) *str++ = sign;
00121
00122 if (type & HEX_PREP)
00123 {
00124 if (base == 8)
00125 *str++ = '0';
00126 else if (base == 16)
00127 {
00128 *str++ = '0';
00129 *str++ = digits[33];
00130 }
00131 }
00132
00133 if (!(type & LEFT)) while (size-- > 0) *str++ = c;
00134 while (i < precision--) *str++ = '0';
00135 while (i-- > 0) *str++ = tmp[i];
00136 while (size-- > 0) *str++ = ' ';
00137
00138 return str;
00139 }
00140
00141 static char *eaddr(char *str, unsigned char *addr, int size, int precision, int type)
00142 {
00143 char tmp[24];
00144 char *dig = digits;
00145 int i, len;
00146
00147 if (type & UPPERCASE) dig = upper_digits;
00148 len = 0;
00149 for (i = 0; i < 6; i++)
00150 {
00151 if (i != 0) tmp[len++] = ':';
00152 tmp[len++] = dig[addr[i] >> 4];
00153 tmp[len++] = dig[addr[i] & 0x0F];
00154 }
00155
00156 if (!(type & LEFT)) while (len < size--) *str++ = ' ';
00157 for (i = 0; i < len; ++i) *str++ = tmp[i];
00158 while (len < size--) *str++ = ' ';
00159
00160 return str;
00161 }
00162
00163 static char *iaddr(char *str, unsigned char *addr, int size, int precision, int type)
00164 {
00165 char tmp[24];
00166 int i, n, len;
00167
00168 len = 0;
00169 for (i = 0; i < 4; i++)
00170 {
00171 if (i != 0) tmp[len++] = '.';
00172 n = addr[i];
00173
00174 if (n == 0)
00175 tmp[len++] = digits[0];
00176 else
00177 {
00178 if (n >= 100)
00179 {
00180 tmp[len++] = digits[n / 100];
00181 n = n % 100;
00182 tmp[len++] = digits[n / 10];
00183 n = n % 10;
00184 }
00185 else if (n >= 10)
00186 {
00187 tmp[len++] = digits[n / 10];
00188 n = n % 10;
00189 }
00190
00191 tmp[len++] = digits[n];
00192 }
00193 }
00194
00195 if (!(type & LEFT)) while (len < size--) *str++ = ' ';
00196 for (i = 0; i < len; ++i) *str++ = tmp[i];
00197 while (len < size--) *str++ = ' ';
00198
00199 return str;
00200 }
00201
00202 #ifdef HAS_FLOAT
00203
00204 char *ecvtbuf(double arg, int ndigits, int *decpt, int *sign, char *buf);
00205 char *fcvtbuf(double arg, int ndigits, int *decpt, int *sign, char *buf);
00206
00207 static void parse_float(double value, char *buffer, char fmt, int precision)
00208 {
00209 int decpt, sign, exp, pos;
00210 char *digits = NULL;
00211 char cvtbuf[80];
00212 int capexp = 0;
00213 int magnitude;
00214
00215 if (fmt == 'G' || fmt == 'E')
00216 {
00217 capexp = 1;
00218 fmt += 'a' - 'A';
00219 }
00220
00221 if (fmt == 'g')
00222 {
00223 digits = ecvtbuf(value, precision, &decpt, &sign, cvtbuf);
00224 magnitude = decpt - 1;
00225 if (magnitude < -4 || magnitude > precision - 1)
00226 {
00227 fmt = 'e';
00228 precision -= 1;
00229 }
00230 else
00231 {
00232 fmt = 'f';
00233 precision -= decpt;
00234 }
00235 }
00236
00237 if (fmt == 'e')
00238 {
00239 digits = ecvtbuf(value, precision + 1, &decpt, &sign, cvtbuf);
00240
00241 if (sign) *buffer++ = '-';
00242 *buffer++ = *digits;
00243 if (precision > 0) *buffer++ = '.';
00244 memcpy(buffer, digits + 1, precision);
00245 buffer += precision;
00246 *buffer++ = capexp ? 'E' : 'e';
00247
00248 if (decpt == 0)
00249 {
00250 if (value == 0.0)
00251 exp = 0;
00252 else
00253 exp = -1;
00254 }
00255 else
00256 exp = decpt - 1;
00257
00258 if (exp < 0)
00259 {
00260 *buffer++ = '-';
00261 exp = -exp;
00262 }
00263 else
00264 *buffer++ = '+';
00265
00266 buffer[2] = (exp % 10) + '0';
00267 exp = exp / 10;
00268 buffer[1] = (exp % 10) + '0';
00269 exp = exp / 10;
00270 buffer[0] = (exp % 10) + '0';
00271 buffer += 3;
00272 }
00273 else if (fmt == 'f')
00274 {
00275 digits = fcvtbuf(value, precision, &decpt, &sign, cvtbuf);
00276 if (sign) *buffer++ = '-';
00277 if (*digits)
00278 {
00279 if (decpt <= 0)
00280 {
00281 *buffer++ = '0';
00282 *buffer++ = '.';
00283 for (pos = 0; pos < -decpt; pos++) *buffer++ = '0';
00284 while (*digits) *buffer++ = *digits++;
00285 }
00286 else
00287 {
00288 pos = 0;
00289 while (*digits)
00290 {
00291 if (pos++ == decpt) *buffer++ = '.';
00292 *buffer++ = *digits++;
00293 }
00294 }
00295 }
00296 else
00297 {
00298 *buffer++ = '0';
00299 if (precision > 0)
00300 {
00301 *buffer++ = '.';
00302 for (pos = 0; pos < precision; pos++) *buffer++ = '0';
00303 }
00304 }
00305 }
00306
00307 *buffer = '\0';
00308 }
00309
00310 static void decimal_point(char *buffer)
00311 {
00312 while (*buffer)
00313 {
00314 if (*buffer == '.') return;
00315 if (*buffer == 'e' || *buffer == 'E') break;
00316 buffer++;
00317 }
00318
00319 if (*buffer)
00320 {
00321 int n = strlen(buffer);
00322 while (n > 0)
00323 {
00324 buffer[n + 1] = buffer[n];
00325 n--;
00326 }
00327
00328 *buffer = '.';
00329 }
00330 else
00331 {
00332 *buffer++ = '.';
00333 *buffer = '\0';
00334 }
00335 }
00336
00337 static void cropzeros(char *buffer)
00338 {
00339 char *stop;
00340
00341 while (*buffer && *buffer != '.') buffer++;
00342 if (*buffer++)
00343 {
00344 while (*buffer && *buffer != 'e' && *buffer != 'E') buffer++;
00345 stop = buffer--;
00346 while (*buffer == '0') buffer--;
00347 if (*buffer == '.') buffer--;
00348 while (*++buffer = *stop++);
00349 }
00350 }
00351
00352 static char *flt(char *str, double num, int size, int precision, char fmt, int flags)
00353 {
00354 char tmp[80];
00355 char c, sign;
00356 int n, i;
00357
00358
00359 if (flags & LEFT) flags &= ~ZEROPAD;
00360
00361
00362 c = (flags & ZEROPAD) ? '0' : ' ';
00363 sign = 0;
00364 if (flags & SIGN)
00365 {
00366 if (num < 0.0)
00367 {
00368 sign = '-';
00369 num = -num;
00370 size--;
00371 }
00372 else if (flags & PLUS)
00373 {
00374 sign = '+';
00375 size--;
00376 }
00377 else if (flags & SPACE)
00378 {
00379 sign = ' ';
00380 size--;
00381 }
00382 }
00383
00384
00385 if (precision < 0)
00386 precision = 6;
00387
00388
00389 parse_float(num, tmp, fmt, precision);
00390
00391 if ((flags & HEX_PREP) && precision == 0) decimal_point(tmp);
00392 if (fmt == 'g' && !(flags & HEX_PREP)) cropzeros(tmp);
00393
00394 n = strlen(tmp);
00395
00396
00397 size -= n;
00398 if (!(flags & (ZEROPAD | LEFT))) while (size-- > 0) *str++ = ' ';
00399 if (sign) *str++ = sign;
00400 if (!(flags & LEFT)) while (size-- > 0) *str++ = c;
00401 for (i = 0; i < n; i++) *str++ = tmp[i];
00402 while (size-- > 0) *str++ = ' ';
00403
00404 return str;
00405 }
00406
00407 #endif
00408
00409 static int ee_vsprintf(char *buf, const char *fmt, va_list args)
00410 {
00411 int len;
00412 unsigned long num;
00413 int i, base;
00414 char *str;
00415 char *s;
00416
00417 int flags;
00418
00419 int field_width;
00420 int precision;
00421 int qualifier;
00422
00423 for (str = buf; *fmt; fmt++)
00424 {
00425 if (*fmt != '%')
00426 {
00427 *str++ = *fmt;
00428 continue;
00429 }
00430
00431
00432 flags = 0;
00433 repeat:
00434 fmt++;
00435 switch (*fmt)
00436 {
00437 case '-': flags |= LEFT; goto repeat;
00438 case '+': flags |= PLUS; goto repeat;
00439 case ' ': flags |= SPACE; goto repeat;
00440 case '#': flags |= HEX_PREP; goto repeat;
00441 case '0': flags |= ZEROPAD; goto repeat;
00442 }
00443
00444
00445 field_width = -1;
00446 if (is_digit(*fmt))
00447 field_width = skip_atoi(&fmt);
00448 else if (*fmt == '*')
00449 {
00450 fmt++;
00451 field_width = va_arg(args, int);
00452 if (field_width < 0)
00453 {
00454 field_width = -field_width;
00455 flags |= LEFT;
00456 }
00457 }
00458
00459
00460 precision = -1;
00461 if (*fmt == '.')
00462 {
00463 ++fmt;
00464 if (is_digit(*fmt))
00465 precision = skip_atoi(&fmt);
00466 else if (*fmt == '*')
00467 {
00468 ++fmt;
00469 precision = va_arg(args, int);
00470 }
00471 if (precision < 0) precision = 0;
00472 }
00473
00474
00475 qualifier = -1;
00476 if (*fmt == 'l' || *fmt == 'L')
00477 {
00478 qualifier = *fmt;
00479 fmt++;
00480 }
00481
00482
00483 base = 10;
00484
00485 switch (*fmt)
00486 {
00487 case 'c':
00488 if (!(flags & LEFT)) while (--field_width > 0) *str++ = ' ';
00489 *str++ = (unsigned char) va_arg(args, int);
00490 while (--field_width > 0) *str++ = ' ';
00491 continue;
00492
00493 case 's':
00494 s = va_arg(args, char *);
00495 if (!s) s = "<NULL>";
00496 len = strnlen(s, precision);
00497 if (!(flags & LEFT)) while (len < field_width--) *str++ = ' ';
00498 for (i = 0; i < len; ++i) *str++ = *s++;
00499 while (len < field_width--) *str++ = ' ';
00500 continue;
00501
00502 case 'p':
00503 if (field_width == -1)
00504 {
00505 field_width = 2 * sizeof(void *);
00506 flags |= ZEROPAD;
00507 }
00508 str = number(str, (unsigned long) va_arg(args, void *), 16, field_width, precision, flags);
00509 continue;
00510
00511 case 'A':
00512 flags |= UPPERCASE;
00513
00514 case 'a':
00515 if (qualifier == 'l')
00516 str = eaddr(str, va_arg(args, unsigned char *), field_width, precision, flags);
00517 else
00518 str = iaddr(str, va_arg(args, unsigned char *), field_width, precision, flags);
00519 continue;
00520
00521
00522 case 'o':
00523 base = 8;
00524 break;
00525
00526 case 'X':
00527 flags |= UPPERCASE;
00528
00529 case 'x':
00530 base = 16;
00531 break;
00532
00533 case 'd':
00534 case 'i':
00535 flags |= SIGN;
00536
00537 case 'u':
00538 break;
00539
00540 #ifdef HAS_FLOAT
00541
00542 case 'f':
00543 str = flt(str, va_arg(args, double), field_width, precision, *fmt, flags | SIGN);
00544 continue;
00545
00546 #endif
00547
00548 default:
00549 if (*fmt != '%') *str++ = '%';
00550 if (*fmt)
00551 *str++ = *fmt;
00552 else
00553 --fmt;
00554 continue;
00555 }
00556
00557 if (qualifier == 'l')
00558 num = va_arg(args, unsigned long);
00559 else if (flags & SIGN)
00560 num = va_arg(args, int);
00561 else
00562 num = va_arg(args, unsigned int);
00563
00564 str = number(str, num, base, field_width, precision, flags);
00565 }
00566
00567 *str = '\0';
00568 return str - buf;
00569 }
00570
00571 void uart_send_char(char c) {
00572 #error "You must implement the method uart_send_char to use this file!\n";
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585 }
00586
00587 int ee_printf(const char *fmt, ...)
00588 {
00589 char buf[80],*p;
00590 va_list args;
00591 int n=0;
00592
00593 va_start(args, fmt);
00594 ee_vsprintf(buf, fmt, args);
00595 va_end(args);
00596 p=buf;
00597 while (*p) {
00598 uart_send_char(*p);
00599 n++;
00600 p++;
00601 }
00602
00603 return n;
00604 }
00605