git.zerfleddert.de Git - proxmark3-svn/blob

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 */

 #define OP_ZSTRING      'z'             /* zero-terminated string */

 #define OP_BSTRING      'p'             /* string preceded by length byte */

 #define OP_WSTRING      'P'             /* string preceded by length word */

 #define OP_SSTRING      'a'             /* string preceded by length size_t */

 #define OP_STRING       'A'             /* string */

 #define OP_FLOAT        'f'             /* float */

 #define OP_DOUBLE       'd'             /* double */

 #define OP_NUMBER       'n'             /* Lua number */

 #define OP_CHAR         'c'             /* char (1-byte int) */

 #define OP_BYTE         'C'             /* byte = unsigned char (1-byte unsigned int) */

 #define OP_SHORT        's'             /* short (2-byte int) */

 #define OP_USHORT       'S'             /* unsigned short (2-byte unsigned int) */

 #define OP_INT          'i'             /* int (4-byte int) */

 #define OP_UINT         'I'             /* unsigned int (4-byte unsigned int) */

 #define OP_LONG         'l'             /* long (8-byte int) */

 #define OP_ULONG        'L'             /* unsigned long (8-byte unsigned int) */

 #define OP_LITTLEENDIAN '<'             /* little endian */

 #define OP_BIGENDIAN    '>'             /* big endian */

 #define OP_NATIVE       '='             /* native endian */

31 #define OP_HEX 'H'

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"

 static void badcode(lua_State *L, int c)

43 {

44 char s[]="bad code `?'";

  s[sizeof(s)-3]=c;

  luaL_argerror(L,1,s);

47 }

 static int doendian(int c)

50 {

51 int x=1;

  int e=*(char*)&x;

  if (c==OP_LITTLEENDIAN) return !e;

  if (c==OP_BIGENDIAN) return e;

  if (c==OP_NATIVE) return 0;

56 return 0;

57 }

 static void doswap(int swap, void *p, size_t n)

60 {

61 if (swap)

62 {

   char *a=(char*)p;

64 int i,j;

   for (i=0, j=n-1, n=n/2; n--; i++, j--)

66 {

    char t=a[i]; a[i]=a[j]; a[j]=t;

68 }

69 }

70 }

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 }

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

 static int l_unpack(lua_State *L)               /** unpack(f,s, [init]) */

106 {

107 size_t len;

  const char *s=luaL_checklstring(L,2,&len); /* switched s and f */

  const char *f=luaL_checkstring(L,1);

  int i_read = luaL_optinteger(L,3,1)-1;

111 // int i_read = luaL_optint(L,3,1)-1;

112 unsigned int i;

  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);

  while (*f && done == 0)

123 {

124 int c=*f++;

125 int N=1;

   if (isdigit((int) (unsigned char) *f))

127 {

128 N=0;

    while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';

    if (N==0 && c==OP_STRING) { lua_pushliteral(L,""); ++n; }

131 }

   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;

     if (i+N>len) {done = 1; break; }

     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;

     if (i>=len) {done = 1; break; }

     l=strlen(s+i);

     lua_pushlstring(L,s+i,l);

158 i+=l+1;

159 ++n;

160 break;

161 }

162

    UNPACKSTRING(OP_BSTRING, uint8_t)

    UNPACKSTRING(OP_WSTRING, uint16_t)

    UNPACKSTRING(OP_SSTRING, uint32_t)

166 UNPACKNUMBER(OP_NUMBER, lua_Number)

    UNPACKNUMBER(OP_DOUBLE, double)

    UNPACKNUMBER(OP_FLOAT, float)

    UNPACKNUMBER(OP_CHAR, int8_t)

    UNPACKNUMBER(OP_BYTE, uint8_t)

    UNPACKNUMBER(OP_SHORT, int16_t)

    UNPACKNUMBER(OP_USHORT, uint16_t)

    UNPACKNUMBER(OP_INT, int32_t)

    UNPACKNUMBER(OP_UINT, uint32_t)

    UNPACKNUMBER(OP_LONG, int64_t)

    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++;

       if (i+N > len) {done = 1; break;}

       for (unsigned int ii = i; ii < i+N; ii++) {

     val = s[ii] & 0xF0;

187 val = val >> 4;

188 hdigit = HEXDIGITS(val);

     luaL_addlstring(&buf, &hdigit, 1);

190

     val = s[ii] & 0x0F;

192 hdigit = HEXDIGITS(val);

     luaL_addlstring(&buf, &hdigit, 1);

194 }

195 luaL_pushresult(&buf);

196 n++;

197 i += N;

198 N = 0;

199 break;

200 }

201

    case ' ': case ',':

203 break;

204 default:

205 badcode(L,c);

206 break;

207 }

208 }

  lua_pushnumber(L,i+1);

  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

 static int l_pack(lua_State *L)                 /** pack(f,...) */

236 {

237 int i=2;

  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;

   if (isdigit((int) (unsigned char) *f))

247 {

248 N=0;

    while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';

250 }

   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;

     const char *a=luaL_checklstring(L,i++,&l);

     luaL_addlstring(&b,a,l+(c==OP_ZSTRING));

267 break;

268 }

    PACKSTRING(OP_BSTRING, uint8_t)

    PACKSTRING(OP_WSTRING, uint16_t)

    PACKSTRING(OP_SSTRING, uint32_t)

272 PACKNUMBER(OP_NUMBER, lua_Number)

    PACKNUMBER(OP_DOUBLE, double)

    PACKNUMBER(OP_FLOAT, float)

    PACKNUMBER(OP_CHAR, int8_t)

    PACKNUMBER(OP_BYTE, uint8_t)

    PACKNUMBER(OP_SHORT, int16_t)

    PACKNUMBER(OP_USHORT, uint16_t)

    PACKNUMBER(OP_INT, int32_t)

    PACKNUMBER(OP_UINT, uint32_t)

    PACKNUMBER(OP_LONG, int64_t)

    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;

       const char *a = luaL_checklstring(L, i++, &l);

       for (ii = 0; ii < l; ii++) {

     if (isxdigit((int) (unsigned char) a[ii])) {

       if (isdigit((int) (unsigned char) a[ii])) {

         sbyte += a[ii] - '0';

294 odd++;

       } else if (a[ii] >= 'A' && a[ii] <= 'F') {

         sbyte += a[ii] - 'A' + 10;

297 odd++;

       } else if (a[ii] >= 'a' && a[ii] <= 'f') {

         sbyte += a[ii] - 'a' + 10;

300 odd++;

301 }

       if (odd == 1) {

303 sbyte = sbyte << 4;

       } else if (odd == 2) {

         luaL_addlstring(&b, (char *) &sbyte, 1);

306 sbyte = 0;

307 odd = 0;

308 }

     } else if (isspace(a[ii])) {

310 /* ignore */

311 } else {

312 /* err ... ignore too*/

313 }

314 }

       if (odd == 1) {

     luaL_addlstring(&b, (char *) &sbyte, 1);

317 }

318 break;

319 }

    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

 LUALIB_API int luaopen_binlib (lua_State *L) {

339 luaL_newlib(L, binlib);

340 return 1;

341 }

342 /*

343 ** Open bin library

344 */

 int set_bin_library (lua_State *L) {

346

    luaL_requiref(L, "bin", luaopen_binlib, 1);

    lua_pop(L, 1);

349 return 1;

350 }