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