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