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