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