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 #include <stdint.h>
  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_optinteger(L,3,1)-1;
 111  // int i_read = luaL_optint(L,3,1)-1;
 112  unsigned int i;
 113  if (i_read >= 0) {
 114    i = i_read;
 115  } else {
 116    i = 0;
 117  }
 118  int n=0;
 119  int swap=0;
 120  int done=0;
 121  lua_pushnil(L);
 122  while (*f && done == 0)
 123  {
 124   int c=*f++;
 125   int N=1;
 126   if (isdigit((int) (unsigned char) *f))
 127   {
 128    N=0;
 129    while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';
 130    if (N==0 && c==OP_STRING) { lua_pushliteral(L,""); ++n; }
 131   }
 132   while (N-- && done == 0) switch (c)
 133   {
 134    case OP_LITTLEENDIAN:
 135    case OP_BIGENDIAN:
 136    case OP_NATIVE:
 137    {
 138     swap=doendian(c);
 139     N=0;
 140     break;
 141    }
 142    case OP_STRING:
 143    {
 144     ++N;
 145     if (i+N>len) {done = 1; break; }
 146     lua_pushlstring(L,s+i,N);
 147     i+=N;
 148     ++n;
 149     N=0;
 150     break;
 151    }
 152    case OP_ZSTRING:
 153    {
 154     size_t l;
 155     if (i>=len) {done = 1; break; }
 156     l=strlen(s+i);
 157     lua_pushlstring(L,s+i,l);
 158     i+=l+1;
 159     ++n;
 160     break;
 161    }
 162
 163    UNPACKSTRING(OP_BSTRING, uint8_t)
 164    UNPACKSTRING(OP_WSTRING, uint16_t)
 165    UNPACKSTRING(OP_SSTRING, uint32_t)
 166    UNPACKNUMBER(OP_NUMBER, lua_Number)
 167    UNPACKNUMBER(OP_DOUBLE, double)
 168    UNPACKNUMBER(OP_FLOAT, float)
 169    UNPACKNUMBER(OP_CHAR, int8_t)
 170    UNPACKNUMBER(OP_BYTE, uint8_t)
 171    UNPACKNUMBER(OP_SHORT, int16_t)
 172    UNPACKNUMBER(OP_USHORT, uint16_t)
 173    UNPACKNUMBER(OP_INT, int32_t)
 174    UNPACKNUMBER(OP_UINT, uint32_t)
 175    UNPACKNUMBER(OP_LONG, int64_t)
 176    UNPACKNUMBER(OP_ULONG, uint64_t)
 177   case OP_HEX:
 178     {
 179       luaL_Buffer buf;
 180       char hdigit = '0';
 181       int val = 0;
 182       luaL_buffinit(L,&buf);
 183       N++;
 184       if (i+N > len) {done = 1; break;}
 185       for (unsigned int ii = i; ii < i+N; ii++) {
 186     val = s[ii] & 0xF0;
 187     val = val >> 4;
 188     hdigit = HEXDIGITS(val);
 189     luaL_addlstring(&buf, &hdigit, 1);
 190
 191     val = s[ii] & 0x0F;
 192     hdigit = HEXDIGITS(val);
 193     luaL_addlstring(&buf, &hdigit, 1);
 194       }
 195       luaL_pushresult(&buf);
 196       n++;
 197       i += N;
 198       N = 0;
 199       break;
 200     }
 201
 202    case ' ': case ',':
 203     break;
 204    default:
 205     badcode(L,c);
 206     break;
 207   }
 208  }
 209  lua_pushnumber(L,i+1);
 210  lua_replace(L,-n-2);
 211  return n+1;
 212 }
 213
 214 #define PACKNUMBER(OP,T)                        \
 215    case OP:                                     \
 216    {                                            \
 217     T a=(T)luaL_checknumber(L,i++);             \
 218     doswap(swap,&a,sizeof(a));                  \
 219     luaL_addlstring(&b,(char*)&a,sizeof(a));    \
 220     break;                                      \
 221    }
 222
 223 #define PACKSTRING(OP,T)                        \
 224    case OP:                                     \
 225    {                                            \
 226     size_t l;                                   \
 227     const char *a=luaL_checklstring(L,i++,&l);  \
 228     T ll=(T)l;                                  \
 229     doswap(swap,&ll,sizeof(ll));                \
 230     luaL_addlstring(&b,(char*)&ll,sizeof(ll));  \
 231     luaL_addlstring(&b,a,l);                    \
 232     break;                                      \
 233    }
 234
 235 static int l_pack(lua_State *L)                 /** pack(f,...) */
 236 {
 237  int i=2;
 238  const char *f=luaL_checkstring(L,1);
 239  int swap=0;
 240  luaL_Buffer b;
 241  luaL_buffinit(L,&b);
 242  while (*f)
 243  {
 244   int c=*f++;
 245   int N=1;
 246   if (isdigit((int) (unsigned char) *f))
 247   {
 248    N=0;
 249    while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';
 250   }
 251   while (N--) switch (c)
 252   {
 253    case OP_LITTLEENDIAN:
 254    case OP_BIGENDIAN:
 255    case OP_NATIVE:
 256    {
 257     swap=doendian(c);
 258     N=0;
 259     break;
 260    }
 261    case OP_STRING:
 262    case OP_ZSTRING:
 263    {
 264     size_t l;
 265     const char *a=luaL_checklstring(L,i++,&l);
 266     luaL_addlstring(&b,a,l+(c==OP_ZSTRING));
 267     break;
 268    }
 269    PACKSTRING(OP_BSTRING, uint8_t)
 270    PACKSTRING(OP_WSTRING, uint16_t)
 271    PACKSTRING(OP_SSTRING, uint32_t)
 272    PACKNUMBER(OP_NUMBER, lua_Number)
 273    PACKNUMBER(OP_DOUBLE, double)
 274    PACKNUMBER(OP_FLOAT, float)
 275    PACKNUMBER(OP_CHAR, int8_t)
 276    PACKNUMBER(OP_BYTE, uint8_t)
 277    PACKNUMBER(OP_SHORT, int16_t)
 278    PACKNUMBER(OP_USHORT, uint16_t)
 279    PACKNUMBER(OP_INT, int32_t)
 280    PACKNUMBER(OP_UINT, uint32_t)
 281    PACKNUMBER(OP_LONG, int64_t)
 282    PACKNUMBER(OP_ULONG, uint64_t)
 283   case OP_HEX:
 284     { // doing digit parsing the lpack way
 285       unsigned char sbyte = 0;
 286       size_t l;
 287       unsigned int ii = 0;
 288       int odd = 0;
 289       const char *a = luaL_checklstring(L, i++, &l);
 290       for (ii = 0; ii < l; ii++) {
 291     if (isxdigit((int) (unsigned char) a[ii])) {
 292       if (isdigit((int) (unsigned char) a[ii])) {
 293         sbyte += a[ii] - '0';
 294         odd++;
 295       } else if (a[ii] >= 'A' && a[ii] <= 'F') {
 296         sbyte += a[ii] - 'A' + 10;
 297         odd++;
 298       } else if (a[ii] >= 'a' && a[ii] <= 'f') {
 299         sbyte += a[ii] - 'a' + 10;
 300         odd++;
 301       }
 302       if (odd == 1) {
 303         sbyte = sbyte << 4;
 304       } else if (odd == 2) {
 305         luaL_addlstring(&b, (char *) &sbyte, 1);
 306         sbyte = 0;
 307         odd = 0;
 308       }
 309     } else if (isspace((unsigned char)a[ii])) {
 310       /* ignore */
 311     } else {
 312       /* err ... ignore too*/
 313     }
 314       }
 315       if (odd == 1) {
 316     luaL_addlstring(&b, (char *) &sbyte, 1);
 317       }
 318       break;
 319     }
 320    case ' ': case ',':
 321     break;
 322    default:
 323     badcode(L,c);
 324     break;
 325   }
 326  }
 327  luaL_pushresult(&b);
 328  return 1;
 329 }
 330
 331 static const luaL_Reg binlib[] =
 332 {
 333         {"pack",        l_pack},
 334         {"unpack",      l_unpack},
 335         {NULL,  NULL}
 336 };
 337
 338 LUALIB_API int luaopen_binlib (lua_State *L) {
 339   luaL_newlib(L, binlib);
 340   return 1;
 341 }
 342 /*
 343 ** Open bin library
 344 */
 345 int set_bin_library (lua_State *L) {
 346
 347    luaL_requiref(L, "bin", luaopen_binlib, 1);
 348    lua_pop(L, 1);
 349   return 1;
 350 }