| 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 | } |