]> git.zerfleddert.de Git - proxmark3-svn/blob - client/pm3_binlib.c
fix 'hf iclass sim':
[proxmark3-svn] / 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 }
Impressum, Datenschutz