Contiki 2.6
|
00001 #include <strformat.h> 00002 00003 #define HAVE_DOUBLE 00004 00005 #define HAVE_LONGLONG 00006 #ifndef LARGEST_SIGNED 00007 #ifdef HAVE_LONGLONG 00008 #define LARGEST_SIGNED long long int 00009 #else 00010 #define LARGEST_UNSIGNED long int 00011 #endif 00012 #endif 00013 00014 #ifndef LARGEST_UNSIGNED 00015 #ifdef HAVE_LONGLONG 00016 #define LARGEST_UNSIGNED unsigned long long int 00017 #else 00018 #define LARGEST_UNSIGNED unsigned long int 00019 #endif 00020 #endif 00021 00022 #ifndef POINTER_INT 00023 #define POINTER_INT unsigned long 00024 #endif 00025 00026 typedef unsigned int FormatFlags; 00027 00028 #define MAKE_MASK(shift,size) (((1 << size) - 1) << (shift)) 00029 00030 #define JUSTIFY_SHIFT 0 00031 #define JUSTIFY_SIZE 1 00032 #define JUSTIFY_RIGHT 0x0000 00033 #define JUSTIFY_LEFT 0x0001 00034 #define JUSTIFY_MASK MAKE_MASK(JUSTIFY_SHIFT,JUSTIFY_SIZE) 00035 00036 00037 /* How a positive number is prefixed */ 00038 #define POSITIVE_SHIFT (JUSTIFY_SHIFT + JUSTIFY_SIZE) 00039 #define POSITIVE_NONE (0x0000 << POSITIVE_SHIFT) 00040 #define POSITIVE_SPACE (0x0001 << POSITIVE_SHIFT) 00041 #define POSITIVE_PLUS (0x0003 << POSITIVE_SHIFT) 00042 #define POSITIVE_MASK MAKE_MASK(POSITIVE_SHIFT, POSITIVE_SIZE) 00043 00044 #define POSITIVE_SIZE 2 00045 00046 #define ALTERNATE_FORM_SHIFT (POSITIVE_SHIFT + POSITIVE_SIZE) 00047 #define ALTERNATE_FORM_SIZE 1 00048 #define ALTERNATE_FORM (0x0001 << ALTERNATE_FORM_SHIFT) 00049 00050 00051 #define PAD_SHIFT (ALTERNATE_FORM_SHIFT + ALTERNATE_FORM_SIZE) 00052 #define PAD_SIZE 1 00053 #define PAD_SPACE (0x0000 << PAD_SHIFT) 00054 #define PAD_ZERO (0x0001 << PAD_SHIFT) 00055 00056 #define SIZE_SHIFT (PAD_SHIFT + PAD_SIZE) 00057 #define SIZE_SIZE 3 00058 #define SIZE_CHAR (0x0001 << SIZE_SHIFT) 00059 #define SIZE_SHORT (0x0002 << SIZE_SHIFT) 00060 #define SIZE_INT (0x0000 << SIZE_SHIFT) 00061 #define SIZE_LONG (0x0003 << SIZE_SHIFT) 00062 #define SIZE_LONGLONG (0x0004 << SIZE_SHIFT) 00063 #define SIZE_MASK MAKE_MASK(SIZE_SHIFT,SIZE_SIZE) 00064 00065 #define CONV_SHIFT (SIZE_SHIFT + SIZE_SIZE) 00066 #define CONV_SIZE 3 00067 #define CONV_INTEGER (0x0001 << CONV_SHIFT) 00068 #define CONV_FLOAT (0x0002 << CONV_SHIFT) 00069 #define CONV_POINTER (0x0003 << CONV_SHIFT) 00070 #define CONV_STRING (0x0004 << CONV_SHIFT) 00071 #define CONV_CHAR (0x0005 << CONV_SHIFT) 00072 #define CONV_PERCENT (0x0006 << CONV_SHIFT) 00073 #define CONV_WRITTEN (0x0007 << CONV_SHIFT) 00074 #define CONV_MASK MAKE_MASK(CONV_SHIFT, CONV_SIZE) 00075 00076 #define RADIX_SHIFT (CONV_SHIFT + CONV_SIZE) 00077 #define RADIX_SIZE 2 00078 #define RADIX_DECIMAL (0x0001 << RADIX_SHIFT) 00079 #define RADIX_OCTAL (0x0002 << RADIX_SHIFT) 00080 #define RADIX_HEX (0x0003 << RADIX_SHIFT) 00081 #define RADIX_MASK MAKE_MASK(RADIX_SHIFT,RADIX_SIZE) 00082 00083 #define SIGNED_SHIFT (RADIX_SHIFT + RADIX_SIZE) 00084 #define SIGNED_SIZE 1 00085 #define SIGNED_NO (0x0000 << SIGNED_SHIFT) 00086 #define SIGNED_YES (0x0001 << SIGNED_SHIFT) 00087 #define SIGNED_MASK MAKE_MASK(SIGNED_SHIFT,SIGNED_SIZE) 00088 00089 #define CAPS_SHIFT (SIGNED_SHIFT + SIGNED_SIZE) 00090 #define CAPS_SIZE 1 00091 #define CAPS_NO (0x0000 << CAPS_SHIFT) 00092 #define CAPS_YES (0x0001 << CAPS_SHIFT) 00093 #define CAPS_MASK MAKE_MASK(CAPS_SHIFT,CAPS_SIZE) 00094 00095 #define FLOAT_SHIFT (CAPS_SHIFT + CAPS_SIZE) 00096 #define FLOAT_SIZE 2 00097 #define FLOAT_NORMAL (0x0000 << FLOAT_SHIFT) 00098 #define FLOAT_EXPONENT (0x0001 << FLOAT_SHIFT) 00099 #define FLOAT_DEPENDANT (0x0002 << FLOAT_SHIFT) 00100 #define FLOAT_HEX (0x0003 << FLOAT_SHIFT) 00101 #define FLOAT_MASK MAKE_MASK(FLOAT_SHIFT, FLOAT_SIZE) 00102 00103 static FormatFlags 00104 parse_flags(const char **posp) 00105 { 00106 FormatFlags flags = 0; 00107 const char *pos = *posp; 00108 while (1) { 00109 switch(*pos) { 00110 case '-': 00111 flags |= JUSTIFY_LEFT; 00112 break; 00113 case '+': 00114 flags |= POSITIVE_PLUS; 00115 break; 00116 case ' ': 00117 flags |= POSITIVE_SPACE; 00118 break; 00119 case '#': 00120 flags |= ALTERNATE_FORM; 00121 break; 00122 case '0': 00123 flags |= PAD_ZERO; 00124 break; 00125 default: 00126 *posp = pos; 00127 return flags; 00128 } 00129 pos++; 00130 } 00131 00132 } 00133 00134 static unsigned int 00135 parse_uint(const char **posp) 00136 { 00137 unsigned v = 0; 00138 const char *pos = *posp; 00139 char ch; 00140 while((ch = *pos) >= '0' && ch <= '9') { 00141 v = v * 10 + (ch - '0'); 00142 pos++; 00143 } 00144 *posp = pos; 00145 return v; 00146 } 00147 00148 #define MAXCHARS_HEX ((sizeof(LARGEST_UNSIGNED) * 8) / 4 ) 00149 00150 /* Largest number of characters needed for converting an unsigned integer. 00151 */ 00152 #define MAXCHARS ((sizeof(LARGEST_UNSIGNED) * 8 + 2) / 3 ) 00153 00154 static unsigned int 00155 output_uint_decimal(char **posp, LARGEST_UNSIGNED v) 00156 { 00157 unsigned int len; 00158 char *pos = *posp; 00159 while (v > 0) { 00160 *--pos = (v % 10) + '0'; 00161 v /= 10; 00162 } 00163 len = *posp - pos; 00164 *posp = pos; 00165 return len; 00166 } 00167 00168 static unsigned int 00169 output_uint_hex(char **posp, LARGEST_UNSIGNED v, unsigned int flags) 00170 { 00171 unsigned int len; 00172 const char *hex = (flags & CAPS_YES) ?"0123456789ABCDEF":"0123456789abcdef"; 00173 char *pos = *posp; 00174 while (v > 0) { 00175 *--pos = hex[(v % 16)]; 00176 v /= 16; 00177 } 00178 len = *posp - pos; 00179 *posp = pos; 00180 return len; 00181 } 00182 00183 static unsigned int 00184 output_uint_octal(char **posp, LARGEST_UNSIGNED v) 00185 { 00186 unsigned int len; 00187 char *pos = *posp; 00188 while (v > 0) { 00189 *--pos = (v % 8) + '0'; 00190 v /= 8; 00191 } 00192 len = *posp - pos; 00193 *posp = pos; 00194 return len; 00195 } 00196 00197 static StrFormatResult 00198 fill_space(const StrFormatContext *ctxt, unsigned int len) 00199 { 00200 StrFormatResult res; 00201 static const char buffer[16] = " "; 00202 while(len > 16) { 00203 res = ctxt->write_str(ctxt->user_data, buffer, 16); 00204 if (res != STRFORMAT_OK) return res; 00205 len -= 16; 00206 } 00207 if (len == 0) return STRFORMAT_OK; 00208 return ctxt->write_str(ctxt->user_data, buffer, len); 00209 } 00210 00211 static StrFormatResult 00212 fill_zero(const StrFormatContext *ctxt, unsigned int len) 00213 { 00214 StrFormatResult res; 00215 static const char buffer[16] = "0000000000000000"; 00216 while(len > 16) { 00217 res = ctxt->write_str(ctxt->user_data, buffer, 16); 00218 if (res != STRFORMAT_OK) return res; 00219 len -= 16; 00220 } 00221 if (len == 0) return STRFORMAT_OK; 00222 return ctxt->write_str(ctxt->user_data, buffer, len); 00223 } 00224 00225 #define CHECKCB(res) {if ((res) != STRFORMAT_OK) {va_end(ap); return -1;}} 00226 00227 int 00228 format_str(const StrFormatContext *ctxt, const char *format, ...) 00229 { 00230 int ret; 00231 va_list ap; 00232 va_start(ap, format); 00233 ret = format_str_v(ctxt, format, ap); 00234 va_end(ap); 00235 return ret; 00236 } 00237 00238 int 00239 format_str_v(const StrFormatContext *ctxt, const char *format, va_list ap) 00240 { 00241 unsigned int written = 0; 00242 const char *pos = format; 00243 while(*pos != '\0') { 00244 FormatFlags flags; 00245 unsigned int minwidth = 0; 00246 int precision = -1; /* Negative means no precision */ 00247 char ch; 00248 const char *start = pos; 00249 while( (ch = *pos) != '\0' && ch != '%') pos++; 00250 if (pos != start) { 00251 CHECKCB(ctxt->write_str(ctxt->user_data, start, pos - start)); 00252 written += pos - start; 00253 } 00254 if (*pos == '\0') { 00255 va_end(ap); 00256 return written; 00257 } 00258 pos++; 00259 if (*pos == '\0') { 00260 va_end(ap); 00261 return written; 00262 } 00263 flags = parse_flags(&pos); 00264 00265 /* parse width */ 00266 if (*pos >= '1' && *pos <= '9') { 00267 minwidth = parse_uint(&pos); 00268 } else if (*pos == '*') { 00269 int w = va_arg(ap,int); 00270 if (w < 0) { 00271 flags |= JUSTIFY_LEFT; 00272 minwidth = w; 00273 } else { 00274 minwidth = w; 00275 } 00276 pos ++; 00277 } 00278 00279 /* parse precision */ 00280 if (*pos == '.') { 00281 pos++; 00282 if (*pos >= '0' && *pos <= '9') { 00283 precision = parse_uint(&pos); 00284 } else if (*pos == '*') { 00285 pos++; 00286 precision = va_arg(ap,int); 00287 } 00288 } 00289 if (*pos == 'l') { 00290 pos++; 00291 if (*pos == 'l') { 00292 flags |= SIZE_LONGLONG; 00293 pos++; 00294 } else { 00295 flags |= SIZE_LONG; 00296 } 00297 } else if (*pos == 'h') { 00298 pos++; 00299 if (*pos == 'h') { 00300 flags |= SIZE_CHAR; 00301 pos++; 00302 } else { 00303 flags |= SIZE_SHORT; 00304 } 00305 } 00306 00307 /* parse conversion specifier */ 00308 switch(*pos) { 00309 case 'd': 00310 case 'i': 00311 flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_YES; 00312 break; 00313 case 'u': 00314 flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_NO; 00315 break; 00316 case 'o': 00317 flags |= CONV_INTEGER | RADIX_OCTAL | SIGNED_NO; 00318 break; 00319 case 'x': 00320 flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO; 00321 break; 00322 case 'X': 00323 flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO | CAPS_YES; 00324 break; 00325 #ifdef HAVE_DOUBLE 00326 case 'f': 00327 flags |= CONV_FLOAT | FLOAT_NORMAL; 00328 break; 00329 case 'F': 00330 flags |= CONV_FLOAT | FLOAT_NORMAL | CAPS_YES; 00331 break; 00332 case 'e': 00333 flags |= CONV_FLOAT | FLOAT_EXPONENT; 00334 break; 00335 case 'E': 00336 flags |= CONV_FLOAT | FLOAT_EXPONENT | CAPS_YES; 00337 break; 00338 case 'g': 00339 flags |= CONV_FLOAT | FLOAT_DEPENDANT; 00340 break; 00341 case 'G': 00342 flags |= CONV_FLOAT | FLOAT_DEPENDANT | CAPS_YES; 00343 break; 00344 case 'a': 00345 flags |= CONV_FLOAT | FLOAT_HEX; 00346 break; 00347 case 'A': 00348 flags |= CONV_FLOAT | FLOAT_HEX | CAPS_YES; 00349 break; 00350 #endif 00351 case 'c': 00352 flags |= CONV_CHAR; 00353 break; 00354 case 's': 00355 flags |= CONV_STRING; 00356 break; 00357 case 'p': 00358 flags |= CONV_POINTER; 00359 break; 00360 case 'n': 00361 flags |= CONV_WRITTEN; 00362 break; 00363 case '%': 00364 flags |= CONV_PERCENT; 00365 break; 00366 case '\0': 00367 va_end(ap); 00368 return written; 00369 } 00370 pos++; 00371 switch(flags & CONV_MASK) { 00372 case CONV_PERCENT: 00373 CHECKCB(ctxt->write_str(ctxt->user_data, "%", 1)); 00374 written++; 00375 break; 00376 case CONV_INTEGER: 00377 { 00378 /* unsigned integers */ 00379 char *prefix = 0; /* sign, "0x" or "0X" */ 00380 unsigned int prefix_len = 0; 00381 char buffer[MAXCHARS]; 00382 char *conv_pos = buffer + MAXCHARS; 00383 unsigned int conv_len = 0; 00384 unsigned int width = 0; 00385 unsigned int precision_fill; 00386 unsigned int field_fill; 00387 LARGEST_UNSIGNED uvalue = 0; 00388 int negative = 0; 00389 00390 if (precision < 0) precision = 1; 00391 else flags &= ~PAD_ZERO; 00392 00393 if (flags & SIGNED_YES) { 00394 /* signed integers */ 00395 LARGEST_SIGNED value = 0; 00396 switch(flags & SIZE_MASK) { 00397 case SIZE_CHAR: 00398 value = (signed char)va_arg(ap, int); 00399 break; 00400 case SIZE_SHORT: 00401 value = (short)va_arg(ap, int); 00402 break; 00403 case SIZE_INT: 00404 value = va_arg(ap, int); 00405 break; 00406 #ifndef HAVE_LONGLONG 00407 case SIZE_LONGLONG: /* Treat long long the same as long */ 00408 #endif 00409 case SIZE_LONG: 00410 value = va_arg(ap, long); 00411 break; 00412 #ifdef HAVE_LONGLONG 00413 case SIZE_LONGLONG: 00414 value = va_arg(ap, long long); 00415 break; 00416 #endif 00417 } 00418 if (value < 0) { 00419 uvalue = -value; 00420 negative = 1; 00421 } else { 00422 uvalue = value; 00423 } 00424 } else { 00425 00426 switch(flags & SIZE_MASK) { 00427 case SIZE_CHAR: 00428 uvalue = (unsigned char)va_arg(ap,unsigned int); 00429 break; 00430 case SIZE_SHORT: 00431 uvalue = (unsigned short)va_arg(ap,unsigned int); 00432 break; 00433 case SIZE_INT: 00434 uvalue = va_arg(ap,unsigned int); 00435 break; 00436 #ifndef HAVE_LONGLONG 00437 case SIZE_LONGLONG: /* Treat long long the same as long */ 00438 #endif 00439 case SIZE_LONG: 00440 uvalue = va_arg(ap,unsigned long); 00441 break; 00442 #ifdef HAVE_LONGLONG 00443 case SIZE_LONGLONG: 00444 uvalue = va_arg(ap,unsigned long long); 00445 break; 00446 #endif 00447 } 00448 } 00449 00450 switch(flags & (RADIX_MASK)) { 00451 case RADIX_DECIMAL: 00452 conv_len = output_uint_decimal(&conv_pos,uvalue); 00453 break; 00454 case RADIX_OCTAL: 00455 conv_len = output_uint_octal(&conv_pos,uvalue); 00456 break; 00457 case RADIX_HEX: 00458 conv_len = output_uint_hex(&conv_pos,uvalue, flags); 00459 break; 00460 } 00461 00462 width += conv_len; 00463 precision_fill = (precision > conv_len) ? precision - conv_len : 0; 00464 if ((flags & (RADIX_MASK | ALTERNATE_FORM)) 00465 == (RADIX_OCTAL | ALTERNATE_FORM)) { 00466 if (precision_fill < 1) precision_fill = 1; 00467 } 00468 00469 width += precision_fill; 00470 00471 if ((flags & (RADIX_MASK | ALTERNATE_FORM)) 00472 == (RADIX_HEX | ALTERNATE_FORM) && uvalue != 0) { 00473 prefix_len = 2; 00474 if (flags & CAPS_YES) { 00475 prefix = "0X"; 00476 } else { 00477 prefix = "0x"; 00478 } 00479 } 00480 00481 if (flags & SIGNED_YES) { 00482 if (negative) { 00483 prefix = "-"; 00484 prefix_len = 1; 00485 } else { 00486 switch(flags & POSITIVE_MASK) { 00487 case POSITIVE_SPACE: 00488 prefix = " "; 00489 prefix_len = 1; 00490 break; 00491 case POSITIVE_PLUS: 00492 prefix = "+"; 00493 prefix_len = 1; 00494 break; 00495 } 00496 } 00497 } 00498 00499 width += prefix_len; 00500 00501 field_fill = (minwidth > width) ? minwidth - width : 0; 00502 00503 if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { 00504 if (flags & PAD_ZERO) { 00505 precision_fill += field_fill; 00506 field_fill = 0; /* Do not double count padding */ 00507 } else { 00508 CHECKCB(fill_space(ctxt,field_fill)); 00509 } 00510 } 00511 00512 if (prefix_len > 0) 00513 CHECKCB(ctxt->write_str(ctxt->user_data, prefix, prefix_len)); 00514 written += prefix_len; 00515 00516 CHECKCB(fill_zero(ctxt,precision_fill)); 00517 written += precision_fill; 00518 00519 CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len)); 00520 written += conv_len; 00521 00522 if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { 00523 CHECKCB(fill_space(ctxt,field_fill)); 00524 } 00525 written += field_fill; 00526 } 00527 break; 00528 case CONV_STRING: 00529 { 00530 unsigned int field_fill; 00531 unsigned int len; 00532 char *str = va_arg(ap,char *); 00533 if (str) { 00534 char *pos = str; 00535 while(*pos != '\0') pos++; 00536 len = pos - str; 00537 } else { 00538 str = "(null)"; 00539 len = 6; 00540 } 00541 if (precision >= 0 && precision < len) len = precision; 00542 field_fill = (minwidth > len) ? minwidth - len : 0; 00543 if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { 00544 CHECKCB(fill_space(ctxt,field_fill)); 00545 } 00546 CHECKCB(ctxt->write_str(ctxt->user_data, str,len)); 00547 written += len; 00548 if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { 00549 CHECKCB(fill_space(ctxt,field_fill)); 00550 } 00551 written += field_fill; 00552 } 00553 break; 00554 case CONV_POINTER: 00555 { 00556 LARGEST_UNSIGNED uvalue = 00557 (LARGEST_UNSIGNED)(POINTER_INT)va_arg(ap,void *); 00558 char buffer[MAXCHARS_HEX + 3]; 00559 char *conv_pos = buffer + MAXCHARS_HEX+3; 00560 unsigned int conv_len; 00561 unsigned int field_fill; 00562 00563 conv_len = output_uint_hex(&conv_pos,uvalue,flags); 00564 if (conv_len == 0) { 00565 *--conv_pos = '0'; 00566 conv_len++; 00567 } 00568 *--conv_pos = 'x'; 00569 *--conv_pos = '0'; 00570 *--conv_pos = '#'; 00571 conv_len += 3; 00572 00573 field_fill = (minwidth > conv_len) ? minwidth - conv_len : 0; 00574 00575 if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { 00576 CHECKCB(fill_space(ctxt,field_fill)); 00577 } 00578 00579 CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len)); 00580 written += conv_len; 00581 00582 if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { 00583 CHECKCB(fill_space(ctxt,field_fill)); 00584 } 00585 written += field_fill; 00586 } 00587 break; 00588 case CONV_CHAR: 00589 { 00590 char ch = va_arg(ap,int); 00591 unsigned int field_fill = (minwidth > 1) ? minwidth - 1 : 0; 00592 if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { 00593 CHECKCB(fill_space(ctxt,field_fill)); 00594 written += field_fill; 00595 } 00596 00597 CHECKCB(ctxt->write_str(ctxt->user_data, &ch, 1)); 00598 written++; 00599 00600 if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { 00601 CHECKCB(fill_space(ctxt,field_fill)); 00602 } 00603 written+= field_fill; 00604 } 00605 break; 00606 case CONV_WRITTEN: 00607 { 00608 int *p = va_arg(ap,int*); 00609 *p = written; 00610 } 00611 break; 00612 00613 } 00614 } 00615 00616 return written; 00617 }