[proxmark3-svn] / client / pm3_binlib.c

/*
* lpack.c
* a Lua library for packing and unpacking binary data
* Luiz Henrique de Figueiredo <lhf@tecgraf.puc-rio.br>
* 29 Jun 2007 19:27:20
* This code is hereby placed in the public domain.
* with contributions from Ignacio Castao <castanyo@yahoo.es> and
* Roberto Ierusalimschy <roberto@inf.puc-rio.br>.
*/

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

#define OP_HEX 'H'

#include <ctype.h>
#include <string.h>
#include <lua.h>
#include <lualib.h>
#include <lauxlib.h>
#include <stdint.h>
#include "pm3_binlib.h"


static void badcode(lua_State *L, int c)
{
 char s[]="bad code `?'";
 s[sizeof(s)-3]=c;
 luaL_argerror(L,1,s);
}

static int doendian(int c)
{
 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;
 return 0;
}

static void doswap(int swap, void *p, size_t n)
{
 if (swap)
 {
  char *a = (char*)p;
  int i,j;
  for (i=0, j=n-1, n=n/2; n--; i++, j--)
  {
   char t=a[i]; a[i]=a[j]; a[j]=t;
  }
 }
}

#define UNPACKNUMBER(OP,T)		\
   case OP:				\
   {					\
    T a;				\
    int m=sizeof(a);			\
    if (i+m>len) { done = 1;	break;}	\
    memcpy(&a,s+i,m);			\
    i+=m;				\
    doswap(swap,&a,m);			\
    lua_pushnumber(L,(lua_Number)a);	\
    ++n;				\
    break;				\
   }

#define UNPACKSTRING(OP,T)		\
   case OP:				\
   {					\
    T l;				\
    int m = sizeof(l);			\
    if (i + m > len) { done = 1;	break; }	\
    memcpy(&l, s+i, m);			\
    doswap(swap,&l,m);			\
    if (i + m + l > len) { done = 1; break;}		\
    i += m;				\
    lua_pushlstring(L,s+i,l);		\
    i += l;				\
    ++n;				\
    break;				\
   }

#define HEXDIGITS(DIG) \
 "0123456789ABCDEF"[DIG]

static int l_unpack(lua_State *L) 		/** unpack(f,s, [init]) */
{
 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 = (int)luaL_optint(L,(3),(1))-1;
 unsigned int i;
 if (i_read >= 0) {
   i = i_read;
 } else {
   i = 0;
 }
 int n=0;
 int swap=0;
 int done=0;
 lua_pushnil(L);
 while (*f && done == 0)
 {
  int c = *f++;
  int N = 1;
  if (isdigit((int) (unsigned char) *f))
  {
   N=0;
   while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';
   if (N==0 && c==OP_STRING) { lua_pushliteral(L,""); ++n; }
  }
  while (N-- && done == 0) switch (c)
  {
   case OP_LITTLEENDIAN:
   case OP_BIGENDIAN:
   case OP_NATIVE:
   {
    swap=doendian(c);
    N=0;
    break;
   }
   case OP_STRING:
   {
    ++N;
    if (i+N>len) {done = 1; break; }
    lua_pushlstring(L,s+i,N);
    i+=N;
    ++n;
    N=0;
    break;
   }
   case OP_ZSTRING:
   {
    size_t l;
    if (i>=len) {done = 1; break; }
    l=strlen(s+i);
    lua_pushlstring(L,s+i,l);
    i+=l+1;
    ++n;
    break;
   }

   UNPACKSTRING(OP_BSTRING, uint8_t)
   UNPACKSTRING(OP_WSTRING, uint16_t)
   UNPACKSTRING(OP_SSTRING, uint32_t)
   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)
  case OP_HEX:
    {
      luaL_Buffer buf;
      char hdigit = '0';
      int val = 0;
      luaL_buffinit(L,&buf);
      N++;
      if (i+N > len) {done = 1; break;}
      for (unsigned int ii = i; ii < i+N; ii++) {
    val = s[ii] & 0xF0;
    val = val >> 4;
    hdigit = HEXDIGITS(val);
    luaL_addlstring(&buf, &hdigit, 1);

    val = s[ii] & 0x0F;
    hdigit = HEXDIGITS(val);
    luaL_addlstring(&buf, &hdigit, 1);
      }
      luaL_pushresult(&buf);
      n++;
      i += N;
      N = 0;
      break;
    }

   case ' ': case ',':
    break;
   default:
    badcode(L,c);
    break;
  }
 }
 lua_pushnumber(L,i+1);
 lua_replace(L,-n-2);
 return n+1;
}

#define PACKNUMBER(OP,T)			\
   case OP:					\
   {						\
    T a=(T)luaL_checknumber(L,i++);		\
    doswap(swap,&a,sizeof(a));			\
    luaL_addlstring(&b,(char*)&a,sizeof(a));	\
    break;					\
   }

#define PACKSTRING(OP,T)			\
   case OP:					\
   {						\
    size_t l;					\
    const char *a=luaL_checklstring(L,i++,&l);	\
    T ll=(T)l;					\
    doswap(swap,&ll,sizeof(ll));		\
    luaL_addlstring(&b,(char*)&ll,sizeof(ll));	\
    luaL_addlstring(&b,a,l);			\
    break;					\
   }

static int l_pack(lua_State *L) 		/** pack(f,...) */
{
 int i = 2;
 const char *f = luaL_checkstring(L,1);
 int swap = 0;
 luaL_Buffer b;
 luaL_buffinit(L,&b);
 while (*f)
 {
  int c = *f++;
  int N = 1;
  if (isdigit((int) (unsigned char) *f))
  {
   N=0;
   while (isdigit((int) (unsigned char) *f)) N=10*N+(*f++)-'0';
  }
  while (N--) switch (c)
  {
   case OP_LITTLEENDIAN:
   case OP_BIGENDIAN:
   case OP_NATIVE:
   {
    swap=doendian(c);
    N=0;
    break;
   }
   case OP_STRING:
   case OP_ZSTRING:
   {
    size_t l;
    const char *a=luaL_checklstring(L,i++,&l);
    luaL_addlstring(&b,a,l+(c==OP_ZSTRING));
    break;
   }
   PACKSTRING(OP_BSTRING, uint8_t)
   PACKSTRING(OP_WSTRING, uint16_t)
   PACKSTRING(OP_SSTRING, uint32_t)
   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)
  case OP_HEX:
    { // doing digit parsing the lpack way
      unsigned char sbyte = 0;
      size_t l;
      unsigned int ii = 0;
      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';
        odd++;
      } else if (a[ii] >= 'A' && a[ii] <= 'F') {
        sbyte += a[ii] - 'A' + 10;
        odd++;
      } else if (a[ii] >= 'a' && a[ii] <= 'f') {
        sbyte += a[ii] - 'a' + 10;
        odd++;
      }
      if (odd == 1) {
        sbyte = sbyte << 4;
      } else if (odd == 2) {
        luaL_addlstring(&b, (char *) &sbyte, 1);
        sbyte = 0;
        odd = 0;
      }
    } else if (isspace(a[ii])) {
      /* ignore */
    } else {
      /* err ... ignore too*/
    }
      }
      if (odd == 1) {
    luaL_addlstring(&b, (char *) &sbyte, 1);
      }
      break;
    }
   case ' ': case ',':
    break;
   default:
    badcode(L,c);
    break;
  }
 }
 luaL_pushresult(&b);
 return 1;
}

static const luaL_Reg binlib[] =
{
	{"pack",	l_pack},
	{"unpack",	l_unpack},
	{NULL,	NULL}
};

LUALIB_API int luaopen_binlib (lua_State *L) {
  luaL_newlib(L, binlib);
  return 1;
}
/*
** Open bin library
*/
int set_bin_library (lua_State *L) {

   luaL_requiref(L, "bin", luaopen_binlib, 1);
   lua_pop(L, 1);
  return 1;
}
Commit	Line	Data
f057bddb	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>
ca363c23	38	#include <stdint.h>
f057bddb	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	{
7d0efb37	51	int x = 1;
7d0efb37	52	int e = (char)&x;
f057bddb	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	{
7d0efb37	63	char a = (char)p;
f057bddb	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; \
7d0efb37	90	int m = sizeof(l); \
	91	if (i + m > len) { done = 1; break; } \
	92	memcpy(&l, s+i, m); \
f057bddb	93	doswap(swap,&l,m); \
7d0efb37	94	if (i + m + l > len) { done = 1; break;} \
7d0efb37	95	i += m; \
f057bddb	96	lua_pushlstring(L,s+i,l); \
7d0efb37	97	i += l; \
f057bddb	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);
1a570b0a	110	int i_read = (int)luaL_optint(L,(3),(1))-1;
f057bddb	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	{
1c59e80a	123	int c = *f++;
1c59e80a	124	int N = 1;
f057bddb	125	if (isdigit((int) (unsigned char) *f))
	126	{
	127	N=0;
	128	while (isdigit((int) (unsigned char) f)) N=10N+(*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	}
ca363c23	161
	162	UNPACKSTRING(OP_BSTRING, uint8_t)
	163	UNPACKSTRING(OP_WSTRING, uint16_t)
	164	UNPACKSTRING(OP_SSTRING, uint32_t)
f057bddb	165	UNPACKNUMBER(OP_NUMBER, lua_Number)
	166	UNPACKNUMBER(OP_DOUBLE, double)
	167	UNPACKNUMBER(OP_FLOAT, float)
ca363c23	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)
f057bddb	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	{
1c59e80a	236	int i = 2;
	237	const char *f = luaL_checkstring(L,1);
	238	int swap = 0;
f057bddb	239	luaL_Buffer b;
	240	luaL_buffinit(L,&b);
	241	while (*f)
	242	{
1c59e80a	243	int c = *f++;
1c59e80a	244	int N = 1;
f057bddb	245	if (isdigit((int) (unsigned char) *f))
	246	{
	247	N=0;
	248	while (isdigit((int) (unsigned char) f)) N=10N+(*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	}
ca363c23	268	PACKSTRING(OP_BSTRING, uint8_t)
	269	PACKSTRING(OP_WSTRING, uint16_t)
	270	PACKSTRING(OP_SSTRING, uint32_t)
f057bddb	271	PACKNUMBER(OP_NUMBER, lua_Number)
	272	PACKNUMBER(OP_DOUBLE, double)
	273	PACKNUMBER(OP_FLOAT, float)
ca363c23	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)
f057bddb	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