]>
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" | |
ace26dbd | 12 | #define MAX_BIN_BREAK_LENGTH (3072+384+1) |
534983d7 | 13 | |
cb64309e | 14 | #ifndef _WIN32 |
873014de M |
15 | #include <termios.h> |
16 | #include <sys/ioctl.h> | |
79544b28 | 17 | |
ace26dbd | 18 | |
f397b5cc M |
19 | int ukbhit(void) |
20 | { | |
21 | int cnt = 0; | |
22 | int error; | |
23 | static struct termios Otty, Ntty; | |
24 | ||
25 | ||
26 | tcgetattr( 0, &Otty); | |
27 | Ntty = Otty; | |
28 | ||
29 | Ntty.c_iflag = 0; /* input mode */ | |
30 | Ntty.c_oflag = 0; /* output mode */ | |
31 | Ntty.c_lflag &= ~ICANON; /* raw mode */ | |
32 | Ntty.c_cc[VMIN] = CMIN; /* minimum time to wait */ | |
33 | Ntty.c_cc[VTIME] = CTIME; /* minimum characters to wait for */ | |
34 | ||
35 | if (0 == (error = tcsetattr(0, TCSANOW, &Ntty))) { | |
36 | error += ioctl(0, FIONREAD, &cnt); | |
37 | error += tcsetattr(0, TCSANOW, &Otty); | |
38 | } | |
39 | ||
40 | return ( error == 0 ? cnt : -1 ); | |
41 | } | |
42 | ||
43 | #else | |
44 | #include <conio.h> | |
45 | int ukbhit(void) { | |
46 | return kbhit(); | |
47 | } | |
48 | #endif | |
49 | ||
55acbb2a | 50 | // log files functions |
b915fda3 | 51 | void AddLogLine(char *file, char *extData, char *c) { |
55acbb2a | 52 | FILE *fLog = NULL; |
b915fda3 | 53 | char filename[FILE_PATH_SIZE] = {0x00}; |
54 | int len = 0; | |
55 | ||
56 | len = strlen(file); | |
57 | if (len > FILE_PATH_SIZE) len = FILE_PATH_SIZE; | |
58 | memcpy(filename, file, len); | |
59 | ||
60 | fLog = fopen(filename, "a"); | |
55acbb2a | 61 | if (!fLog) { |
b915fda3 | 62 | printf("Could not append log file %s", filename); |
55acbb2a M |
63 | return; |
64 | } | |
65 | ||
66 | fprintf(fLog, "%s", extData); | |
67 | fprintf(fLog, "%s\n", c); | |
68 | fclose(fLog); | |
69 | } | |
70 | ||
71 | void AddLogHex(char *fileName, char *extData, const uint8_t * data, const size_t len){ | |
72 | AddLogLine(fileName, extData, sprint_hex(data, len)); | |
73 | } | |
74 | ||
75 | void AddLogUint64(char *fileName, char *extData, const uint64_t data) { | |
76 | char buf[100] = {0}; | |
77 | sprintf(buf, "%x%x", (unsigned int)((data & 0xFFFFFFFF00000000) >> 32), (unsigned int)(data & 0xFFFFFFFF)); | |
78 | AddLogLine(fileName, extData, buf); | |
79 | } | |
80 | ||
81 | void AddLogCurrentDT(char *fileName) { | |
82 | char buff[20]; | |
83 | struct tm *curTime; | |
84 | ||
85 | time_t now = time(0); | |
86 | curTime = gmtime(&now); | |
87 | ||
88 | strftime (buff, sizeof(buff), "%Y-%m-%d %H:%M:%S", curTime); | |
89 | AddLogLine(fileName, "\nanticollision: ", buff); | |
90 | } | |
91 | ||
e0c635d1 | 92 | void FillFileNameByUID(char *fileName, uint8_t * uid, char *ext, int byteCount) { |
55acbb2a M |
93 | char * fnameptr = fileName; |
94 | memset(fileName, 0x00, 200); | |
95 | ||
e0c635d1 | 96 | for (int j = 0; j < byteCount; j++, fnameptr += 2) |
55acbb2a M |
97 | sprintf(fnameptr, "%02x", uid[j]); |
98 | sprintf(fnameptr, "%s", ext); | |
55acbb2a M |
99 | } |
100 | ||
101 | // printing and converting functions | |
f397b5cc | 102 | |
534983d7 | 103 | void print_hex(const uint8_t * data, const size_t len) |
104 | { | |
105 | size_t i; | |
106 | ||
107 | for (i=0; i < len; i++) | |
108 | printf("%02x ", data[i]); | |
109 | ||
110 | printf("\n"); | |
111 | } | |
112 | ||
4973f23d | 113 | char *sprint_hex(const uint8_t *data, const size_t len) { |
b915fda3 | 114 | |
115 | int maxLen = ( len > 1024/3) ? 1024/3 : len; | |
534983d7 | 116 | static char buf[1024]; |
c585a5cf | 117 | memset(buf, 0x00, 1024); |
4973f23d | 118 | char *tmp = buf; |
534983d7 | 119 | size_t i; |
120 | ||
b915fda3 | 121 | for (i=0; i < maxLen; ++i, tmp += 3) |
534983d7 | 122 | sprintf(tmp, "%02x ", data[i]); |
123 | ||
124 | return buf; | |
125 | } | |
f89c7050 | 126 | |
2767fc02 | 127 | char *sprint_bin_break(const uint8_t *data, const size_t len, const uint8_t breaks) { |
ace26dbd | 128 | // make sure we don't go beyond our char array memory |
7bc6fac3 | 129 | int max_len; |
130 | if (breaks==0) | |
131 | max_len = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len; | |
132 | else | |
133 | max_len = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks); | |
134 | ||
ace26dbd | 135 | static char buf[MAX_BIN_BREAK_LENGTH]; // 3072 + end of line characters if broken at 8 bits |
136 | //clear memory | |
137 | memset(buf, 0x00, sizeof(buf)); | |
2767fc02 | 138 | char *tmp = buf; |
79544b28 | 139 | |
ace26dbd | 140 | size_t in_index = 0; |
141 | // loop through the out_index to make sure we don't go too far | |
142 | for (size_t out_index=0; out_index < max_len; out_index++) { | |
143 | // set character | |
144 | sprintf(tmp++, "%u", data[in_index]); | |
6ca1477c | 145 | // check if a line break is needed and we have room to print it in our array |
ace26dbd | 146 | if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 != max_len) ) { |
147 | // increment and print line break | |
148 | out_index++; | |
2767fc02 | 149 | sprintf(tmp++, "%s","\n"); |
ace26dbd | 150 | } |
151 | in_index++; | |
2767fc02 | 152 | } |
79544b28 | 153 | |
154 | return buf; | |
155 | } | |
156 | ||
2767fc02 | 157 | char *sprint_bin(const uint8_t *data, const size_t len) { |
158 | return sprint_bin_break(data, len, 0); | |
159 | } | |
f89c7050 M |
160 | void num_to_bytes(uint64_t n, size_t len, uint8_t* dest) |
161 | { | |
162 | while (len--) { | |
163 | dest[len] = (uint8_t) n; | |
164 | n >>= 8; | |
165 | } | |
166 | } | |
167 | ||
168 | uint64_t bytes_to_num(uint8_t* src, size_t len) | |
169 | { | |
170 | uint64_t num = 0; | |
171 | while (len--) | |
172 | { | |
173 | num = (num << 8) | (*src); | |
174 | src++; | |
175 | } | |
176 | return num; | |
177 | } | |
f397b5cc | 178 | |
709665b5 | 179 | void num_to_bytebits(uint64_t n, size_t len, uint8_t *dest) { |
180 | while (len--) { | |
181 | dest[len] = n & 1; | |
182 | n >>= 1; | |
183 | } | |
184 | } | |
185 | ||
f9848fd6 | 186 | // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp |
187 | // to | |
188 | // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii | |
189 | // up to 64 bytes or 512 bits | |
2b3af97d | 190 | uint8_t *SwapEndian64(const uint8_t *src, const size_t len, const uint8_t blockSize){ |
191 | static uint8_t buf[64]; | |
192 | memset(buf, 0x00, 64); | |
193 | uint8_t *tmp = buf; | |
194 | for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){ | |
195 | for (size_t i = 0; i < blockSize; i++){ | |
196 | tmp[i+(blockSize*block)] = src[(blockSize-1-i)+(blockSize*block)]; | |
f9848fd6 | 197 | } |
198 | } | |
2b3af97d | 199 | return tmp; |
f9848fd6 | 200 | } |
201 | ||
79544b28 | 202 | //assumes little endian |
203 | char * printBits(size_t const size, void const * const ptr) | |
204 | { | |
205 | unsigned char *b = (unsigned char*) ptr; | |
206 | unsigned char byte; | |
207 | static char buf[1024]; | |
208 | char * tmp = buf; | |
209 | int i, j; | |
210 | ||
211 | for (i=size-1;i>=0;i--) | |
212 | { | |
213 | for (j=7;j>=0;j--) | |
214 | { | |
215 | byte = b[i] & (1<<j); | |
216 | byte >>= j; | |
217 | sprintf(tmp, "%u", byte); | |
218 | tmp++; | |
219 | } | |
220 | } | |
221 | return buf; | |
222 | } | |
223 | ||
9ca155ba M |
224 | // ------------------------------------------------------------------------- |
225 | // string parameters lib | |
226 | // ------------------------------------------------------------------------- | |
227 | ||
f397b5cc M |
228 | // ------------------------------------------------------------------------- |
229 | // line - param line | |
230 | // bg, en - symbol numbers in param line of beginning an ending parameter | |
231 | // paramnum - param number (from 0) | |
232 | // ------------------------------------------------------------------------- | |
233 | int param_getptr(const char *line, int *bg, int *en, int paramnum) | |
234 | { | |
235 | int i; | |
236 | int len = strlen(line); | |
237 | ||
238 | *bg = 0; | |
239 | *en = 0; | |
240 | ||
241 | // skip spaces | |
242 | while (line[*bg] ==' ' || line[*bg]=='\t') (*bg)++; | |
243 | if (*bg >= len) { | |
244 | return 1; | |
245 | } | |
246 | ||
247 | for (i = 0; i < paramnum; i++) { | |
248 | while (line[*bg]!=' ' && line[*bg]!='\t' && line[*bg] != '\0') (*bg)++; | |
249 | while (line[*bg]==' ' || line[*bg]=='\t') (*bg)++; | |
250 | ||
251 | if (line[*bg] == '\0') return 1; | |
252 | } | |
253 | ||
254 | *en = *bg; | |
255 | while (line[*en] != ' ' && line[*en] != '\t' && line[*en] != '\0') (*en)++; | |
256 | ||
257 | (*en)--; | |
258 | ||
259 | return 0; | |
260 | } | |
261 | ||
31abe49f | 262 | |
f397b5cc M |
263 | char param_getchar(const char *line, int paramnum) |
264 | { | |
265 | int bg, en; | |
266 | ||
267 | if (param_getptr(line, &bg, &en, paramnum)) return 0x00; | |
268 | ||
269 | return line[bg]; | |
270 | } | |
271 | ||
272 | uint8_t param_get8(const char *line, int paramnum) | |
273 | { | |
6ca1477c | 274 | return param_get8ex(line, paramnum, 0, 10); |
f397b5cc M |
275 | } |
276 | ||
f6d9fb17 | 277 | /** |
31abe49f | 278 | * @brief Reads a decimal integer (actually, 0-254, not 255) |
f6d9fb17 MHS |
279 | * @param line |
280 | * @param paramnum | |
31abe49f | 281 | * @return -1 if error |
f6d9fb17 MHS |
282 | */ |
283 | uint8_t param_getdec(const char *line, int paramnum, uint8_t *destination) | |
284 | { | |
31abe49f | 285 | uint8_t val = param_get8ex(line, paramnum, 255, 10); |
31abe49f | 286 | if( (int8_t) val == -1) return 1; |
f6d9fb17 MHS |
287 | (*destination) = val; |
288 | return 0; | |
289 | } | |
290 | /** | |
291 | * @brief Checks if param is decimal | |
292 | * @param line | |
293 | * @param paramnum | |
294 | * @return | |
295 | */ | |
296 | uint8_t param_isdec(const char *line, int paramnum) | |
297 | { | |
298 | int bg, en; | |
299 | //TODO, check more thorougly | |
300 | if (!param_getptr(line, &bg, &en, paramnum)) return 1; | |
301 | // return strtoul(&line[bg], NULL, 10) & 0xff; | |
302 | ||
303 | return 0; | |
304 | } | |
305 | ||
f397b5cc M |
306 | uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base) |
307 | { | |
308 | int bg, en; | |
309 | ||
310 | if (!param_getptr(line, &bg, &en, paramnum)) | |
6c6d1ac1 | 311 | return strtoul(&line[bg], NULL, base) & 0xff; |
f397b5cc M |
312 | else |
313 | return deflt; | |
314 | } | |
315 | ||
316 | uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base) | |
317 | { | |
318 | int bg, en; | |
319 | ||
320 | if (!param_getptr(line, &bg, &en, paramnum)) | |
6c6d1ac1 | 321 | return strtoul(&line[bg], NULL, base); |
f397b5cc M |
322 | else |
323 | return deflt; | |
324 | } | |
325 | ||
326 | uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base) | |
327 | { | |
328 | int bg, en; | |
329 | ||
330 | if (!param_getptr(line, &bg, &en, paramnum)) | |
6c6d1ac1 | 331 | return strtoull(&line[bg], NULL, base); |
f397b5cc M |
332 | else |
333 | return deflt; | |
334 | ||
335 | return 0; | |
336 | } | |
337 | ||
338 | int param_gethex(const char *line, int paramnum, uint8_t * data, int hexcnt) | |
339 | { | |
340 | int bg, en, temp, i; | |
341 | ||
342 | if (hexcnt % 2) | |
343 | return 1; | |
344 | ||
345 | if (param_getptr(line, &bg, &en, paramnum)) return 1; | |
346 | ||
347 | if (en - bg + 1 != hexcnt) | |
348 | return 1; | |
349 | ||
350 | for(i = 0; i < hexcnt; i += 2) { | |
351 | if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1])) ) return 1; | |
352 | ||
353 | sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp); | |
354 | data[i / 2] = temp & 0xff; | |
355 | } | |
356 | ||
357 | return 0; | |
358 | } | |
1a5a73ab | 359 | int param_gethex_ex(const char *line, int paramnum, uint8_t * data, int *hexcnt) |
360 | { | |
361 | int bg, en, temp, i; | |
362 | ||
363 | //if (hexcnt % 2) | |
364 | // return 1; | |
365 | ||
366 | if (param_getptr(line, &bg, &en, paramnum)) return 1; | |
367 | ||
368 | *hexcnt = en - bg + 1; | |
369 | if (*hexcnt % 2) //error if not complete hex bytes | |
370 | return 1; | |
aea4d766 | 371 | |
1a5a73ab | 372 | for(i = 0; i < *hexcnt; i += 2) { |
373 | if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1])) ) return 1; | |
374 | ||
375 | sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp); | |
376 | data[i / 2] = temp & 0xff; | |
377 | } | |
378 | ||
379 | return 0; | |
380 | } | |
aea4d766 | 381 | int param_getstr(const char *line, int paramnum, char * str) |
382 | { | |
383 | int bg, en; | |
384 | ||
385 | if (param_getptr(line, &bg, &en, paramnum)) return 0; | |
386 | ||
387 | memcpy(str, line + bg, en - bg + 1); | |
388 | str[en - bg + 1] = 0; | |
389 | ||
390 | return en - bg + 1; | |
391 | } | |
79544b28 | 392 | |
393 | /* | |
394 | The following methods comes from Rfidler sourcecode. | |
395 | https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/ | |
396 | */ | |
397 | ||
398 | // convert hex to sequence of 0/1 bit values | |
399 | // returns number of bits converted | |
400 | int hextobinarray(char *target, char *source) | |
401 | { | |
402 | int length, i, count= 0; | |
403 | char x; | |
404 | ||
405 | length = strlen(source); | |
406 | // process 4 bits (1 hex digit) at a time | |
407 | while(length--) | |
408 | { | |
409 | x= *(source++); | |
410 | // capitalize | |
411 | if (x >= 'a' && x <= 'f') | |
412 | x -= 32; | |
413 | // convert to numeric value | |
414 | if (x >= '0' && x <= '9') | |
415 | x -= '0'; | |
416 | else if (x >= 'A' && x <= 'F') | |
417 | x -= 'A' - 10; | |
418 | else | |
419 | return 0; | |
420 | // output | |
421 | for(i= 0 ; i < 4 ; ++i, ++count) | |
422 | *(target++)= (x >> (3 - i)) & 1; | |
423 | } | |
424 | ||
425 | return count; | |
426 | } | |
427 | ||
428 | // convert hex to human readable binary string | |
429 | int hextobinstring(char *target, char *source) | |
430 | { | |
431 | int length; | |
432 | ||
433 | if(!(length= hextobinarray(target, source))) | |
434 | return 0; | |
435 | binarraytobinstring(target, target, length); | |
436 | return length; | |
437 | } | |
438 | ||
439 | // convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source) | |
440 | // return number of bits converted | |
1c4c0b06 | 441 | int binarraytohex(char *target,char *source, int length) |
79544b28 | 442 | { |
443 | unsigned char i, x; | |
444 | int j = length; | |
445 | ||
446 | if(j % 4) | |
447 | return 0; | |
448 | ||
449 | while(j) | |
450 | { | |
451 | for(i= x= 0 ; i < 4 ; ++i) | |
452 | x += ( source[i] << (3 - i)); | |
453 | sprintf(target,"%X", x); | |
454 | ++target; | |
455 | source += 4; | |
456 | j -= 4; | |
457 | } | |
458 | return length; | |
459 | } | |
460 | ||
461 | // convert binary array to human readable binary | |
462 | void binarraytobinstring(char *target, char *source, int length) | |
463 | { | |
464 | int i; | |
465 | ||
466 | for(i= 0 ; i < length ; ++i) | |
467 | *(target++)= *(source++) + '0'; | |
468 | *target= '\0'; | |
469 | } | |
470 | ||
471 | // return parity bit required to match type | |
709665b5 | 472 | uint8_t GetParity( uint8_t *bits, uint8_t type, int length) |
79544b28 | 473 | { |
474 | int x; | |
475 | ||
476 | for(x= 0 ; length > 0 ; --length) | |
477 | x += bits[length - 1]; | |
478 | x %= 2; | |
479 | ||
480 | return x ^ type; | |
481 | } | |
482 | ||
483 | // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half | |
709665b5 | 484 | void wiegand_add_parity(uint8_t *target, uint8_t *source, uint8_t length) |
79544b28 | 485 | { |
486 | *(target++)= GetParity(source, EVEN, length / 2); | |
487 | memcpy(target, source, length); | |
488 | target += length; | |
489 | *(target)= GetParity(source + length / 2, ODD, length / 2); | |
490 | } | |
4973f23d | 491 | |
492 | void xor(unsigned char *dst, unsigned char *src, size_t len) { | |
493 | for( ; len > 0; len--,dst++,src++) | |
494 | *dst ^= *src; | |
495 | } | |
496 | ||
497 | int32_t le24toh (uint8_t data[3]) { | |
498 | return (data[2] << 16) | (data[1] << 8) | data[0]; | |
499 | } |