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 #include <cfg/crt.h>
00081
00082 #include <string.h>
00083 #include "nut_io.h"
00084 #include <stdlib.h>
00085
00090
00091 #ifdef STDIO_FLOATING_POINT
00092
00093 #include <math.h>
00094 #define BUF 16
00095 #define DEFPREC 6
00096
00097 #else
00098
00099 #define BUF 16
00100
00101 #endif
00102
00103 #define PADSIZE 16
00104 static char blanks[PADSIZE] = { ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
00105 ' ', ' '
00106 };
00107 static char zeroes[PADSIZE] = { '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
00108 '0', '0'
00109 };
00110
00111
00112
00113
00114 static void _putpad(int _putb(int fd, CONST void *, size_t), int fd, char *padch, int count)
00115 {
00116 while (count > PADSIZE) {
00117 _putb(fd, padch, PADSIZE);
00118 count -= PADSIZE;
00119 }
00120 if (count > 0)
00121 _putb(fd, padch, count);
00122 }
00123
00124
00125
00126
00127 #define ALT 0x01
00128 #define LADJUST 0x04
00129 #define LONGINT 0x08
00130 #define ZEROPAD 0x10
00131
00143 int _putf(int _putb(int, CONST void *, size_t), int fd, CONST char *fmt, va_list ap)
00144 {
00145 u_char ch;
00146 int n;
00147 char *cp;
00148 u_char flags;
00149 int rc;
00150 int width;
00151 int prec;
00152 int dprec;
00153 int realsz;
00154 u_char sign;
00155 u_long ulval;
00156 int size;
00157 char *xdigs;
00158 char buf[BUF];
00159 #ifdef STDIO_FLOATING_POINT
00160 double _double;
00161 #ifdef __IMAGECRAFT__
00162 int iccfmt;
00163 int fps;
00164 extern char *FormatFP_1(int format, float f, unsigned flag, int field_width, int prec);
00165 extern char *ftoa(float f, int *status);
00166 #else
00167 char *dtostre(double f, char *str, u_char prec, u_char flags);
00168 char *dtostrf(double f, signed char width, unsigned char prec, char *str);
00169 #endif
00170 #endif
00171
00172 rc = 0;
00173
00174 for (;;) {
00175
00176
00177
00178
00179 for (cp = (char *) fmt; (ch = *fmt) != 0 && ch != '%'; fmt++);
00180 if ((n = fmt - cp) != 0) {
00181 _putb(fd, cp, n);
00182 rc += n;
00183 }
00184 if (ch == 0)
00185 break;
00186 fmt++;
00187
00188
00189
00190
00191 flags = 0;
00192 sign = 0;
00193 width = 0;
00194 dprec = 0;
00195 prec = -1;
00196 #if defined(STDIO_FLOATING_POINT) && defined(__IMAGECRAFT__)
00197 iccfmt = 0;
00198 #endif
00199 for (;;) {
00200 ch = *fmt++;
00201 if (ch == ' ') {
00202 if (!sign)
00203 sign = ' ';
00204 } else if (ch == '+')
00205 sign = '+';
00206 else if (ch == '-')
00207 flags |= LADJUST;
00208 else if (ch == '#')
00209 flags |= ALT;
00210 else if (ch == '0')
00211 flags |= ZEROPAD;
00212 else if (ch == 'l')
00213 flags |= LONGINT;
00214 else if (ch == 'z') {
00215 if (sizeof(size_t) > sizeof(int)) {
00216 flags |= LONGINT;
00217 }
00218 }
00219 else if (ch == '*') {
00220 width = va_arg(ap, int);
00221 if (width < 0) {
00222 flags |= LADJUST;
00223 width = -width;
00224 }
00225 } else if (ch == '.') {
00226 if (*fmt == '*') {
00227 fmt++;
00228 prec = va_arg(ap, int);
00229 } else {
00230 prec = 0;
00231 while (*fmt >= '0' && *fmt <= '9')
00232 prec = 10 * prec + (*fmt++ - '0');
00233 }
00234 if (prec < 0)
00235 prec = -1;
00236 } else if (ch >= '1' && ch <= '9') {
00237 width = ch - '0';
00238 while (*fmt >= '0' && *fmt <= '9')
00239 width = 10 * width + (*fmt++ - '0');
00240 } else
00241 break;
00242 }
00243
00244
00245
00246
00247 switch (ch) {
00248 case 'c':
00249 *(cp = buf) = va_arg(ap, int);
00250 size = 1;
00251 sign = 0;
00252 break;
00253
00254 case 'P':
00255 #ifdef __HARVARD_ARCH__
00256
00257
00258
00259
00260 cp = va_arg(ap, char *);
00261 if (cp == 0) {
00262 ch = 's';
00263 goto putf_s;
00264 }
00265 size = strlen_P(cp);
00266 xdigs = malloc(size + 1);
00267 strcpy_P(xdigs, cp);
00268 cp = xdigs;
00269 goto putf_s;
00270 #endif
00271
00272 case 's':
00273 cp = va_arg(ap, char *);
00274
00275 #ifdef __HARVARD_ARCH__
00276 putf_s:
00277 #endif
00278
00279 if (cp == 0)
00280 cp = "(null)";
00281 if (prec >= 0) {
00282 char *p = memchr(cp, 0, (size_t) prec);
00283
00284 if (p) {
00285 size = p - cp;
00286 if (size > prec)
00287 size = prec;
00288 } else
00289 size = prec;
00290 } else
00291 size = strlen(cp);
00292 sign = 0;
00293 break;
00294
00295 case 'u':
00296 sign = 0;
00297 case 'd':
00298 case 'i':
00299
00300 if (flags & LONGINT)
00301 ulval = va_arg(ap, u_long);
00302 else if (ch == 'u')
00303 ulval = va_arg(ap, u_int);
00304 else
00305 ulval = va_arg(ap, int);
00306 if (ch != 'u' && (long) ulval < 0) {
00307 ulval = (u_long) (-((long) ulval));
00308 sign = '-';
00309 }
00310 if ((dprec = prec) >= 0)
00311 flags &= ~ZEROPAD;
00312 cp = buf + BUF;
00313 if (ulval || prec) {
00314 if (ulval < 10)
00315 *--cp = (char) ulval + '0';
00316 else
00317 do {
00318 *--cp = (char) (ulval % 10) + '0';
00319 ulval /= 10;
00320 } while (ulval);
00321 }
00322 size = buf + BUF - cp;
00323 break;
00324
00325 case 'o':
00326 ulval = (flags & LONGINT) ? va_arg(ap, u_long) : va_arg(ap, u_int);
00327 sign = 0;
00328 if ((dprec = prec) >= 0)
00329 flags &= ~ZEROPAD;
00330 cp = buf + BUF;
00331 if (ulval || prec) {
00332 do {
00333 *--cp = (char) (ulval & 7) + '0';
00334 ulval >>= 3;
00335 } while (ulval);
00336 if ((flags & ALT) != 0 && *cp != '0')
00337 *--cp = '0';
00338 }
00339 size = buf + BUF - cp;
00340 break;
00341
00342 case 'p':
00343 case 'X':
00344 case 'x':
00345 if (ch == 'p') {
00346 ulval = (uptr_t) va_arg(ap, void *);
00347 flags |= ALT;
00348 ch = 'x';
00349 } else
00350 ulval = (flags & LONGINT) ? va_arg(ap, u_long) : (u_long)
00351 va_arg(ap, u_int);
00352
00353 sign = 0;
00354 if ((dprec = prec) >= 0)
00355 flags &= ~ZEROPAD;
00356
00357 if (ch == 'X')
00358 xdigs = "0123456789ABCDEF";
00359 else
00360 xdigs = "0123456789abcdef";
00361
00362 cp = buf + BUF;
00363 do {
00364 *--cp = xdigs[ulval & 0x0f];
00365 ulval >>= 4;
00366 } while (ulval);
00367 if (flags & ALT) {
00368 *--cp = ch;
00369 *--cp = '0';
00370 }
00371 size = buf + BUF - cp;
00372 break;
00373
00374 #ifdef STDIO_FLOATING_POINT
00375 #ifdef __IMAGECRAFT__
00376 case 'G':
00377 iccfmt++;
00378 case 'g':
00379 iccfmt++;
00380 case 'E':
00381 iccfmt++;
00382 case 'e':
00383 iccfmt++;
00384 case 'f':
00385 if (prec == -1)
00386 prec = DEFPREC;
00387 _double = va_arg(ap, double);
00388
00389
00390 cp = ftoa(_double, &fps);
00391 size = strlen(cp);
00392 break;
00393 #else
00394 case 'g':
00395 case 'G':
00396 case 'e':
00397 case 'E':
00398 case 'f':
00399 if (prec == -1)
00400 prec = DEFPREC;
00401 _double = va_arg(ap, double);
00402 if (ch == 'f')
00403 dtostrf(_double, 1, prec, buf);
00404 else
00405 dtostre(_double, buf, prec, 1);
00406 cp = buf;
00407 size = strlen(buf);
00408 break;
00409 #endif
00410 #else
00411 case 'g':
00412 case 'G':
00413 case 'e':
00414 case 'E':
00415 case 'f':
00416 (void) va_arg(ap, long);
00417 #endif
00418
00419 default:
00420 if (ch == 0)
00421 return rc;
00422 cp = buf;
00423 *cp = ch;
00424 size = 1;
00425 sign = '\0';
00426 break;
00427 }
00428
00429
00430
00431
00432 realsz = dprec > size ? dprec : size;
00433 if (sign)
00434 realsz++;
00435
00436 if ((flags & (LADJUST | ZEROPAD)) == 0)
00437 _putpad(_putb, fd, blanks, width - realsz);
00438
00439 if (sign)
00440 _putb(fd, &sign, 1);
00441
00442 if ((flags & (LADJUST | ZEROPAD)) == ZEROPAD)
00443 _putpad(_putb, fd, zeroes, width - realsz);
00444
00445 _putpad(_putb, fd, zeroes, dprec - size);
00446
00447 if (size)
00448 _putb(fd, cp, size);
00449
00450 #ifdef __HARVARD_ARCH__
00451 if (ch == 'P')
00452 free(cp);
00453 #endif
00454
00455 if (flags & LADJUST)
00456 _putpad(_putb, fd, blanks, width - realsz);
00457
00458 if (width >= realsz)
00459 rc += width;
00460 else
00461 rc += realsz;
00462 }
00463 return rc;
00464 }
00465