Contiki 2.6

strformat.c

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 }