cvs.zerfleddert.de Git - proxmark3-svn/blob - client/pm3_binlib.c

   1 /*
   2 * lpack.c
   3 * a Lua library for packing and unpacking binary data
   4 * Luiz Henrique de Figueiredo <lhf@tecgraf.puc-rio.br>
   5 * 29 Jun 2007 19:27:20
   6 * This code is hereby placed in the public domain.
   7 * with contributions from Ignacio Castao <castanyo@yahoo.es> and
   8 * Roberto Ierusalimschy <roberto@inf.puc-rio.br>.
   9 */
  10
  11 #define OP_ZSTRING      'z'             /* zero-terminated string */
  12 #define OP_BSTRING      'p'             /* string preceded by length byte */
  13 #define OP_WSTRING      'P'             /* string preceded by length word */
  14 #define OP_SSTRING      'a'             /* string preceded by length size_t */
  15 #define OP_STRING       'A'             /* string */
  16 #define OP_FLOAT        'f'             /* float */
  17 #define OP_DOUBLE       'd'             /* double */
  18 #define OP_NUMBER       'n'             /* Lua number */
  19 #define OP_CHAR         'c'             /* char (1-byte int) */
  20 #define OP_BYTE         'C'             /* byte = unsigned char (1-byte unsigned int) */
  21 #define OP_SHORT        's'             /* short (2-byte int) */
  22 #define OP_USHORT       'S'             /* unsigned short (2-byte unsigned int) */
  23 #define OP_INT          'i'             /* int (4-byte int) */
  24 #define OP_UINT         'I'             /* unsigned int (4-byte unsigned int) */
  25 #define OP_LONG         'l'             /* long (8-byte int) */
  26 #define OP_ULONG        'L'             /* unsigned long (8-byte unsigned int) */
  27 #define OP_LITTLEENDIAN '<'             /* little endian */
  28 #define OP_BIGENDIAN    '>'             /* big endian */
  29 #define OP_NATIVE       '='             /* native endian */
  30
  31 #define OP_HEX 'H'
  32
  33 #include <ctype.h>
  34 #include <string.h>
  35 #include <lua.h>
  36 #include <lualib.h>
  37 #include <lauxlib.h>
  38
  39 #include "pm3_binlib.h"
  40
  41
  42 static void badcode(lua_State *L, int c)
  43 {
  44  char s[]="bad code `?'";
  45  s[sizeof(s)-3]=c;
  46  luaL_argerror(L,1,s);
  47 }
  48
  49 static int doendian(int c)
  50 {
  51  int x=1;
  52  int e=*(char*)&x;
  53  if (c==OP_LITTLEENDIAN) return !e;
  54  if (c==OP_BIGENDIAN) return e;
  55  if (c==OP_NATIVE) return 0;
  56  return 0;
  57 }
  58
  59 static void doswap(int swap, void *p, size_t n)
  60 {
  61  if (swap)
  62  {
  63   char *a=(char*)p;
  64   int i,j;
  65   for (i=0, j=n-1, n=n/2; n--; i++, j--)
  66   {
  67    char t=a[i]; a[i]=a[j]; a[j]=t;
  68   }
  69  }
  70 }
  71
  72 #define UNPACKNUMBER(OP,T)              \
  73    case OP:                             \
  74    {                                    \
  75     T a;                                \
  76     int m=sizeof(a);                    \
  77     if (i+m>len) { done = 1;    break;} \
  78     memcpy(&a,s+i,m);                   \
  79     i+=m;                               \
  80     doswap(swap,&a,m);                  \
  81     lua_pushnumber(L,(lua_Number)a);    \
  82     ++n;                                \
  83     break;                              \
  84    }
  85
  86 #define UNPACKSTRING(OP,T)              \
  87    case OP:                             \
  88    {                                    \
  89     T l;                                \
  90     int m=sizeof(l);                    \
  91     if (i+m>len) { done = 1;    break; }        \
  92     memcpy(&l,s+i,m);                   \
  93     doswap(swap,&l,m);                  \
  94     if (i+m+l>len) { done = 1; break;}          \
  95     i+=m;                               \
  96     lua_pushlstring(L,s+i,l);           \
  97     i+=l;                               \
  98     ++n;                                \
  99     break;                              \
 100    }
 101
 102 #define HEXDIGITS(DIG) \
 103  "0123456789ABCDEF"[DIG]
 104
 105 static int l_unpack(lua_State *L)               /** unpack(f,s, [init]) */
 106 {
 107  size_t len;
 108  const char *s=luaL_checklstring(L,2,&len); /* switched s and f */
 109  const char *f=luaL_checkstring(L,1);
 110  int i_read = luaL_optint(L,3,1)-1;
 111  unsigned int i;
 112  if (i_read >= 0) {
 113    i = i_read;
 114  } else {
 115    i = 0;
 116  }
 117  int n=0;
 118  int swap=0;
 119  int done=0;
 120  lua_pushnil(L);
 121  while (*f && done == 0)
 122  {
 123   int c=*f++;
 124   int N=1;
 125   if (isdigit((int) (unsigned char) *f))
 126   {
 127    N=0;
 128    while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';
 129    if (N==0 && c==OP_STRING) { lua_pushliteral(L,""); ++n; }
 130   }
 131   while (N-- && done == 0) switch (c)
 132   {
 133    case OP_LITTLEENDIAN:
 134    case OP_BIGENDIAN:
 135    case OP_NATIVE:
 136    {
 137     swap=doendian(c);
 138     N=0;
 139     break;
 140    }
 141    case OP_STRING:
 142    {
 143     ++N;
 144     if (i+N>len) {done = 1; break; }
 145     lua_pushlstring(L,s+i,N);
 146     i+=N;
 147     ++n;
 148     N=0;
 149     break;
 150    }
 151    case OP_ZSTRING:
 152    {
 153     size_t l;
 154     if (i>=len) {done = 1; break; }
 155     l=strlen(s+i);
 156     lua_pushlstring(L,s+i,l);
 157     i+=l+1;
 158     ++n;
 159     break;
 160    }
 161    UNPACKSTRING(OP_BSTRING, unsigned char)
 162    UNPACKSTRING(OP_WSTRING, unsigned short)
 163    UNPACKSTRING(OP_SSTRING, size_t)
 164    UNPACKNUMBER(OP_NUMBER, lua_Number)
 165    UNPACKNUMBER(OP_DOUBLE, double)
 166    UNPACKNUMBER(OP_FLOAT, float)
 167    UNPACKNUMBER(OP_CHAR, char)
 168    UNPACKNUMBER(OP_BYTE, unsigned char)
 169    UNPACKNUMBER(OP_SHORT, short)
 170    UNPACKNUMBER(OP_USHORT, unsigned short)
 171    UNPACKNUMBER(OP_INT, int)
 172    UNPACKNUMBER(OP_UINT, unsigned int)
 173    UNPACKNUMBER(OP_LONG, long)
 174    UNPACKNUMBER(OP_ULONG, unsigned long)
 175   case OP_HEX:
 176     {
 177       luaL_Buffer buf;
 178       char hdigit = '0';
 179       int val = 0;
 180       luaL_buffinit(L,&buf);
 181       N++;
 182       if (i+N > len) {done = 1; break;}
 183       for (unsigned int ii = i; ii < i+N; ii++) {
 184     val = s[ii] & 0xF0;
 185     val = val >> 4;
 186     hdigit = HEXDIGITS(val);
 187     luaL_addlstring(&buf, &hdigit, 1);
 188
 189     val = s[ii] & 0x0F;
 190     hdigit = HEXDIGITS(val);
 191     luaL_addlstring(&buf, &hdigit, 1);
 192       }
 193       luaL_pushresult(&buf);
 194       n++;
 195       i += N;
 196       N = 0;
 197       break;
 198     }
 199
 200    case ' ': case ',':
 201     break;
 202    default:
 203     badcode(L,c);
 204     break;
 205   }
 206  }
 207  lua_pushnumber(L,i+1);
 208  lua_replace(L,-n-2);
 209  return n+1;
 210 }
 211
 212 #define PACKNUMBER(OP,T)                        \
 213    case OP:                                     \
 214    {                                            \
 215     T a=(T)luaL_checknumber(L,i++);             \
 216     doswap(swap,&a,sizeof(a));                  \
 217     luaL_addlstring(&b,(char*)&a,sizeof(a));    \
 218     break;                                      \
 219    }
 220
 221 #define PACKSTRING(OP,T)                        \
 222    case OP:                                     \
 223    {                                            \
 224     size_t l;                                   \
 225     const char *a=luaL_checklstring(L,i++,&l);  \
 226     T ll=(T)l;                                  \
 227     doswap(swap,&ll,sizeof(ll));                \
 228     luaL_addlstring(&b,(char*)&ll,sizeof(ll));  \
 229     luaL_addlstring(&b,a,l);                    \
 230     break;                                      \
 231    }
 232
 233 static int l_pack(lua_State *L)                 /** pack(f,...) */
 234 {
 235  int i=2;
 236  const char *f=luaL_checkstring(L,1);
 237  int swap=0;
 238  luaL_Buffer b;
 239  luaL_buffinit(L,&b);
 240  while (*f)
 241  {
 242   int c=*f++;
 243   int N=1;
 244   if (isdigit((int) (unsigned char) *f))
 245   {
 246    N=0;
 247    while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';
 248   }
 249   while (N--) switch (c)
 250   {
 251    case OP_LITTLEENDIAN:
 252    case OP_BIGENDIAN:
 253    case OP_NATIVE:
 254    {
 255     swap=doendian(c);
 256     N=0;
 257     break;
 258    }
 259    case OP_STRING:
 260    case OP_ZSTRING:
 261    {
 262     size_t l;
 263     const char *a=luaL_checklstring(L,i++,&l);
 264     luaL_addlstring(&b,a,l+(c==OP_ZSTRING));
 265     break;
 266    }
 267    PACKSTRING(OP_BSTRING, unsigned char)
 268    PACKSTRING(OP_WSTRING, unsigned short)
 269    PACKSTRING(OP_SSTRING, size_t)
 270    PACKNUMBER(OP_NUMBER, lua_Number)
 271    PACKNUMBER(OP_DOUBLE, double)
 272    PACKNUMBER(OP_FLOAT, float)
 273    PACKNUMBER(OP_CHAR, char)
 274    PACKNUMBER(OP_BYTE, unsigned char)
 275    PACKNUMBER(OP_SHORT, short)
 276    PACKNUMBER(OP_USHORT, unsigned short)
 277    PACKNUMBER(OP_INT, int)
 278    PACKNUMBER(OP_UINT, unsigned int)
 279    PACKNUMBER(OP_LONG, long)
 280    PACKNUMBER(OP_ULONG, unsigned long)
 281   case OP_HEX:
 282     { // doing digit parsing the lpack way
 283       unsigned char sbyte = 0;
 284       size_t l;
 285       unsigned int ii = 0;
 286       int odd = 0;
 287       const char *a = luaL_checklstring(L, i++, &l);
 288       for (ii = 0; ii < l; ii++) {
 289     if (isxdigit((int) (unsigned char) a[ii])) {
 290       if (isdigit((int) (unsigned char) a[ii])) {
 291         sbyte += a[ii] - '0';
 292         odd++;
 293       } else if (a[ii] >= 'A' && a[ii] <= 'F') {
 294         sbyte += a[ii] - 'A' + 10;
 295         odd++;
 296       } else if (a[ii] >= 'a' && a[ii] <= 'f') {
 297         sbyte += a[ii] - 'a' + 10;
 298         odd++;
 299       }
 300       if (odd == 1) {
 301         sbyte = sbyte << 4;
 302       } else if (odd == 2) {
 303         luaL_addlstring(&b, (char *) &sbyte, 1);
 304         sbyte = 0;
 305         odd = 0;
 306       }
 307     } else if (isspace(a[ii])) {
 308       /* ignore */
 309     } else {
 310       /* err ... ignore too*/
 311     }
 312       }
 313       if (odd == 1) {
 314     luaL_addlstring(&b, (char *) &sbyte, 1);
 315       }
 316       break;
 317     }
 318    case ' ': case ',':
 319     break;
 320    default:
 321     badcode(L,c);
 322     break;
 323   }
 324  }
 325  luaL_pushresult(&b);
 326  return 1;
 327 }
 328
 329 static const luaL_Reg binlib[] =
 330 {
 331         {"pack",        l_pack},
 332         {"unpack",      l_unpack},
 333         {NULL,  NULL}
 334 };
 335
 336 LUALIB_API int luaopen_binlib (lua_State *L) {
 337   luaL_newlib(L, binlib);
 338   return 1;
 339 }
 340 /*
 341 ** Open bin library
 342 */
 343 int set_bin_library (lua_State *L) {
 344
 345    luaL_requiref(L, "bin", luaopen_binlib, 1);
 346    lua_pop(L, 1);
 347   return 1;
 348 }
 349