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