[proxmark3-svn] / client / util.c

//-----------------------------------------------------------------------------
// Copyright (C) 2010 iZsh <izsh at fail0verflow.com>
//
// This code is licensed to you under the terms of the GNU GPL, version 2 or,
// at your option, any later version. See the LICENSE.txt file for the text of
// the license.
//-----------------------------------------------------------------------------
// utilities
//-----------------------------------------------------------------------------

#include "util.h"

#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <stdarg.h>

#ifdef _WIN32
#include <windows.h>
#endif

#define MAX_BIN_BREAK_LENGTH   (3072+384+1)

#ifndef _WIN32
#include <termios.h>
#include <sys/ioctl.h> 
#include <unistd.h>

int ukbhit(void)
{
  int cnt = 0;
  int error;
  static struct termios Otty, Ntty;

  if ( tcgetattr(STDIN_FILENO, &Otty) == -1 ) return -1;
  Ntty = Otty;

  Ntty.c_iflag          = 0x0000;   // input mode
  Ntty.c_oflag          = 0x0000;   // output mode
  Ntty.c_lflag         &= ~ICANON;  // control mode = raw
  Ntty.c_cc[VMIN]       = 1;        // return if at least 1 character is in the queue
  Ntty.c_cc[VTIME]      = 0;   	    // no timeout. Wait forever
  
  if (0 == (error = tcsetattr(STDIN_FILENO, TCSANOW, &Ntty))) {   // set new attributes
    error += ioctl(STDIN_FILENO, FIONREAD, &cnt);                 // get number of characters availabe
    error += tcsetattr(STDIN_FILENO, TCSANOW, &Otty);             // reset attributes
  }

  return ( error == 0 ? cnt : -1 );
}

#else

#include <conio.h>
int ukbhit(void) {
	return kbhit();
}
#endif

// log files functions
void AddLogLine(char *file, char *extData, char *c) {
	FILE *fLog = NULL;
    char filename[FILE_PATH_SIZE] = {0x00};
    int len = 0;

    len = strlen(file);
    if (len > FILE_PATH_SIZE) len = FILE_PATH_SIZE;
    memcpy(filename, file, len);
   
	fLog = fopen(filename, "a");
	if (!fLog) {
		printf("Could not append log file %s", filename);
		return;
	}

	fprintf(fLog, "%s", extData);
	fprintf(fLog, "%s\n", c);
	fclose(fLog);
}

void AddLogHex(char *fileName, char *extData, const uint8_t * data, const size_t len){
	AddLogLine(fileName, extData, sprint_hex(data, len));
}

void AddLogUint64(char *fileName, char *extData, const uint64_t data) {
  char buf[100] = {0};
	sprintf(buf, "%x%x", (unsigned int)((data & 0xFFFFFFFF00000000) >> 32), (unsigned int)(data & 0xFFFFFFFF));
	AddLogLine(fileName, extData, buf);
}

void AddLogCurrentDT(char *fileName) {
	char buff[20];
	struct tm *curTime;

	time_t now = time(0);
	curTime = gmtime(&now);

	strftime (buff, sizeof(buff), "%Y-%m-%d %H:%M:%S", curTime);
	AddLogLine(fileName, "\nanticollision: ", buff);
}

void FillFileNameByUID(char *fileName, uint8_t * uid, char *ext, int byteCount) {
	char * fnameptr = fileName;
	memset(fileName, 0x00, 200);
	
	for (int j = 0; j < byteCount; j++, fnameptr += 2)
		sprintf(fnameptr, "%02x", (unsigned int) uid[j]); 
	sprintf(fnameptr, "%s", ext); 
}

// fill buffer from structure [{uint8_t data, size_t length},...]
int FillBuffer(uint8_t *data, size_t maxDataLength, size_t *dataLength, ...) {
	*dataLength = 0;
	va_list valist;
	va_start(valist, dataLength);
	
	uint8_t *vdata = NULL;
	size_t vlength = 0;
	do{
		vdata = va_arg(valist, uint8_t *);
		if (!vdata)
			break;
		
		vlength = va_arg(valist, size_t);
		if (*dataLength + vlength >  maxDataLength) {
			va_end(valist);
			return 1;
		}
		
		memcpy(&data[*dataLength], vdata, vlength);
		*dataLength += vlength;
		
	} while (vdata);
	
	va_end(valist);

	return 0;
}

void hex_to_buffer(const uint8_t *buf, const uint8_t *hex_data, const size_t hex_len, const size_t hex_max_len, 
	const size_t min_str_len, const size_t spaces_between, bool uppercase) {
		
	char *tmp = (char *)buf;
	size_t i;
	memset(tmp, 0x00, hex_max_len);

	int maxLen = ( hex_len > hex_max_len) ? hex_max_len : hex_len;

	for (i = 0; i < maxLen; ++i, tmp += 2 + spaces_between) {
		sprintf(tmp, (uppercase) ? "%02X" : "%02x", (unsigned int) hex_data[i]); 
		
		for (int j = 0; j < spaces_between; j++)
			sprintf(tmp + 2 + j, " ");
	}
	
	i *= (2 + spaces_between);
	int minStrLen = min_str_len > i ? min_str_len : 0;
	if (minStrLen > hex_max_len)
		minStrLen = hex_max_len;
	for(; i < minStrLen; i++, tmp += 1) 
		sprintf(tmp, " ");

	return;
}

// printing and converting functions

char *sprint_hex(const uint8_t *data, const size_t len) {
	static char buf[4097] = {0};
	
	hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, 0, 1, false);

	return buf;
}

char *sprint_hex_inrow_ex(const uint8_t *data, const size_t len, const size_t min_str_len) {
	static char buf[4097] = {0};

	hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, min_str_len, 0, false);

	return buf;
}

char *sprint_hex_inrow(const uint8_t *data, const size_t len) {
	return sprint_hex_inrow_ex(data, len, 0);
}

char *sprint_bin_break(const uint8_t *data, const size_t len, const uint8_t breaks) {
	// make sure we don't go beyond our char array memory
	int max_len;
	if (breaks==0)
		max_len = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len;
	else
		max_len = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks);

	static char buf[MAX_BIN_BREAK_LENGTH]; // 3072 + end of line characters if broken at 8 bits
	//clear memory
	memset(buf, 0x00, sizeof(buf));
	char *tmp = buf;

	size_t in_index = 0;
	// loop through the out_index to make sure we don't go too far
	for (size_t out_index=0; out_index < max_len; out_index++) {
		// set character - (should be binary but verify it isn't more than 1 digit)
		if (data[in_index]<10)
			sprintf(tmp++, "%u", (unsigned int) data[in_index]);
		// check if a line break is needed and we have room to print it in our array
		if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 < max_len) ) {
			// increment and print line break
			out_index++;
			sprintf(tmp++, "%s","\n");
		}
		in_index++;
	}

	return buf;
}

char *sprint_bin(const uint8_t *data, const size_t len) {
	return sprint_bin_break(data, len, 0);
}

char *sprint_ascii_ex(const uint8_t *data, const size_t len, const size_t min_str_len) {
	static char buf[1024];
	char *tmp = buf;
	memset(buf, 0x00, 1024);
	size_t max_len = (len > 1010) ? 1010 : len;
	size_t i = 0;
	while(i < max_len){
		char c = data[i];
		tmp[i] = ((c < 32) || (c == 127)) ? '.' : c;
		++i;
	}
	
	int minStrLen = min_str_len > i ? min_str_len : 0;
	for(; i < minStrLen; ++i) 
		tmp[i] = ' ';
	
	return buf;
}

void num_to_bytes(uint64_t n, size_t len, uint8_t* dest)
{
	while (len--) {
		dest[len] = (uint8_t) n;
		n >>= 8;
	}
}

uint64_t bytes_to_num(uint8_t* src, size_t len)
{
	uint64_t num = 0;
	while (len--)
	{
		num = (num << 8) | (*src);
		src++;
	}
	return num;
}

void num_to_bytebits(uint64_t	n, size_t len, uint8_t *dest) {
	while (len--) {
		dest[len] = n & 1;
		n >>= 1;
	}
}

//least significant bit first
void num_to_bytebitsLSBF(uint64_t n, size_t len, uint8_t *dest) {
	for(int i = 0 ; i < len ; ++i) {
		dest[i] =  n & 1;
		n >>= 1;
	}
}

// Swap bit order on a uint32_t value.  Can be limited by nrbits just use say 8bits reversal
// And clears the rest of the bits.
uint32_t SwapBits(uint32_t value, int nrbits) {
	uint32_t newvalue = 0;
	for(int i = 0; i < nrbits; i++) {
		newvalue ^= ((value >> i) & 1) << (nrbits - 1 - i);
	}
	return newvalue;
}

// aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
// to
// hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
// up to 64 bytes or 512 bits
uint8_t *SwapEndian64(const uint8_t *src, const size_t len, const uint8_t blockSize){
	static uint8_t buf[64];
	memset(buf, 0x00, 64);
	uint8_t *tmp = buf;
	for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){
		for (size_t i = 0; i < blockSize; i++){
			tmp[i+(blockSize*block)] = src[(blockSize-1-i)+(blockSize*block)];
		}
	}
	return tmp;
}

//assumes little endian
char * printBits(size_t const size, void const * const ptr)
{
    unsigned char *b = (unsigned char*) ptr;	
    unsigned char byte;
	static char buf[1024];
	char * tmp = buf;
    int i, j;

    for (i=size-1;i>=0;i--)
    {
        for (j=7;j>=0;j--)
        {
            byte = b[i] & (1<<j);
            byte >>= j;
            sprintf(tmp, "%u", (unsigned int)byte);
			tmp++;
        }
    }
	return buf;
}

char * printBitsPar(const uint8_t *b, size_t len) {
	static char buf1[512] = {0};
	static char buf2[512] = {0};
	static char *buf;
	if (buf != buf1)
		buf = buf1;
	else
		buf = buf2;
	memset(buf, 0x00, 512);

	for (int i = 0; i < len; i++) {
		buf[i] = ((b[i / 8] << (i % 8)) & 0x80) ? '1':'0';
	}
	return buf;
}


//  -------------------------------------------------------------------------
//  string parameters lib
//  -------------------------------------------------------------------------

//  -------------------------------------------------------------------------
//  line     - param line
//  bg, en   - symbol numbers in param line of beginning and ending parameter
//  paramnum - param number (from 0)
//  -------------------------------------------------------------------------
int param_getptr(const char *line, int *bg, int *en, int paramnum)
{
	int i;
	int len = strlen(line);
	
	*bg = 0;
	*en = 0;
	
  // skip spaces
	while (line[*bg] ==' ' || line[*bg]=='\t') (*bg)++;
	if (*bg >= len) {
		return 1;
	}

	for (i = 0; i < paramnum; i++) {
		while (line[*bg]!=' ' && line[*bg]!='\t' && line[*bg] != '\0') (*bg)++;
		while (line[*bg]==' ' || line[*bg]=='\t') (*bg)++;
		
		if (line[*bg] == '\0') return 1;
	}
	
	*en = *bg;
	while (line[*en] != ' ' && line[*en] != '\t' && line[*en] != '\0') (*en)++;
	
	(*en)--;

	return 0;
}


int param_getlength(const char *line, int paramnum)
{
	int bg, en;
	
	if (param_getptr(line, &bg, &en, paramnum)) return 0;

	return en - bg + 1;
}

char param_getchar(const char *line, int paramnum) {
	return param_getchar_indx(line, 0, paramnum);
}

char param_getchar_indx(const char *line, int indx, int paramnum) {
	int bg, en;
	
	if (param_getptr(line, &bg, &en, paramnum)) return 0x00;

	if (bg + indx > en)
		return '\0';
	
	return line[bg + indx];
}

uint8_t param_get8(const char *line, int paramnum)
{
	return param_get8ex(line, paramnum, 0, 10);
}

/**
 * @brief Reads a decimal integer (actually, 0-254, not 255)
 * @param line
 * @param paramnum
 * @return -1 if error
 */
uint8_t param_getdec(const char *line, int paramnum, uint8_t *destination)
{
	uint8_t val =  param_get8ex(line, paramnum, 255, 10);
	if( (int8_t) val == -1) return 1;
	(*destination) = val;
	return 0;
}
/**
 * @brief Checks if param is decimal
 * @param line
 * @param paramnum
 * @return
 */
uint8_t param_isdec(const char *line, int paramnum)
{
	int bg, en;
	//TODO, check more thorougly
	if (!param_getptr(line, &bg, &en, paramnum)) return 1;
		//		return strtoul(&line[bg], NULL, 10) & 0xff;

	return 0;
}

uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base)
{
	int bg, en;

	if (!param_getptr(line, &bg, &en, paramnum)) 
		return strtoul(&line[bg], NULL, base) & 0xff;
	else
		return deflt;
}

uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base)
{
	int bg, en;

	if (!param_getptr(line, &bg, &en, paramnum)) 
		return strtoul(&line[bg], NULL, base);
	else
		return deflt;
}

uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base)
{
	int bg, en;

	if (!param_getptr(line, &bg, &en, paramnum)) 
		return strtoull(&line[bg], NULL, base);
	else
		return deflt;
}

int param_gethex(const char *line, int paramnum, uint8_t * data, int hexcnt)
{
	int bg, en, temp, i;

	if (hexcnt % 2)
		return 1;
	
	if (param_getptr(line, &bg, &en, paramnum)) return 1;

	if (en - bg + 1 != hexcnt) 
		return 1;

	for(i = 0; i < hexcnt; i += 2) {
		if (!(isxdigit((unsigned char)line[bg + i]) && isxdigit((unsigned char)line[bg + i + 1])) )	return 1;
		
		sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
		data[i / 2] = temp & 0xff;
	}	

	return 0;
}
int param_gethex_ex(const char *line, int paramnum, uint8_t * data, int *hexcnt)
{
	int bg, en, temp, i;

	//if (hexcnt % 2)
	//	return 1;
	
	if (param_getptr(line, &bg, &en, paramnum)) return 1;

	*hexcnt = en - bg + 1;
	if (*hexcnt % 2) //error if not complete hex bytes
		return 1;

	for(i = 0; i < *hexcnt; i += 2) {
		if (!(isxdigit((unsigned char)line[bg + i]) && isxdigit((unsigned char)line[bg + i + 1])) )	return 1;
		
		sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
		data[i / 2] = temp & 0xff;
	}	

	return 0;
}

int param_gethex_to_eol(const char *line, int paramnum, uint8_t * data, int maxdatalen, int *datalen) {
	int bg, en;
	uint32_t temp;
	char buf[5] = {0};

	if (param_getptr(line, &bg, &en, paramnum)) return 1;

	*datalen = 0;
	
	int indx = bg;
	while (line[indx]) {
		if (line[indx] == '\t' || line[indx] == ' ') {
			indx++;
			continue;
		}
		
		if (isxdigit((unsigned char)line[indx])) {
			buf[strlen(buf) + 1] = 0x00;
			buf[strlen(buf)] = line[indx];
		} else {
			// if we have symbols other than spaces and hex
			return 1;
		}				

		if (*datalen >= maxdatalen) {
			// if we dont have space in buffer and have symbols to translate
			return 2;
		}

		if (strlen(buf) >= 2) {
			sscanf(buf, "%x", &temp);
			data[*datalen] = (uint8_t)(temp & 0xff);
			*buf = 0;
			(*datalen)++;
		}
		
		indx++;
	}

	if (strlen(buf) > 0) 
		//error when not completed hex bytes
		return 3;
		
	return 0;
}

int param_getstr(const char *line, int paramnum, char * str, size_t buffersize)
{
	int bg, en;

	if (param_getptr(line, &bg, &en, paramnum)) {	
		return 0;
	}

	// Prevent out of bounds errors
	if (en - bg + 1 >= buffersize) {
		printf("out of bounds error: want %d bytes have %zd bytes\n", en - bg + 1 + 1, buffersize);
		return 0;
	}
	
	memcpy(str, line + bg, en - bg + 1);
	str[en - bg + 1] = 0;
	
	return en - bg + 1;
}

/*
The following methods comes from Rfidler sourcecode.
https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
*/

// convert hex to sequence of 0/1 bit values
// returns number of bits converted
int hextobinarray(char *target, char *source)
{
    int length, i, count= 0;
    char* start = source;
    char x;

    length = strlen(source);
    // process 4 bits (1 hex digit) at a time
    while(length--)
    {
        x= *(source++);
        // capitalize
        if (x >= 'a' && x <= 'f')
            x -= 32;
        // convert to numeric value
        if (x >= '0' && x <= '9')
            x -= '0';
        else if (x >= 'A' && x <= 'F')
            x -= 'A' - 10;
        else {
        	printf("Discovered unknown character %c %d at idx %tu of %s\n", x, x, source - start, start);
            return 0;
        }
        // output
        for(i= 0 ; i < 4 ; ++i, ++count)
            *(target++)= (x >> (3 - i)) & 1;
    }
    
    return count;
}

// convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source)
// return number of bits converted
int binarraytohex(char *target,char *source, int length)
{
    unsigned char i, x;
    int j = length;

    if(j % 4)
        return 0;

    while(j)
    {
        for(i= x= 0 ; i < 4 ; ++i)
            x +=  ( source[i] << (3 - i));
        sprintf(target,"%X", (unsigned int)x);
        ++target;
        source += 4;
        j -= 4;
    }
    return length;
}

// return parity bit required to match type
uint8_t GetParity( uint8_t *bits, uint8_t type, int length)
{
    int x;

    for(x= 0 ; length > 0 ; --length)
        x += bits[length - 1];
    x %= 2;

    return x ^ type;
}

// add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
void wiegand_add_parity(uint8_t *target, uint8_t *source, uint8_t length)
{
    *(target++)= GetParity(source, EVEN, length / 2);
    memcpy(target, source, length);
    target += length;
    *(target)= GetParity(source + length / 2, ODD, length / 2);
}

// xor two arrays together for len items.  The dst array contains the new xored values.
void xor(unsigned char *dst, unsigned char *src, size_t len) {
   for( ; len > 0; len--,dst++,src++)
       *dst ^= *src;
}

// RotateLeft - Ultralight, Desfire, works on byte level
// 00-01-02  >> 01-02-00
void rol(uint8_t *data, const size_t len){
    uint8_t first = data[0];
    for (size_t i = 0; i < len-1; i++) {
        data[i] = data[i+1];
    }
    data[len-1] = first;
}


// Replace unprintable characters with a dot in char buffer
void clean_ascii(unsigned char *buf, size_t len) {
  for (size_t i = 0; i < len; i++) {
    if (!isprint(buf[i]))
      buf[i] = '.';
  }
}

// replace \r \n to \0
void strcleanrn(char *buf, size_t len) {
	strcreplace(buf, len, '\n', '\0');
	strcreplace(buf, len, '\r', '\0');
}

// replace char in buffer
void strcreplace(char *buf, size_t len, char from, char to) {
  for (size_t i = 0; i < len; i++) {
    if (buf[i] == from)
      buf[i] = to;
  }
}

char *strmcopy(char *buf) {
	char * str = NULL;
	if ((str = (char*) malloc(strlen(buf) + 1)) != NULL) {
		memset(str, 0, strlen(buf) + 1);
		strcpy(str, buf);
	}	
	return str;
}


// determine number of logical CPU cores (use for multithreaded functions)
extern int num_CPUs(void)
{
#if defined(_WIN32)
	#include <sysinfoapi.h>
	SYSTEM_INFO sysinfo;
	GetSystemInfo(&sysinfo);
	return sysinfo.dwNumberOfProcessors;
#elif defined(__linux__) || defined(__APPLE__)
	#include <unistd.h>
	return sysconf(_SC_NPROCESSORS_ONLN);
#else
	return 1;
#endif
}
Commit	Line	Data
20f9a2a1 M	1	//-----------------------------------------------------------------------------
	2	// Copyright (C) 2010 iZsh <izsh at fail0verflow.com>
	3	//
	4	// This code is licensed to you under the terms of the GNU GPL, version 2 or,
	5	// at your option, any later version. See the LICENSE.txt file for the text of
	6	// the license.
	7	//-----------------------------------------------------------------------------
	8	// utilities
	9	//-----------------------------------------------------------------------------
	10
	11	#include "util.h"
acf0582d	12
	13	#include <stdint.h>
	14	#include <string.h>
	15	#include <ctype.h>
	16	#include <stdlib.h>
	17	#include <stdio.h>
	18	#include <time.h>
03439be3	19	#include <stdarg.h>
acf0582d	20
ec9c7112	21	#ifdef _WIN32
	22	#include <windows.h>
	23	#endif
	24
ace26dbd	25	#define MAX_BIN_BREAK_LENGTH (3072+384+1)
534983d7	26
cb64309e	27	#ifndef _WIN32
873014de M	28	#include <termios.h>
873014de M	29	#include <sys/ioctl.h>
8c0ccdef	30	#include <unistd.h>
79544b28	31
f397b5cc M	32	int ukbhit(void)
	33	{
	34	int cnt = 0;
	35	int error;
	36	static struct termios Otty, Ntty;
	37
8c0ccdef	38	if ( tcgetattr(STDIN_FILENO, &Otty) == -1 ) return -1;
f397b5cc M	39	Ntty = Otty;
f397b5cc M	40
8c0ccdef	41	Ntty.c_iflag = 0x0000; // input mode
	42	Ntty.c_oflag = 0x0000; // output mode
	43	Ntty.c_lflag &= ~ICANON; // control mode = raw
	44	Ntty.c_cc[VMIN] = 1; // return if at least 1 character is in the queue
	45	Ntty.c_cc[VTIME] = 0; // no timeout. Wait forever
	46
	47	if (0 == (error = tcsetattr(STDIN_FILENO, TCSANOW, &Ntty))) { // set new attributes
	48	error += ioctl(STDIN_FILENO, FIONREAD, &cnt); // get number of characters availabe
	49	error += tcsetattr(STDIN_FILENO, TCSANOW, &Otty); // reset attributes
f397b5cc M	50	}
	51
	52	return ( error == 0 ? cnt : -1 );
	53	}
	54
	55	#else
acf0582d	56
f397b5cc M	57	#include <conio.h>
	58	int ukbhit(void) {
	59	return kbhit();
	60	}
	61	#endif
	62
55acbb2a	63	// log files functions
b915fda3	64	void AddLogLine(char file, char extData, char *c) {
55acbb2a	65	FILE *fLog = NULL;
b915fda3	66	char filename[FILE_PATH_SIZE] = {0x00};
	67	int len = 0;
	68
	69	len = strlen(file);
	70	if (len > FILE_PATH_SIZE) len = FILE_PATH_SIZE;
	71	memcpy(filename, file, len);
	72
	73	fLog = fopen(filename, "a");
55acbb2a	74	if (!fLog) {
b915fda3	75	printf("Could not append log file %s", filename);
55acbb2a M	76	return;
	77	}
	78
	79	fprintf(fLog, "%s", extData);
	80	fprintf(fLog, "%s\n", c);
	81	fclose(fLog);
	82	}
	83
	84	void AddLogHex(char fileName, char extData, const uint8_t * data, const size_t len){
	85	AddLogLine(fileName, extData, sprint_hex(data, len));
	86	}
	87
	88	void AddLogUint64(char fileName, char extData, const uint64_t data) {
	89	char buf[100] = {0};
	90	sprintf(buf, "%x%x", (unsigned int)((data & 0xFFFFFFFF00000000) >> 32), (unsigned int)(data & 0xFFFFFFFF));
	91	AddLogLine(fileName, extData, buf);
	92	}
	93
	94	void AddLogCurrentDT(char *fileName) {
	95	char buff[20];
	96	struct tm *curTime;
	97
	98	time_t now = time(0);
	99	curTime = gmtime(&now);
	100
	101	strftime (buff, sizeof(buff), "%Y-%m-%d %H:%M:%S", curTime);
	102	AddLogLine(fileName, "\nanticollision: ", buff);
	103	}
	104
e0c635d1	105	void FillFileNameByUID(char fileName, uint8_t uid, char *ext, int byteCount) {
55acbb2a M	106	char * fnameptr = fileName;
	107	memset(fileName, 0x00, 200);
	108
e0c635d1	109	for (int j = 0; j < byteCount; j++, fnameptr += 2)
d1869c33	110	sprintf(fnameptr, "%02x", (unsigned int) uid[j]);
55acbb2a	111	sprintf(fnameptr, "%s", ext);
55acbb2a M	112	}
55acbb2a M	113
6b882a39 OM	114	// fill buffer from structure [{uint8_t data, size_t length},...]
	115	int FillBuffer(uint8_t data, size_t maxDataLength, size_t dataLength, ...) {
	116	*dataLength = 0;
	117	va_list valist;
	118	va_start(valist, dataLength);
	119
	120	uint8_t *vdata = NULL;
	121	size_t vlength = 0;
	122	do{
	123	vdata = va_arg(valist, uint8_t *);
	124	if (!vdata)
	125	break;
	126
	127	vlength = va_arg(valist, size_t);
	128	if (*dataLength + vlength > maxDataLength) {
	129	va_end(valist);
	130	return 1;
	131	}
	132
	133	memcpy(&data[*dataLength], vdata, vlength);
	134	*dataLength += vlength;
	135
	136	} while (vdata);
	137
	138	va_end(valist);
	139
	140	return 0;
	141	}
	142
66efdc1f	143	void hex_to_buffer(const uint8_t buf, const uint8_t hex_data, const size_t hex_len, const size_t hex_max_len,
	144	const size_t min_str_len, const size_t spaces_between, bool uppercase) {
	145
	146	char tmp = (char )buf;
	147	size_t i;
edd4c838	148	memset(tmp, 0x00, hex_max_len);
66efdc1f	149
	150	int maxLen = ( hex_len > hex_max_len) ? hex_max_len : hex_len;
	151
	152	for (i = 0; i < maxLen; ++i, tmp += 2 + spaces_between) {
	153	sprintf(tmp, (uppercase) ? "%02X" : "%02x", (unsigned int) hex_data[i]);
	154
	155	for (int j = 0; j < spaces_between; j++)
	156	sprintf(tmp + 2 + j, " ");
	157	}
	158
	159	i *= (2 + spaces_between);
	160	int minStrLen = min_str_len > i ? min_str_len : 0;
	161	if (minStrLen > hex_max_len)
	162	minStrLen = hex_max_len;
	163	for(; i < minStrLen; i++, tmp += 1)
	164	sprintf(tmp, " ");
	165
	166	return;
	167	}
	168
55acbb2a	169	// printing and converting functions
f397b5cc	170
4973f23d	171	char sprint_hex(const uint8_t data, const size_t len) {
39cc1c87	172	static char buf[4097] = {0};
b915fda3	173
66efdc1f	174	hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, 0, 1, false);
534983d7	175
	176	return buf;
	177	}
f89c7050	178
3c5fce2b	179	char sprint_hex_inrow_ex(const uint8_t data, const size_t len, const size_t min_str_len) {
39cc1c87	180	static char buf[4097] = {0};
3c5fce2b	181
66efdc1f	182	hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, min_str_len, 0, false);
3c5fce2b OM	183
	184	return buf;
	185	}
	186
	187	char sprint_hex_inrow(const uint8_t data, const size_t len) {
	188	return sprint_hex_inrow_ex(data, len, 0);
	189	}
	190
2767fc02	191	char sprint_bin_break(const uint8_t data, const size_t len, const uint8_t breaks) {
ace26dbd	192	// make sure we don't go beyond our char array memory
7bc6fac3	193	int max_len;
	194	if (breaks==0)
	195	max_len = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len;
	196	else
	197	max_len = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks);
	198
ace26dbd	199	static char buf[MAX_BIN_BREAK_LENGTH]; // 3072 + end of line characters if broken at 8 bits
	200	//clear memory
	201	memset(buf, 0x00, sizeof(buf));
2767fc02	202	char *tmp = buf;
79544b28	203
ace26dbd	204	size_t in_index = 0;
ace26dbd	205	// loop through the out_index to make sure we don't go too far
b97311b1	206	for (size_t out_index=0; out_index < max_len; out_index++) {
8ea57060	207	// set character - (should be binary but verify it isn't more than 1 digit)
8ea57060	208	if (data[in_index]<10)
d1869c33	209	sprintf(tmp++, "%u", (unsigned int) data[in_index]);
6ca1477c	210	// check if a line break is needed and we have room to print it in our array
b9957414	211	if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 < max_len) ) {
ace26dbd	212	// increment and print line break
ace26dbd	213	out_index++;
2767fc02	214	sprintf(tmp++, "%s","\n");
ace26dbd	215	}
ace26dbd	216	in_index++;
2767fc02	217	}
79544b28	218
	219	return buf;
	220	}
	221
2767fc02	222	char sprint_bin(const uint8_t data, const size_t len) {
	223	return sprint_bin_break(data, len, 0);
	224	}
59f726c9	225
3c5fce2b	226	char sprint_ascii_ex(const uint8_t data, const size_t len, const size_t min_str_len) {
59f726c9	227	static char buf[1024];
	228	char *tmp = buf;
	229	memset(buf, 0x00, 1024);
	230	size_t max_len = (len > 1010) ? 1010 : len;
	231	size_t i = 0;
	232	while(i < max_len){
	233	char c = data[i];
	234	tmp[i] = ((c < 32) \|\| (c == 127)) ? '.' : c;
	235	++i;
	236	}
3c5fce2b OM	237
	238	int minStrLen = min_str_len > i ? min_str_len : 0;
	239	for(; i < minStrLen; ++i)
	240	tmp[i] = ' ';
	241
59f726c9	242	return buf;
	243	}
	244
f89c7050 M	245	void num_to_bytes(uint64_t n, size_t len, uint8_t* dest)
	246	{
	247	while (len--) {
	248	dest[len] = (uint8_t) n;
	249	n >>= 8;
	250	}
	251	}
	252
	253	uint64_t bytes_to_num(uint8_t* src, size_t len)
	254	{
	255	uint64_t num = 0;
	256	while (len--)
	257	{
	258	num = (num << 8) \| (*src);
	259	src++;
	260	}
	261	return num;
	262	}
f397b5cc	263
709665b5	264	void num_to_bytebits(uint64_t n, size_t len, uint8_t *dest) {
	265	while (len--) {
	266	dest[len] = n & 1;
	267	n >>= 1;
	268	}
	269	}
	270
59f726c9	271	//least significant bit first
	272	void num_to_bytebitsLSBF(uint64_t n, size_t len, uint8_t *dest) {
	273	for(int i = 0 ; i < len ; ++i) {
	274	dest[i] = n & 1;
	275	n >>= 1;
	276	}
	277	}
	278
2d42ea1e	279	// Swap bit order on a uint32_t value. Can be limited by nrbits just use say 8bits reversal
	280	// And clears the rest of the bits.
	281	uint32_t SwapBits(uint32_t value, int nrbits) {
	282	uint32_t newvalue = 0;
	283	for(int i = 0; i < nrbits; i++) {
	284	newvalue ^= ((value >> i) & 1) << (nrbits - 1 - i);
	285	}
	286	return newvalue;
	287	}
59f726c9	288
f9848fd6	289	// aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
	290	// to
	291	// hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
	292	// up to 64 bytes or 512 bits
2b3af97d	293	uint8_t SwapEndian64(const uint8_t src, const size_t len, const uint8_t blockSize){
	294	static uint8_t buf[64];
	295	memset(buf, 0x00, 64);
	296	uint8_t *tmp = buf;
	297	for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){
	298	for (size_t i = 0; i < blockSize; i++){
	299	tmp[i+(blockSizeblock)] = src[(blockSize-1-i)+(blockSizeblock)];
f9848fd6	300	}
f9848fd6	301	}
2b3af97d	302	return tmp;
f9848fd6	303	}
f9848fd6	304
79544b28	305	//assumes little endian
	306	char * printBits(size_t const size, void const * const ptr)
	307	{
	308	unsigned char b = (unsigned char) ptr;
	309	unsigned char byte;
	310	static char buf[1024];
	311	char * tmp = buf;
	312	int i, j;
	313
	314	for (i=size-1;i>=0;i--)
	315	{
	316	for (j=7;j>=0;j--)
	317	{
	318	byte = b[i] & (1<<j);
	319	byte >>= j;
d1869c33	320	sprintf(tmp, "%u", (unsigned int)byte);
79544b28	321	tmp++;
	322	}
	323	}
	324	return buf;
	325	}
	326
a37725fa OM	327	char * printBitsPar(const uint8_t *b, size_t len) {
	328	static char buf1[512] = {0};
	329	static char buf2[512] = {0};
	330	static char *buf;
	331	if (buf != buf1)
	332	buf = buf1;
	333	else
	334	buf = buf2;
	335	memset(buf, 0x00, 512);
	336
	337	for (int i = 0; i < len; i++) {
	338	buf[i] = ((b[i / 8] << (i % 8)) & 0x80) ? '1':'0';
	339	}
	340	return buf;
	341	}
	342
	343
9ca155ba M	344	// -------------------------------------------------------------------------
	345	// string parameters lib
	346	// -------------------------------------------------------------------------
	347
f397b5cc M	348	// -------------------------------------------------------------------------
f397b5cc M	349	// line - param line
3a05a1e7	350	// bg, en - symbol numbers in param line of beginning and ending parameter
f397b5cc M	351	// paramnum - param number (from 0)
	352	// -------------------------------------------------------------------------
	353	int param_getptr(const char line, int bg, int *en, int paramnum)
	354	{
	355	int i;
	356	int len = strlen(line);
	357
	358	*bg = 0;
	359	*en = 0;
	360
	361	// skip spaces
	362	while (line[bg] ==' ' \|\| line[bg]=='\t') (*bg)++;
	363	if (*bg >= len) {
	364	return 1;
	365	}
	366
	367	for (i = 0; i < paramnum; i++) {
	368	while (line[bg]!=' ' && line[bg]!='\t' && line[bg] != '\0') (bg)++;
	369	while (line[bg]==' ' \|\| line[bg]=='\t') (*bg)++;
	370
	371	if (line[*bg] == '\0') return 1;
	372	}
	373
	374	en = bg;
	375	while (line[en] != ' ' && line[en] != '\t' && line[en] != '\0') (en)++;
	376
	377	(*en)--;
	378
	379	return 0;
	380	}
	381
31abe49f	382
3a05a1e7 OM	383	int param_getlength(const char *line, int paramnum)
	384	{
	385	int bg, en;
	386
	387	if (param_getptr(line, &bg, &en, paramnum)) return 0;
	388
	389	return en - bg + 1;
	390	}
	391
8019540b	392	char param_getchar(const char *line, int paramnum) {
	393	return param_getchar_indx(line, 0, paramnum);
	394	}
	395
	396	char param_getchar_indx(const char *line, int indx, int paramnum) {
f397b5cc M	397	int bg, en;
	398
	399	if (param_getptr(line, &bg, &en, paramnum)) return 0x00;
	400
8019540b	401	if (bg + indx > en)
	402	return '\0';
	403
	404	return line[bg + indx];
f397b5cc M	405	}
	406
	407	uint8_t param_get8(const char *line, int paramnum)
	408	{
6ca1477c	409	return param_get8ex(line, paramnum, 0, 10);
f397b5cc M	410	}
f397b5cc M	411
f6d9fb17	412	/**
31abe49f	413	* @brief Reads a decimal integer (actually, 0-254, not 255)
f6d9fb17 MHS	414	* @param line
f6d9fb17 MHS	415	* @param paramnum
31abe49f	416	* @return -1 if error
f6d9fb17 MHS	417	*/
	418	uint8_t param_getdec(const char line, int paramnum, uint8_t destination)
	419	{
31abe49f	420	uint8_t val = param_get8ex(line, paramnum, 255, 10);
31abe49f	421	if( (int8_t) val == -1) return 1;
f6d9fb17 MHS	422	(*destination) = val;
	423	return 0;
	424	}
	425	/**
	426	* @brief Checks if param is decimal
	427	* @param line
	428	* @param paramnum
	429	* @return
	430	*/
	431	uint8_t param_isdec(const char *line, int paramnum)
	432	{
	433	int bg, en;
	434	//TODO, check more thorougly
	435	if (!param_getptr(line, &bg, &en, paramnum)) return 1;
	436	// return strtoul(&line[bg], NULL, 10) & 0xff;
	437
	438	return 0;
	439	}
	440
f397b5cc M	441	uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base)
	442	{
	443	int bg, en;
	444
	445	if (!param_getptr(line, &bg, &en, paramnum))
6c6d1ac1	446	return strtoul(&line[bg], NULL, base) & 0xff;
f397b5cc M	447	else
	448	return deflt;
	449	}
	450
	451	uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base)
	452	{
	453	int bg, en;
	454
	455	if (!param_getptr(line, &bg, &en, paramnum))
6c6d1ac1	456	return strtoul(&line[bg], NULL, base);
f397b5cc M	457	else
	458	return deflt;
	459	}
	460
	461	uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base)
	462	{
	463	int bg, en;
	464
	465	if (!param_getptr(line, &bg, &en, paramnum))
6c6d1ac1	466	return strtoull(&line[bg], NULL, base);
f397b5cc M	467	else
f397b5cc M	468	return deflt;
f397b5cc M	469	}
	470
	471	int param_gethex(const char line, int paramnum, uint8_t data, int hexcnt)
	472	{
	473	int bg, en, temp, i;
	474
	475	if (hexcnt % 2)
	476	return 1;
	477
	478	if (param_getptr(line, &bg, &en, paramnum)) return 1;
	479
	480	if (en - bg + 1 != hexcnt)
	481	return 1;
	482
	483	for(i = 0; i < hexcnt; i += 2) {
3ded0f97	484	if (!(isxdigit((unsigned char)line[bg + i]) && isxdigit((unsigned char)line[bg + i + 1])) ) return 1;
f397b5cc M	485
	486	sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
	487	data[i / 2] = temp & 0xff;
	488	}
	489
	490	return 0;
	491	}
1a5a73ab	492	int param_gethex_ex(const char line, int paramnum, uint8_t data, int *hexcnt)
	493	{
	494	int bg, en, temp, i;
	495
	496	//if (hexcnt % 2)
	497	// return 1;
	498
	499	if (param_getptr(line, &bg, &en, paramnum)) return 1;
	500
	501	*hexcnt = en - bg + 1;
	502	if (*hexcnt % 2) //error if not complete hex bytes
	503	return 1;
aea4d766	504
1a5a73ab	505	for(i = 0; i < *hexcnt; i += 2) {
3ded0f97	506	if (!(isxdigit((unsigned char)line[bg + i]) && isxdigit((unsigned char)line[bg + i + 1])) ) return 1;
1a5a73ab	507
	508	sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
	509	data[i / 2] = temp & 0xff;
	510	}
	511
	512	return 0;
	513	}
8019540b	514
	515	int param_gethex_to_eol(const char line, int paramnum, uint8_t data, int maxdatalen, int *datalen) {
	516	int bg, en;
	517	uint32_t temp;
	518	char buf[5] = {0};
	519
	520	if (param_getptr(line, &bg, &en, paramnum)) return 1;
	521
	522	*datalen = 0;
	523
	524	int indx = bg;
	525	while (line[indx]) {
3c5fce2b OM	526	if (line[indx] == '\t' \|\| line[indx] == ' ') {
3c5fce2b OM	527	indx++;
8019540b	528	continue;
3c5fce2b	529	}
8019540b	530
3ded0f97	531	if (isxdigit((unsigned char)line[indx])) {
8019540b	532	buf[strlen(buf) + 1] = 0x00;
	533	buf[strlen(buf)] = line[indx];
	534	} else {
	535	// if we have symbols other than spaces and hex
	536	return 1;
	537	}
	538
	539	if (*datalen >= maxdatalen) {
	540	// if we dont have space in buffer and have symbols to translate
	541	return 2;
	542	}
	543
	544	if (strlen(buf) >= 2) {
	545	sscanf(buf, "%x", &temp);
	546	data[*datalen] = (uint8_t)(temp & 0xff);
	547	*buf = 0;
	548	(*datalen)++;
	549	}
	550
	551	indx++;
	552	}
	553
	554	if (strlen(buf) > 0)
	555	//error when not completed hex bytes
	556	return 3;
	557
	558	return 0;
	559	}
	560
874572d4	561	int param_getstr(const char line, int paramnum, char str, size_t buffersize)
aea4d766	562	{
	563	int bg, en;
	564
874572d4 WM	565	if (param_getptr(line, &bg, &en, paramnum)) {
	566	return 0;
	567	}
	568
	569	// Prevent out of bounds errors
	570	if (en - bg + 1 >= buffersize) {
c95affa8	571	printf("out of bounds error: want %d bytes have %zd bytes\n", en - bg + 1 + 1, buffersize);
874572d4 WM	572	return 0;
874572d4 WM	573	}
aea4d766	574
	575	memcpy(str, line + bg, en - bg + 1);
	576	str[en - bg + 1] = 0;
	577
	578	return en - bg + 1;
	579	}
79544b28	580
	581	/*
	582	The following methods comes from Rfidler sourcecode.
	583	https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
	584	*/
	585
	586	// convert hex to sequence of 0/1 bit values
	587	// returns number of bits converted
	588	int hextobinarray(char target, char source)
	589	{
	590	int length, i, count= 0;
874572d4	591	char* start = source;
79544b28	592	char x;
	593
	594	length = strlen(source);
	595	// process 4 bits (1 hex digit) at a time
	596	while(length--)
	597	{
	598	x= *(source++);
	599	// capitalize
	600	if (x >= 'a' && x <= 'f')
	601	x -= 32;
	602	// convert to numeric value
	603	if (x >= '0' && x <= '9')
	604	x -= '0';
	605	else if (x >= 'A' && x <= 'F')
	606	x -= 'A' - 10;
874572d4	607	else {
3ded0f97	608	printf("Discovered unknown character %c %d at idx %tu of %s\n", x, x, source - start, start);
79544b28	609	return 0;
874572d4	610	}
79544b28	611	// output
	612	for(i= 0 ; i < 4 ; ++i, ++count)
	613	*(target++)= (x >> (3 - i)) & 1;
	614	}
	615
	616	return count;
	617	}
	618
79544b28	619	// convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source)
79544b28	620	// return number of bits converted
1c4c0b06	621	int binarraytohex(char target,char source, int length)
79544b28	622	{
	623	unsigned char i, x;
	624	int j = length;
	625
	626	if(j % 4)
	627	return 0;
	628
	629	while(j)
	630	{
	631	for(i= x= 0 ; i < 4 ; ++i)
	632	x += ( source[i] << (3 - i));
d1869c33	633	sprintf(target,"%X", (unsigned int)x);
79544b28	634	++target;
	635	source += 4;
	636	j -= 4;
	637	}
	638	return length;
	639	}
	640
79544b28	641	// return parity bit required to match type
709665b5	642	uint8_t GetParity( uint8_t *bits, uint8_t type, int length)
79544b28	643	{
	644	int x;
	645
	646	for(x= 0 ; length > 0 ; --length)
	647	x += bits[length - 1];
	648	x %= 2;
	649
	650	return x ^ type;
	651	}
	652
	653	// add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
709665b5	654	void wiegand_add_parity(uint8_t target, uint8_t source, uint8_t length)
79544b28	655	{
	656	*(target++)= GetParity(source, EVEN, length / 2);
	657	memcpy(target, source, length);
	658	target += length;
	659	*(target)= GetParity(source + length / 2, ODD, length / 2);
	660	}
4973f23d	661
59f726c9	662	// xor two arrays together for len items. The dst array contains the new xored values.
4973f23d	663	void xor(unsigned char dst, unsigned char src, size_t len) {
	664	for( ; len > 0; len--,dst++,src++)
	665	dst ^= src;
	666	}
	667
c4c3af7c	668	// RotateLeft - Ultralight, Desfire, works on byte level
	669	// 00-01-02 >> 01-02-00
	670	void rol(uint8_t *data, const size_t len){
	671	uint8_t first = data[0];
	672	for (size_t i = 0; i < len-1; i++) {
	673	data[i] = data[i+1];
	674	}
	675	data[len-1] = first;
	676	}
d172c17c JC	677
	678
	679	// Replace unprintable characters with a dot in char buffer
	680	void clean_ascii(unsigned char *buf, size_t len) {
	681	for (size_t i = 0; i < len; i++) {
	682	if (!isprint(buf[i]))
	683	buf[i] = '.';
	684	}
	685	}
acf0582d	686
aa757f71 OM	687	// replace \r \n to \0
	688	void strcleanrn(char *buf, size_t len) {
	689	strcreplace(buf, len, '\n', '\0');
	690	strcreplace(buf, len, '\r', '\0');
	691	}
acf0582d	692
aa757f71 OM	693	// replace char in buffer
	694	void strcreplace(char *buf, size_t len, char from, char to) {
	695	for (size_t i = 0; i < len; i++) {
	696	if (buf[i] == from)
	697	buf[i] = to;
	698	}
	699	}
	700
	701	char strmcopy(char buf) {
	702	char * str = NULL;
	703	if ((str = (char*) malloc(strlen(buf) + 1)) != NULL) {
	704	memset(str, 0, strlen(buf) + 1);
	705	strcpy(str, buf);
	706	}
	707	return str;
	708	}
acf0582d	709
c48c4d78	710
	711	// determine number of logical CPU cores (use for multithreaded functions)
	712	extern int num_CPUs(void)
	713	{
	714	#if defined(_WIN32)
	715	#include <sysinfoapi.h>
	716	SYSTEM_INFO sysinfo;
	717	GetSystemInfo(&sysinfo);
	718	return sysinfo.dwNumberOfProcessors;
	719	#elif defined(__linux__) \|\| defined(__APPLE__)
	720	#include <unistd.h>
	721	return sysconf(_SC_NPROCESSORS_ONLN);
	722	#else
	723	return 1;
	724	#endif
	725	}
ec9c7112	726