]>
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> | |
19 | #include "data.h" | |
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> |
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; |
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 | } |
113 | ||
114 | // printing and converting functions | |
f397b5cc | 115 | |
534983d7 | 116 | void print_hex(const uint8_t * data, const size_t len) |
117 | { | |
118 | size_t i; | |
119 | ||
120 | for (i=0; i < len; i++) | |
121 | printf("%02x ", data[i]); | |
122 | ||
123 | printf("\n"); | |
124 | } | |
125 | ||
59f726c9 | 126 | void print_hex_break(const uint8_t *data, const size_t len, uint8_t breaks) { |
127 | ||
128 | int rownum = 0; | |
129 | printf("[%02d] | ", rownum); | |
130 | for (int i = 0; i < len; ++i) { | |
131 | ||
132 | printf("%02X ", data[i]); | |
133 | ||
134 | // check if a line break is needed | |
135 | if ( breaks > 0 && !((i+1) % breaks) && (i+1 < len) ) { | |
136 | ++rownum; | |
137 | printf("\n[%02d] | ", rownum); | |
138 | } | |
139 | } | |
140 | printf("\n"); | |
141 | } | |
142 | ||
4973f23d | 143 | char *sprint_hex(const uint8_t *data, const size_t len) { |
b915fda3 | 144 | |
145 | int maxLen = ( len > 1024/3) ? 1024/3 : len; | |
534983d7 | 146 | static char buf[1024]; |
c585a5cf | 147 | memset(buf, 0x00, 1024); |
4973f23d | 148 | char *tmp = buf; |
534983d7 | 149 | size_t i; |
150 | ||
b915fda3 | 151 | for (i=0; i < maxLen; ++i, tmp += 3) |
d1869c33 | 152 | sprintf(tmp, "%02x ", (unsigned int) data[i]); |
534983d7 | 153 | |
154 | return buf; | |
155 | } | |
f89c7050 | 156 | |
2767fc02 | 157 | char *sprint_bin_break(const uint8_t *data, const size_t len, const uint8_t breaks) { |
ace26dbd | 158 | // make sure we don't go beyond our char array memory |
7bc6fac3 | 159 | int max_len; |
160 | if (breaks==0) | |
161 | max_len = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len; | |
162 | else | |
163 | max_len = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks); | |
164 | ||
ace26dbd | 165 | static char buf[MAX_BIN_BREAK_LENGTH]; // 3072 + end of line characters if broken at 8 bits |
166 | //clear memory | |
167 | memset(buf, 0x00, sizeof(buf)); | |
2767fc02 | 168 | char *tmp = buf; |
79544b28 | 169 | |
ace26dbd | 170 | size_t in_index = 0; |
171 | // loop through the out_index to make sure we don't go too far | |
b97311b1 | 172 | for (size_t out_index=0; out_index < max_len; out_index++) { |
8ea57060 | 173 | // set character - (should be binary but verify it isn't more than 1 digit) |
174 | if (data[in_index]<10) | |
d1869c33 | 175 | sprintf(tmp++, "%u", (unsigned int) data[in_index]); |
6ca1477c | 176 | // check if a line break is needed and we have room to print it in our array |
b9957414 | 177 | if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 < max_len) ) { |
ace26dbd | 178 | // increment and print line break |
179 | out_index++; | |
2767fc02 | 180 | sprintf(tmp++, "%s","\n"); |
ace26dbd | 181 | } |
182 | in_index++; | |
2767fc02 | 183 | } |
79544b28 | 184 | |
185 | return buf; | |
186 | } | |
187 | ||
2767fc02 | 188 | char *sprint_bin(const uint8_t *data, const size_t len) { |
189 | return sprint_bin_break(data, len, 0); | |
190 | } | |
59f726c9 | 191 | |
192 | char *sprint_hex_ascii(const uint8_t *data, const size_t len) { | |
193 | static char buf[1024]; | |
194 | char *tmp = buf; | |
195 | memset(buf, 0x00, 1024); | |
bf824347 | 196 | size_t max_len = (len > 255) ? 255 : len; |
197 | // max 255 bytes * 3 + 2 characters = 767 in buffer | |
59f726c9 | 198 | sprintf(tmp, "%s| ", sprint_hex(data, max_len) ); |
199 | ||
200 | size_t i = 0; | |
201 | size_t pos = (max_len * 3)+2; | |
bf824347 | 202 | // add another 255 characters ascii = 1020 characters of buffer used |
203 | while(i < max_len) { | |
59f726c9 | 204 | char c = data[i]; |
205 | if ( (c < 32) || (c == 127)) | |
206 | c = '.'; | |
207 | sprintf(tmp+pos+i, "%c", c); | |
208 | ++i; | |
209 | } | |
210 | return buf; | |
211 | } | |
212 | ||
213 | char *sprint_ascii(const uint8_t *data, const size_t len) { | |
214 | static char buf[1024]; | |
215 | char *tmp = buf; | |
216 | memset(buf, 0x00, 1024); | |
217 | size_t max_len = (len > 1010) ? 1010 : len; | |
218 | size_t i = 0; | |
219 | while(i < max_len){ | |
220 | char c = data[i]; | |
221 | tmp[i] = ((c < 32) || (c == 127)) ? '.' : c; | |
222 | ++i; | |
223 | } | |
224 | return buf; | |
225 | } | |
226 | ||
f89c7050 M |
227 | void num_to_bytes(uint64_t n, size_t len, uint8_t* dest) |
228 | { | |
229 | while (len--) { | |
230 | dest[len] = (uint8_t) n; | |
231 | n >>= 8; | |
232 | } | |
233 | } | |
234 | ||
235 | uint64_t bytes_to_num(uint8_t* src, size_t len) | |
236 | { | |
237 | uint64_t num = 0; | |
238 | while (len--) | |
239 | { | |
240 | num = (num << 8) | (*src); | |
241 | src++; | |
242 | } | |
243 | return num; | |
244 | } | |
f397b5cc | 245 | |
709665b5 | 246 | void num_to_bytebits(uint64_t n, size_t len, uint8_t *dest) { |
247 | while (len--) { | |
248 | dest[len] = n & 1; | |
249 | n >>= 1; | |
250 | } | |
251 | } | |
252 | ||
59f726c9 | 253 | //least significant bit first |
254 | void num_to_bytebitsLSBF(uint64_t n, size_t len, uint8_t *dest) { | |
255 | for(int i = 0 ; i < len ; ++i) { | |
256 | dest[i] = n & 1; | |
257 | n >>= 1; | |
258 | } | |
259 | } | |
260 | ||
2d42ea1e | 261 | // Swap bit order on a uint32_t value. Can be limited by nrbits just use say 8bits reversal |
262 | // And clears the rest of the bits. | |
263 | uint32_t SwapBits(uint32_t value, int nrbits) { | |
264 | uint32_t newvalue = 0; | |
265 | for(int i = 0; i < nrbits; i++) { | |
266 | newvalue ^= ((value >> i) & 1) << (nrbits - 1 - i); | |
267 | } | |
268 | return newvalue; | |
269 | } | |
59f726c9 | 270 | |
f9848fd6 | 271 | // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp |
272 | // to | |
273 | // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii | |
274 | // up to 64 bytes or 512 bits | |
2b3af97d | 275 | uint8_t *SwapEndian64(const uint8_t *src, const size_t len, const uint8_t blockSize){ |
276 | static uint8_t buf[64]; | |
277 | memset(buf, 0x00, 64); | |
278 | uint8_t *tmp = buf; | |
279 | for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){ | |
280 | for (size_t i = 0; i < blockSize; i++){ | |
281 | tmp[i+(blockSize*block)] = src[(blockSize-1-i)+(blockSize*block)]; | |
f9848fd6 | 282 | } |
283 | } | |
2b3af97d | 284 | return tmp; |
f9848fd6 | 285 | } |
286 | ||
59f726c9 | 287 | // takes a uint8_t src array, for len items and reverses the byte order in blocksizes (8,16,32,64), |
288 | // returns: the dest array contains the reordered src array. | |
289 | void SwapEndian64ex(const uint8_t *src, const size_t len, const uint8_t blockSize, uint8_t *dest){ | |
290 | for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){ | |
291 | for (size_t i = 0; i < blockSize; i++){ | |
292 | dest[i+(blockSize*block)] = src[(blockSize-1-i)+(blockSize*block)]; | |
293 | } | |
294 | } | |
295 | } | |
296 | ||
79544b28 | 297 | //assumes little endian |
298 | char * printBits(size_t const size, void const * const ptr) | |
299 | { | |
300 | unsigned char *b = (unsigned char*) ptr; | |
301 | unsigned char byte; | |
302 | static char buf[1024]; | |
303 | char * tmp = buf; | |
304 | int i, j; | |
305 | ||
306 | for (i=size-1;i>=0;i--) | |
307 | { | |
308 | for (j=7;j>=0;j--) | |
309 | { | |
310 | byte = b[i] & (1<<j); | |
311 | byte >>= j; | |
d1869c33 | 312 | sprintf(tmp, "%u", (unsigned int)byte); |
79544b28 | 313 | tmp++; |
314 | } | |
315 | } | |
316 | return buf; | |
317 | } | |
318 | ||
9ca155ba M |
319 | // ------------------------------------------------------------------------- |
320 | // string parameters lib | |
321 | // ------------------------------------------------------------------------- | |
322 | ||
f397b5cc M |
323 | // ------------------------------------------------------------------------- |
324 | // line - param line | |
3a05a1e7 | 325 | // bg, en - symbol numbers in param line of beginning and ending parameter |
f397b5cc M |
326 | // paramnum - param number (from 0) |
327 | // ------------------------------------------------------------------------- | |
328 | int param_getptr(const char *line, int *bg, int *en, int paramnum) | |
329 | { | |
330 | int i; | |
331 | int len = strlen(line); | |
332 | ||
333 | *bg = 0; | |
334 | *en = 0; | |
335 | ||
336 | // skip spaces | |
337 | while (line[*bg] ==' ' || line[*bg]=='\t') (*bg)++; | |
338 | if (*bg >= len) { | |
339 | return 1; | |
340 | } | |
341 | ||
342 | for (i = 0; i < paramnum; i++) { | |
343 | while (line[*bg]!=' ' && line[*bg]!='\t' && line[*bg] != '\0') (*bg)++; | |
344 | while (line[*bg]==' ' || line[*bg]=='\t') (*bg)++; | |
345 | ||
346 | if (line[*bg] == '\0') return 1; | |
347 | } | |
348 | ||
349 | *en = *bg; | |
350 | while (line[*en] != ' ' && line[*en] != '\t' && line[*en] != '\0') (*en)++; | |
351 | ||
352 | (*en)--; | |
353 | ||
354 | return 0; | |
355 | } | |
356 | ||
31abe49f | 357 | |
3a05a1e7 OM |
358 | int param_getlength(const char *line, int paramnum) |
359 | { | |
360 | int bg, en; | |
361 | ||
362 | if (param_getptr(line, &bg, &en, paramnum)) return 0; | |
363 | ||
364 | return en - bg + 1; | |
365 | } | |
366 | ||
f397b5cc M |
367 | char param_getchar(const char *line, int paramnum) |
368 | { | |
369 | int bg, en; | |
370 | ||
371 | if (param_getptr(line, &bg, &en, paramnum)) return 0x00; | |
372 | ||
373 | return line[bg]; | |
374 | } | |
375 | ||
376 | uint8_t param_get8(const char *line, int paramnum) | |
377 | { | |
6ca1477c | 378 | return param_get8ex(line, paramnum, 0, 10); |
f397b5cc M |
379 | } |
380 | ||
f6d9fb17 | 381 | /** |
31abe49f | 382 | * @brief Reads a decimal integer (actually, 0-254, not 255) |
f6d9fb17 MHS |
383 | * @param line |
384 | * @param paramnum | |
31abe49f | 385 | * @return -1 if error |
f6d9fb17 MHS |
386 | */ |
387 | uint8_t param_getdec(const char *line, int paramnum, uint8_t *destination) | |
388 | { | |
31abe49f | 389 | uint8_t val = param_get8ex(line, paramnum, 255, 10); |
31abe49f | 390 | if( (int8_t) val == -1) return 1; |
f6d9fb17 MHS |
391 | (*destination) = val; |
392 | return 0; | |
393 | } | |
394 | /** | |
395 | * @brief Checks if param is decimal | |
396 | * @param line | |
397 | * @param paramnum | |
398 | * @return | |
399 | */ | |
400 | uint8_t param_isdec(const char *line, int paramnum) | |
401 | { | |
402 | int bg, en; | |
403 | //TODO, check more thorougly | |
404 | if (!param_getptr(line, &bg, &en, paramnum)) return 1; | |
405 | // return strtoul(&line[bg], NULL, 10) & 0xff; | |
406 | ||
407 | return 0; | |
408 | } | |
409 | ||
f397b5cc M |
410 | uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base) |
411 | { | |
412 | int bg, en; | |
413 | ||
414 | if (!param_getptr(line, &bg, &en, paramnum)) | |
6c6d1ac1 | 415 | return strtoul(&line[bg], NULL, base) & 0xff; |
f397b5cc M |
416 | else |
417 | return deflt; | |
418 | } | |
419 | ||
420 | uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base) | |
421 | { | |
422 | int bg, en; | |
423 | ||
424 | if (!param_getptr(line, &bg, &en, paramnum)) | |
6c6d1ac1 | 425 | return strtoul(&line[bg], NULL, base); |
f397b5cc M |
426 | else |
427 | return deflt; | |
428 | } | |
429 | ||
430 | uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base) | |
431 | { | |
432 | int bg, en; | |
433 | ||
434 | if (!param_getptr(line, &bg, &en, paramnum)) | |
6c6d1ac1 | 435 | return strtoull(&line[bg], NULL, base); |
f397b5cc M |
436 | else |
437 | return deflt; | |
f397b5cc M |
438 | } |
439 | ||
440 | int param_gethex(const char *line, int paramnum, uint8_t * data, int hexcnt) | |
441 | { | |
442 | int bg, en, temp, i; | |
443 | ||
444 | if (hexcnt % 2) | |
445 | return 1; | |
446 | ||
447 | if (param_getptr(line, &bg, &en, paramnum)) return 1; | |
448 | ||
449 | if (en - bg + 1 != hexcnt) | |
450 | return 1; | |
451 | ||
452 | for(i = 0; i < hexcnt; i += 2) { | |
453 | if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1])) ) return 1; | |
454 | ||
455 | sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp); | |
456 | data[i / 2] = temp & 0xff; | |
457 | } | |
458 | ||
459 | return 0; | |
460 | } | |
1a5a73ab | 461 | int param_gethex_ex(const char *line, int paramnum, uint8_t * data, int *hexcnt) |
462 | { | |
463 | int bg, en, temp, i; | |
464 | ||
465 | //if (hexcnt % 2) | |
466 | // return 1; | |
467 | ||
468 | if (param_getptr(line, &bg, &en, paramnum)) return 1; | |
469 | ||
470 | *hexcnt = en - bg + 1; | |
471 | if (*hexcnt % 2) //error if not complete hex bytes | |
472 | return 1; | |
aea4d766 | 473 | |
1a5a73ab | 474 | for(i = 0; i < *hexcnt; i += 2) { |
475 | if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1])) ) return 1; | |
476 | ||
477 | sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp); | |
478 | data[i / 2] = temp & 0xff; | |
479 | } | |
480 | ||
481 | return 0; | |
482 | } | |
aea4d766 | 483 | int param_getstr(const char *line, int paramnum, char * str) |
484 | { | |
485 | int bg, en; | |
486 | ||
487 | if (param_getptr(line, &bg, &en, paramnum)) return 0; | |
488 | ||
489 | memcpy(str, line + bg, en - bg + 1); | |
490 | str[en - bg + 1] = 0; | |
491 | ||
492 | return en - bg + 1; | |
493 | } | |
79544b28 | 494 | |
495 | /* | |
496 | The following methods comes from Rfidler sourcecode. | |
497 | https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/ | |
498 | */ | |
499 | ||
500 | // convert hex to sequence of 0/1 bit values | |
501 | // returns number of bits converted | |
502 | int hextobinarray(char *target, char *source) | |
503 | { | |
504 | int length, i, count= 0; | |
505 | char x; | |
506 | ||
507 | length = strlen(source); | |
508 | // process 4 bits (1 hex digit) at a time | |
509 | while(length--) | |
510 | { | |
511 | x= *(source++); | |
512 | // capitalize | |
513 | if (x >= 'a' && x <= 'f') | |
514 | x -= 32; | |
515 | // convert to numeric value | |
516 | if (x >= '0' && x <= '9') | |
517 | x -= '0'; | |
518 | else if (x >= 'A' && x <= 'F') | |
519 | x -= 'A' - 10; | |
520 | else | |
521 | return 0; | |
522 | // output | |
523 | for(i= 0 ; i < 4 ; ++i, ++count) | |
524 | *(target++)= (x >> (3 - i)) & 1; | |
525 | } | |
526 | ||
527 | return count; | |
528 | } | |
529 | ||
530 | // convert hex to human readable binary string | |
531 | int hextobinstring(char *target, char *source) | |
532 | { | |
533 | int length; | |
534 | ||
535 | if(!(length= hextobinarray(target, source))) | |
536 | return 0; | |
537 | binarraytobinstring(target, target, length); | |
538 | return length; | |
539 | } | |
540 | ||
541 | // convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source) | |
542 | // return number of bits converted | |
1c4c0b06 | 543 | int binarraytohex(char *target,char *source, int length) |
79544b28 | 544 | { |
545 | unsigned char i, x; | |
546 | int j = length; | |
547 | ||
548 | if(j % 4) | |
549 | return 0; | |
550 | ||
551 | while(j) | |
552 | { | |
553 | for(i= x= 0 ; i < 4 ; ++i) | |
554 | x += ( source[i] << (3 - i)); | |
d1869c33 | 555 | sprintf(target,"%X", (unsigned int)x); |
79544b28 | 556 | ++target; |
557 | source += 4; | |
558 | j -= 4; | |
559 | } | |
560 | return length; | |
561 | } | |
562 | ||
563 | // convert binary array to human readable binary | |
564 | void binarraytobinstring(char *target, char *source, int length) | |
565 | { | |
566 | int i; | |
567 | ||
568 | for(i= 0 ; i < length ; ++i) | |
569 | *(target++)= *(source++) + '0'; | |
570 | *target= '\0'; | |
571 | } | |
572 | ||
573 | // return parity bit required to match type | |
709665b5 | 574 | uint8_t GetParity( uint8_t *bits, uint8_t type, int length) |
79544b28 | 575 | { |
576 | int x; | |
577 | ||
578 | for(x= 0 ; length > 0 ; --length) | |
579 | x += bits[length - 1]; | |
580 | x %= 2; | |
581 | ||
582 | return x ^ type; | |
583 | } | |
584 | ||
585 | // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half | |
709665b5 | 586 | void wiegand_add_parity(uint8_t *target, uint8_t *source, uint8_t length) |
79544b28 | 587 | { |
588 | *(target++)= GetParity(source, EVEN, length / 2); | |
589 | memcpy(target, source, length); | |
590 | target += length; | |
591 | *(target)= GetParity(source + length / 2, ODD, length / 2); | |
592 | } | |
4973f23d | 593 | |
59f726c9 | 594 | // xor two arrays together for len items. The dst array contains the new xored values. |
4973f23d | 595 | void xor(unsigned char *dst, unsigned char *src, size_t len) { |
596 | for( ; len > 0; len--,dst++,src++) | |
597 | *dst ^= *src; | |
598 | } | |
599 | ||
600 | int32_t le24toh (uint8_t data[3]) { | |
601 | return (data[2] << 16) | (data[1] << 8) | data[0]; | |
602 | } | |
c872d8c1 | 603 | uint32_t le32toh (uint8_t *data) { |
604 | return (uint32_t)( (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0]); | |
605 | } | |
c4c3af7c | 606 | |
607 | // RotateLeft - Ultralight, Desfire, works on byte level | |
608 | // 00-01-02 >> 01-02-00 | |
609 | void rol(uint8_t *data, const size_t len){ | |
610 | uint8_t first = data[0]; | |
611 | for (size_t i = 0; i < len-1; i++) { | |
612 | data[i] = data[i+1]; | |
613 | } | |
614 | data[len-1] = first; | |
615 | } | |
d172c17c JC |
616 | |
617 | ||
618 | // Replace unprintable characters with a dot in char buffer | |
619 | void clean_ascii(unsigned char *buf, size_t len) { | |
620 | for (size_t i = 0; i < len; i++) { | |
621 | if (!isprint(buf[i])) | |
622 | buf[i] = '.'; | |
623 | } | |
624 | } | |
acf0582d | 625 | |
aa757f71 OM |
626 | // replace \r \n to \0 |
627 | void strcleanrn(char *buf, size_t len) { | |
628 | strcreplace(buf, len, '\n', '\0'); | |
629 | strcreplace(buf, len, '\r', '\0'); | |
630 | } | |
acf0582d | 631 | |
aa757f71 OM |
632 | // replace char in buffer |
633 | void strcreplace(char *buf, size_t len, char from, char to) { | |
634 | for (size_t i = 0; i < len; i++) { | |
635 | if (buf[i] == from) | |
636 | buf[i] = to; | |
637 | } | |
638 | } | |
639 | ||
640 | char *strmcopy(char *buf) { | |
641 | char * str = NULL; | |
642 | if ((str = (char*) malloc(strlen(buf) + 1)) != NULL) { | |
643 | memset(str, 0, strlen(buf) + 1); | |
644 | strcpy(str, buf); | |
645 | } | |
646 | return str; | |
647 | } | |
acf0582d | 648 | |
c48c4d78 | 649 | |
650 | // determine number of logical CPU cores (use for multithreaded functions) | |
651 | extern int num_CPUs(void) | |
652 | { | |
653 | #if defined(_WIN32) | |
654 | #include <sysinfoapi.h> | |
655 | SYSTEM_INFO sysinfo; | |
656 | GetSystemInfo(&sysinfo); | |
657 | return sysinfo.dwNumberOfProcessors; | |
658 | #elif defined(__linux__) || defined(__APPLE__) | |
659 | #include <unistd.h> | |
660 | return sysconf(_SC_NPROCESSORS_ONLN); | |
661 | #else | |
662 | return 1; | |
663 | #endif | |
664 | } | |
ec9c7112 | 665 |