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
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 #include <cfg/crt.h>
00100
00101 #include <string.h>
00102 #include "nut_io.h"
00103 #include <stdlib.h>
00104 #include <memdebug.h>
00105
00110
00111 #ifdef STDIO_FLOATING_POINT
00112
00113 #include <math.h>
00114 #define BUF 16
00115 #define DEFPREC 6
00116
00117 #if defined(__arm__)
00118
00119
00120
00121
00122
00123
00124
00125 extern char *_sbrk(size_t nbytes);
00126 char *(*sbrk_force)(size_t) = _sbrk;
00127 #endif
00128
00129 #else
00130
00131 #define BUF 16
00132
00133 #endif
00134
00135 #define PADSIZE 16
00136 static char blanks[PADSIZE] = { ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
00137 ' ', ' '
00138 };
00139 static char zeroes[PADSIZE] = { '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
00140 '0', '0'
00141 };
00142
00143
00144
00145
00146 static void _putpad(int _putb(int fd, CONST void *, size_t), int fd, char *padch, int count)
00147 {
00148 while (count > PADSIZE) {
00149 _putb(fd, padch, PADSIZE);
00150 count -= PADSIZE;
00151 }
00152 if (count > 0)
00153 _putb(fd, padch, count);
00154 }
00155
00156
00157
00158
00159 #define ALT 0x01
00160 #define LADJUST 0x04
00161 #define LONGINT 0x08
00162 #define ZEROPAD 0x10
00163
00175 int _putf(int _putb(int, CONST void *, size_t), int fd, CONST char *fmt, va_list ap)
00176 {
00177 uint8_t ch;
00178 int n;
00179 char *cp;
00180 uint8_t flags;
00181 int rc;
00182 int width;
00183 int prec;
00184 int dprec;
00185 int realsz;
00186 uint8_t sign;
00187 uint32_t ulval;
00188 int size;
00189 char *xdigs;
00190 char buf[BUF];
00191
00192 #ifdef STDIO_FLOATING_POINT
00193 double _double;
00194
00195 #ifdef __IMAGECRAFT__
00196 int iccfmt;
00197 int fps;
00198 extern char *FormatFP_1(int format, float f, unsigned flag, int field_width, int prec);
00199 extern char *ftoa(float f, int *status);
00200 #else
00201 char *dtostre(double f, char *str, uint8_t prec, uint8_t flags);
00202 #if __AVR_LIBC_VERSION__ >= 10600
00203 char *dtostrf(double f, signed char width, unsigned char prec, char *str);
00204 #else
00205 char *dtostrf(double f, char width, char prec, char *str);
00206 #endif
00207 #endif
00208
00209 #endif
00210
00211 rc = 0;
00212
00213 for (;;) {
00214
00215
00216
00217
00218 for (cp = (char *) fmt; (ch = *fmt) != 0 && ch != '%'; fmt++);
00219 if ((n = fmt - cp) != 0) {
00220 _putb(fd, cp, n);
00221 rc += n;
00222 }
00223 if (ch == 0)
00224 break;
00225 fmt++;
00226
00227
00228
00229
00230 flags = 0;
00231 sign = 0;
00232 width = 0;
00233 dprec = 0;
00234 prec = -1;
00235 #if defined(STDIO_FLOATING_POINT) && defined(__IMAGECRAFT__)
00236 iccfmt = 0;
00237 #endif
00238 for (;;) {
00239 ch = *fmt++;
00240 if (ch == ' ') {
00241 if (!sign)
00242 sign = ' ';
00243 } else if (ch == '+')
00244 sign = '+';
00245 else if (ch == '-')
00246 flags |= LADJUST;
00247 else if (ch == '#')
00248 flags |= ALT;
00249 else if (ch == '0')
00250 flags |= ZEROPAD;
00251 else if (ch == 'l')
00252 flags |= LONGINT;
00253 else if (ch == 'z') {
00254 if (sizeof(size_t) > sizeof(int)) {
00255 flags |= LONGINT;
00256 }
00257 }
00258 else if (ch == '*') {
00259 width = va_arg(ap, int);
00260 if (width < 0) {
00261 flags |= LADJUST;
00262 width = -width;
00263 }
00264 } else if (ch == '.') {
00265 if (*fmt == '*') {
00266 fmt++;
00267 prec = va_arg(ap, int);
00268 } else {
00269 prec = 0;
00270 while (*fmt >= '0' && *fmt <= '9')
00271 prec = 10 * prec + (*fmt++ - '0');
00272 }
00273 if (prec < 0)
00274 prec = -1;
00275 } else if (ch >= '1' && ch <= '9') {
00276 width = ch - '0';
00277 while (*fmt >= '0' && *fmt <= '9')
00278 width = 10 * width + (*fmt++ - '0');
00279 } else
00280 break;
00281 }
00282
00283
00284
00285
00286 switch (ch) {
00287 case 'c':
00288 *(cp = buf) = va_arg(ap, int);
00289 size = 1;
00290 sign = 0;
00291 break;
00292
00293 case 'P':
00294 #ifdef __HARVARD_ARCH__
00295
00296
00297
00298
00299 cp = va_arg(ap, char *);
00300 if (cp == 0) {
00301 ch = 's';
00302 goto putf_s;
00303 }
00304 size = strlen_P((PGM_P)cp);
00305 xdigs = malloc(size + 1);
00306 strcpy_P(xdigs, (PGM_P)cp);
00307 cp = xdigs;
00308 goto putf_s;
00309 #endif
00310
00311 case 's':
00312 cp = va_arg(ap, char *);
00313
00314 #ifdef __HARVARD_ARCH__
00315 putf_s:
00316 #endif
00317
00318 if (cp == 0)
00319 cp = "(null)";
00320 if (prec >= 0) {
00321 char *p = memchr(cp, 0, (size_t) prec);
00322
00323 if (p) {
00324 size = p - cp;
00325 if (size > prec)
00326 size = prec;
00327 } else
00328 size = prec;
00329 } else
00330 size = strlen(cp);
00331 sign = 0;
00332 break;
00333
00334 case 'u':
00335 sign = 0;
00336 case 'd':
00337 case 'i':
00338
00339 if (flags & LONGINT)
00340 ulval = va_arg(ap, uint32_t);
00341 else if (ch == 'u')
00342 ulval = va_arg(ap, unsigned int);
00343 else
00344 ulval = va_arg(ap, int);
00345 if (ch != 'u' && (long) ulval < 0) {
00346 ulval = (uint32_t) (-((long) ulval));
00347 sign = '-';
00348 }
00349 if ((dprec = prec) >= 0)
00350 flags &= ~ZEROPAD;
00351 cp = buf + BUF;
00352 if (ulval || prec) {
00353 if (ulval < 10)
00354 *--cp = (char) ulval + '0';
00355 else
00356 do {
00357 *--cp = (char) (ulval % 10) + '0';
00358 ulval /= 10;
00359 } while (ulval);
00360 }
00361 size = buf + BUF - cp;
00362 break;
00363
00364 case 'o':
00365 ulval = (flags & LONGINT) ? va_arg(ap, uint32_t) : va_arg(ap, unsigned int);
00366 sign = 0;
00367 if ((dprec = prec) >= 0)
00368 flags &= ~ZEROPAD;
00369 cp = buf + BUF;
00370 if (ulval || prec) {
00371 do {
00372 *--cp = (char) (ulval & 7) + '0';
00373 ulval >>= 3;
00374 } while (ulval);
00375 if ((flags & ALT) != 0 && *cp != '0')
00376 *--cp = '0';
00377 }
00378 size = buf + BUF - cp;
00379 break;
00380
00381 case 'p':
00382 case 'X':
00383 case 'x':
00384 if (ch == 'p') {
00385 ulval = (uintptr_t) va_arg(ap, void *);
00386 flags |= ALT;
00387 ch = 'x';
00388 } else
00389 ulval = (flags & LONGINT) ? va_arg(ap, uint32_t) : (uint32_t)
00390 va_arg(ap, unsigned int);
00391
00392 sign = 0;
00393 if ((dprec = prec) >= 0)
00394 flags &= ~ZEROPAD;
00395
00396 if (ch == 'X')
00397 xdigs = "0123456789ABCDEF";
00398 else
00399 xdigs = "0123456789abcdef";
00400
00401 cp = buf + BUF;
00402 do {
00403 *--cp = xdigs[ulval & 0x0f];
00404 ulval >>= 4;
00405 } while (ulval);
00406 if (flags & ALT) {
00407 *--cp = ch;
00408 *--cp = '0';
00409 }
00410 size = buf + BUF - cp;
00411 break;
00412
00413 #ifdef STDIO_FLOATING_POINT
00414 #ifdef __IMAGECRAFT__
00415 case 'G':
00416 iccfmt++;
00417 case 'g':
00418 iccfmt++;
00419 case 'E':
00420 iccfmt++;
00421 case 'e':
00422 iccfmt++;
00423 case 'f':
00424 if (prec == -1)
00425 prec = DEFPREC;
00426 _double = va_arg(ap, double);
00427
00428
00429 cp = ftoa(_double, &fps);
00430 size = strlen(cp);
00431 break;
00432 #elif defined(__arm__)
00433 case 'g':
00434 case 'G':
00435 case 'e':
00436 case 'E':
00437 case 'f':
00438 {
00439 int decpt;
00440 int sign;
00441 char *rve = buf;
00442 char *bp = buf;
00443
00444 if (prec == -1)
00445 prec = DEFPREC;
00446 _double = va_arg(ap, double);
00447 cp = _dtoa_r(_REENT, _double, 3, prec, &decpt, &sign, &rve);
00448 if (decpt == 9999) {
00449
00450 strcpy(bp, cp);
00451 } else {
00452
00453 if (decpt > 0) {
00454 while (*cp && decpt > 0) {
00455 *bp++ = *cp++;
00456 decpt--;
00457 }
00458 while (decpt > 0) {
00459 *bp++ = '0';
00460 decpt--;
00461 }
00462 } else {
00463 *bp++ = '0';
00464 }
00465 *bp++ = '.';
00466
00467 while (decpt < 0 && prec > 0) {
00468 *bp++ = '0';
00469 decpt++;
00470 prec--;
00471 }
00472 while (*cp && prec > 0) {
00473 *bp++ = *cp++;
00474 prec--;
00475 }
00476 while (prec > 0) {
00477 *bp++ = '0';
00478 prec--;
00479 }
00480 *bp = 0;
00481 }
00482 cp = buf;
00483 size = strlen(cp);
00484 }
00485 break;
00486 #else
00487 case 'g':
00488 case 'G':
00489 case 'e':
00490 case 'E':
00491 case 'f':
00492 if (prec == -1)
00493 prec = DEFPREC;
00494 _double = va_arg(ap, double);
00495 if (ch == 'f')
00496 dtostrf(_double, 1, prec, buf);
00497 else
00498 dtostre(_double, buf, prec, 1);
00499 cp = buf;
00500 size = strlen(buf);
00501 break;
00502 #endif
00503 #else
00504 case 'g':
00505 case 'G':
00506 case 'e':
00507 case 'E':
00508 case 'f':
00509 (void) va_arg(ap, long);
00510 #endif
00511
00512 default:
00513 if (ch == 0)
00514 return rc;
00515 cp = buf;
00516 *cp = ch;
00517 size = 1;
00518 sign = '\0';
00519 break;
00520 }
00521
00522
00523
00524
00525 realsz = dprec > size ? dprec : size;
00526 if (sign)
00527 realsz++;
00528
00529 if ((flags & (LADJUST | ZEROPAD)) == 0)
00530 _putpad(_putb, fd, blanks, width - realsz);
00531
00532 if (sign)
00533 _putb(fd, &sign, 1);
00534
00535 if ((flags & (LADJUST | ZEROPAD)) == ZEROPAD)
00536 _putpad(_putb, fd, zeroes, width - realsz);
00537
00538 _putpad(_putb, fd, zeroes, dprec - size);
00539
00540 if (size)
00541 _putb(fd, cp, size);
00542
00543 #ifdef __HARVARD_ARCH__
00544 if (ch == 'P')
00545 free(cp);
00546 #endif
00547
00548 if (flags & LADJUST)
00549 _putpad(_putb, fd, blanks, width - realsz);
00550
00551 if (width >= realsz)
00552 rc += width;
00553 else
00554 rc += realsz;
00555 }
00556 return rc;
00557 }
00558