firmware/common/sscanf.c

   1 #include <stdarg.h>
   2 #include <string.h>
   3 #include <stdbool.h>
   4
   5 static inline bool isspace(char c)
   6 {
   7     return (c == ' ') || (c == '\t') || (c == '\n');
   8 }
   9
  10 static inline bool isdigit(char c)
  11 {
  12     return (c >= '0') && (c <= '9');
  13 }
  14
  15 static inline bool isxdigit(char c)
  16 {
  17     return ((c >= '0') && (c <= '9'))
  18         || ((c >= 'a') && (c <= 'f')) || ((c >= 'A') && (c <= 'F'));
  19 }
  20
  21 static int parse_dec(int (*peek)(void *userp),
  22                      void (*pop)(void *userp),
  23                      void *userp,
  24                      long *vp)
  25 {
  26     long v = 0;
  27     int n = 0;
  28     int minus = 0;
  29     char ch;
  30
  31     if ((*peek)(userp) == '-')
  32     {
  33         (*pop)(userp);
  34         n++;
  35         minus = 1;
  36     }
  37
  38     ch = (*peek)(userp);
  39     if (!isdigit(ch))
  40         return -1;
  41
  42     do
  43     {
  44         v = v * 10 + ch - '0';
  45         (*pop)(userp);
  46         n++;
  47         ch = (*peek)(userp);
  48     } while (isdigit(ch));
  49
  50     *vp = minus ? -v : v;
  51     return n;
  52 }
  53
  54 static int parse_chars(int (*peek)(void *userp),
  55                        void (*pop)(void *userp),
  56                        void *userp,
  57                        char *vp,
  58                        bool fake)
  59 {
  60     int n = 0;
  61
  62     char *pt=vp;
  63
  64     while (!isspace((*peek)(userp)))
  65     {
  66         if(fake==false)
  67             *(pt++) = (*peek)(userp);
  68
  69         n++;
  70         (*pop)(userp);
  71     }
  72
  73     if(fake==false)
  74         (*pt)='\0';
  75
  76     return n;
  77 }
  78
  79 static int parse_hex(int (*peek)(void *userp),
  80                      void (*pop)(void *userp),
  81                      void *userp,
  82                      unsigned long *vp)
  83 {
  84     unsigned long v = 0;
  85     int n = 0;
  86     char ch;
  87
  88     ch = (*peek)(userp);
  89     if (!isxdigit(ch))
  90         return -1;
  91
  92     do
  93     {
  94         if (ch >= 'a')
  95             ch = ch - 'a' + 10;
  96         else if (ch >= 'A')
  97             ch = ch - 'A' + 10;
  98         else
  99             ch = ch - '0';
 100         v = v * 16 + ch;
 101         (*pop)(userp);
 102         n++;
 103         ch = (*peek)(userp);
 104     } while (isxdigit(ch));
 105
 106     *vp = v;
 107     return n;
 108 }
 109
 110 static int skip_spaces(int (*peek)(void *userp),
 111                        void (*pop)(void *userp),
 112                        void *userp)
 113 {
 114     int n = 0;
 115     while (isspace((*peek)(userp))) {
 116         n++;
 117         (*pop)(userp);
 118     }
 119     return n;
 120 }
 121
 122 static int scan(int (*peek)(void *userp),
 123                 void (*pop)(void *userp),
 124                 void *userp,
 125                 const char *fmt,
 126                 va_list ap)
 127 {
 128     char ch;
 129     int n = 0;
 130     int n_chars = 0;
 131     int r;
 132     long lval;
 133     bool skip=false;
 134     unsigned long ulval;
 135
 136     while ((ch = *fmt++) != '\0')
 137     {
 138         bool literal = false;
 139
 140         if (ch == '%')
 141         {
 142             ch = *fmt++;
 143
 144             if(ch== '*')  /* We should process this, but not store it in an arguement */
 145             {
 146                 ch=*fmt++;
 147                 skip=true;
 148             }
 149             else
 150             {
 151                 skip=false;
 152             }
 153
 154             switch (ch)
 155             {
 156                 case 'x':
 157                     n_chars += skip_spaces(peek, pop, userp);
 158                     if ((r = parse_hex(peek, pop, userp, &ulval)) >= 0)
 159                     {
 160                         if(skip==false)
 161                         {
 162                             *(va_arg(ap, unsigned int *)) = ulval;
 163                             n++;
 164                         }
 165                         n_chars += r;
 166                     }
 167                     else
 168                         return n;
 169                     break;
 170                 case 'd':
 171                     n_chars += skip_spaces(peek, pop, userp);
 172                     if ((r = parse_dec(peek, pop, userp, &lval)) >= 0)
 173                     {
 174                         if(skip==false)
 175                         {
 176                             *(va_arg(ap, int *)) = lval;
 177                             n++;
 178                         }
 179                         n_chars += r;
 180                     }
 181                     else
 182                         return n;
 183                     break;
 184                 case 'n':
 185                     if(skip==false)
 186                     {
 187                         *(va_arg(ap, int *)) = n_chars;
 188                         n++;
 189                     }
 190                     break;
 191                 case 'l':
 192                     n_chars += skip_spaces(peek, pop, userp);
 193                     ch = *fmt++;
 194                     switch (ch)
 195                     {
 196                         case 'x':
 197                             if ((r = parse_hex(peek, pop, userp, &ulval)) >= 0)
 198                             {
 199                                 if(skip==false)
 200                                 {
 201                                     *(va_arg(ap, unsigned long *)) = ulval;
 202                                     n++;
 203                                 }
 204                                 n_chars += r;
 205                             }
 206                             else
 207                                 return n;
 208                             break;
 209                         case 'd':
 210                             if ((r = parse_dec(peek, pop, userp, &lval)) >= 0)
 211                             {
 212                                 if(skip==false)
 213                                 {
 214                                     *(va_arg(ap, long *)) = lval;
 215                                     n++;
 216                                 }
 217                                 n_chars += r;
 218                             }
 219                             else
 220                                 return n;
 221                             break;
 222                         case '\0':
 223                             return n;
 224                         default:
 225                             literal = true;
 226                             break;
 227                     }
 228                     break;
 229                 case 's':
 230                     n_chars += skip_spaces(peek, pop, userp);
 231                     n_chars += parse_chars(peek,pop, userp,skip?0:va_arg(ap, char *), skip );
 232                     if(skip==false)
 233                     {
 234                         n++;
 235                     }
 236                     break;
 237                 case '\0':
 238                     return n;
 239                 default:
 240                     literal = true;
 241                     break;
 242             }
 243         } else
 244             literal = true;
 245
 246         if (literal)
 247         {
 248             n_chars += skip_spaces(peek, pop, userp);
 249             if ((*peek)(userp) != ch)
 250                 continue;
 251             else
 252             {
 253                 (*pop)(userp);
 254                 n_chars++;
 255             }
 256         }
 257     }
 258     return n;
 259 }
 260
 261 static int sspeek(void *userp)
 262 {
 263     return **((char **)userp);
 264 }
 265
 266 static void sspop(void *userp)
 267 {
 268     (*((char **)userp))++;
 269 }
 270
 271 int sscanf(const char *s, const char *fmt, ...)
 272 {
 273     int r;
 274     va_list ap;
 275     const char *p;
 276
 277     p = s;
 278     va_start(ap, fmt);
 279     r = scan(sspeek, sspop, &p, fmt, ap);
 280     va_end(ap);
 281     return r;
 282 }