]>
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" | |
bf32dd92 | 12 | #define MAX_BIN_BREAK_LENGTH (3072+384+1) |
534983d7 | 13 | |
cb64309e | 14 | #ifndef _WIN32 |
faad338e MF |
15 | #include <sys/ttydefaults.h> |
16 | ||
3e134b4c | 17 | int ukbhit(void) { |
f397b5cc M |
18 | int cnt = 0; |
19 | int error; | |
20 | static struct termios Otty, Ntty; | |
21 | ||
d04b71c1 | 22 | if ( tcgetattr( 0, &Otty) == -1) return -1; |
f397b5cc M |
23 | Ntty = Otty; |
24 | ||
d04b71c1 | 25 | Ntty.c_iflag = 0; /* input mode */ |
26 | Ntty.c_oflag = 0; /* output mode */ | |
27 | Ntty.c_lflag &= ~ICANON; /* raw mode */ | |
28 | Ntty.c_cc[VMIN] = CMIN; /* minimum time to wait */ | |
29 | Ntty.c_cc[VTIME] = CTIME; /* minimum characters to wait for */ | |
f397b5cc M |
30 | |
31 | if (0 == (error = tcsetattr(0, TCSANOW, &Ntty))) { | |
32 | error += ioctl(0, FIONREAD, &cnt); | |
33 | error += tcsetattr(0, TCSANOW, &Otty); | |
34 | } | |
35 | ||
36 | return ( error == 0 ? cnt : -1 ); | |
37 | } | |
38 | ||
39 | #else | |
f397b5cc M |
40 | int ukbhit(void) { |
41 | return kbhit(); | |
42 | } | |
43 | #endif | |
44 | ||
55acbb2a | 45 | // log files functions |
b915fda3 | 46 | void AddLogLine(char *file, char *extData, char *c) { |
c805748f | 47 | FILE *f = NULL; |
b915fda3 | 48 | char filename[FILE_PATH_SIZE] = {0x00}; |
49 | int len = 0; | |
50 | ||
51 | len = strlen(file); | |
52 | if (len > FILE_PATH_SIZE) len = FILE_PATH_SIZE; | |
53 | memcpy(filename, file, len); | |
54 | ||
c805748f | 55 | f = fopen(filename, "a"); |
56 | if (!f) { | |
b915fda3 | 57 | printf("Could not append log file %s", filename); |
55acbb2a M |
58 | return; |
59 | } | |
60 | ||
c805748f | 61 | fprintf(f, "%s", extData); |
62 | fprintf(f, "%s\n", c); | |
63 | fflush(f); | |
2dcf60f3 | 64 | if (f) { |
65 | fclose(f); | |
66 | f = NULL; | |
67 | } | |
55acbb2a M |
68 | } |
69 | ||
70 | void AddLogHex(char *fileName, char *extData, const uint8_t * data, const size_t len){ | |
71 | AddLogLine(fileName, extData, sprint_hex(data, len)); | |
72 | } | |
73 | ||
74 | void AddLogUint64(char *fileName, char *extData, const uint64_t data) { | |
c805748f | 75 | char buf[20] = {0}; |
76 | memset(buf, 0x00, sizeof(buf)); | |
77 | //sprintf(buf, "%X%X", (unsigned int)((data & 0xFFFFFFFF00000000) >> 32), (unsigned int)(data & 0xFFFFFFFF)); | |
78 | sprintf(buf, "%012"llx"", data); | |
55acbb2a M |
79 | AddLogLine(fileName, extData, buf); |
80 | } | |
81 | ||
82 | void AddLogCurrentDT(char *fileName) { | |
c805748f | 83 | char buf[20]; |
84 | memset(buf, 0x00, sizeof(buf)); | |
55acbb2a | 85 | struct tm *curTime; |
55acbb2a M |
86 | time_t now = time(0); |
87 | curTime = gmtime(&now); | |
c805748f | 88 | strftime (buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", curTime); |
89 | AddLogLine(fileName, "\nanticollision: ", buf); | |
55acbb2a M |
90 | } |
91 | ||
c805748f | 92 | void FillFileNameByUID(char *fileName, uint8_t *uid, char *ext, int byteCount) { |
93 | if ( fileName == NULL || uid == NULL || ext == NULL ){ | |
94 | printf("error: parameter is NULL\n"); | |
95 | return; | |
96 | } | |
55acbb2a | 97 | char * fnameptr = fileName; |
c805748f | 98 | memset(fileName, 0x00, FILE_PATH_SIZE); |
55acbb2a | 99 | |
e0c635d1 | 100 | for (int j = 0; j < byteCount; j++, fnameptr += 2) |
c805748f | 101 | sprintf(fnameptr, "%02X", uid[j]); |
55acbb2a | 102 | sprintf(fnameptr, "%s", ext); |
55acbb2a M |
103 | } |
104 | ||
105 | // printing and converting functions | |
9827020a | 106 | void print_hex(const uint8_t * data, const size_t len) { |
534983d7 | 107 | size_t i; |
9827020a | 108 | for (i=0; i < len; ++i) |
534983d7 | 109 | printf("%02x ", data[i]); |
9827020a | 110 | printf("\n"); |
111 | } | |
0d2c5909 | 112 | |
9827020a | 113 | void print_hex_break(const uint8_t *data, const size_t len, uint8_t breaks) { |
a1689f41 | 114 | |
115 | int rownum = 0; | |
116 | printf("[%02d] | ", rownum); | |
117 | for (int i = 0; i < len; ++i) { | |
534983d7 | 118 | |
9827020a | 119 | printf("%02X ", data[i]); |
120 | ||
121 | // check if a line break is needed | |
a1689f41 | 122 | if ( breaks > 0 && !((i+1) % breaks) && (i+1 < len) ) { |
123 | ++rownum; | |
124 | printf("\n[%02d] | ", rownum); | |
125 | } | |
9827020a | 126 | } |
534983d7 | 127 | printf("\n"); |
128 | } | |
129 | ||
334cc089 | 130 | char *sprint_hex(const uint8_t *data, const size_t len) { |
b915fda3 | 131 | |
132 | int maxLen = ( len > 1024/3) ? 1024/3 : len; | |
534983d7 | 133 | static char buf[1024]; |
334cc089 | 134 | memset(buf, 0x00, 1024); |
534983d7 | 135 | char * tmp = buf; |
136 | size_t i; | |
137 | ||
b915fda3 | 138 | for (i=0; i < maxLen; ++i, tmp += 3) |
334cc089 | 139 | sprintf(tmp, "%02X ", data[i]); |
534983d7 | 140 | return buf; |
141 | } | |
f89c7050 | 142 | |
2767fc02 | 143 | char *sprint_bin_break(const uint8_t *data, const size_t len, const uint8_t breaks) { |
581b31fb | 144 | |
bf32dd92 | 145 | // make sure we don't go beyond our char array memory |
581b31fb | 146 | size_t in_index = 0, out_index = 0; |
147 | int max_len; | |
0c97a456 | 148 | if (breaks==0) |
149 | max_len = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len; | |
150 | else | |
151 | max_len = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks); | |
152 | ||
bf32dd92 | 153 | static char buf[MAX_BIN_BREAK_LENGTH]; // 3072 + end of line characters if broken at 8 bits |
154 | //clear memory | |
155 | memset(buf, 0x00, sizeof(buf)); | |
2767fc02 | 156 | char *tmp = buf; |
79544b28 | 157 | |
bf32dd92 | 158 | // loop through the out_index to make sure we don't go too far |
581b31fb | 159 | for (out_index=0; out_index < max_len-2; out_index++) { |
bf32dd92 | 160 | // set character |
05164399 | 161 | sprintf(tmp++, "%u", (unsigned int) data[in_index]); |
9332b857 | 162 | // check if a line break is needed and we have room to print it in our array |
bf32dd92 | 163 | if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 != max_len) ) { |
164 | // increment and print line break | |
165 | out_index++; | |
2767fc02 | 166 | sprintf(tmp++, "%s","\n"); |
581b31fb | 167 | } |
bf32dd92 | 168 | in_index++; |
169 | } | |
581b31fb | 170 | // last char. |
171 | sprintf(tmp++, "%u", (unsigned int) data[in_index]); | |
79544b28 | 172 | return buf; |
173 | } | |
174 | ||
2767fc02 | 175 | char *sprint_bin(const uint8_t *data, const size_t len) { |
176 | return sprint_bin_break(data, len, 0); | |
177 | } | |
0c97a456 | 178 | |
179 | char *sprint_hex_ascii(const uint8_t *data, const size_t len) { | |
180 | static char buf[1024]; | |
508b37ba | 181 | char *tmp = buf; |
9827020a | 182 | memset(buf, 0x00, 1024); |
183 | size_t max_len = (len > 1010) ? 1010 : len; | |
042db564 | 184 | |
185 | sprintf(tmp, "%s| ", sprint_hex(data, max_len) ); | |
186 | ||
187 | size_t i = 0; | |
188 | size_t pos = (max_len * 3)+2; | |
189 | while(i < max_len){ | |
190 | char c = data[i]; | |
191 | if ( (c < 32) || (c == 127)) | |
192 | c = '.'; | |
193 | sprintf(tmp+pos+i, "%c", c); | |
194 | ++i; | |
195 | } | |
0c97a456 | 196 | return buf; |
197 | } | |
0d2c5909 | 198 | |
1cc80785 | 199 | char *sprint_ascii(const uint8_t *data, const size_t len) { |
200 | static char buf[1024]; | |
201 | char *tmp = buf; | |
202 | memset(buf, 0x00, 1024); | |
203 | size_t max_len = (len > 1010) ? 1010 : len; | |
204 | size_t i = 0; | |
205 | while(i < max_len){ | |
206 | char c = data[i]; | |
207 | tmp[i] = ((c < 32) || (c == 127)) ? '.' : c; | |
208 | ++i; | |
209 | } | |
210 | return buf; | |
211 | } | |
212 | ||
1f1d974f | 213 | void num_to_bytes(uint64_t n, size_t len, uint8_t* dest) { |
f89c7050 | 214 | while (len--) { |
7d159efe | 215 | dest[len] = n & 0xFF; |
f89c7050 M |
216 | n >>= 8; |
217 | } | |
218 | } | |
219 | ||
1f1d974f | 220 | uint64_t bytes_to_num(uint8_t* src, size_t len) { |
f89c7050 | 221 | uint64_t num = 0; |
1f1d974f | 222 | while (len--) { |
f89c7050 M |
223 | num = (num << 8) | (*src); |
224 | src++; | |
225 | } | |
226 | return num; | |
227 | } | |
f397b5cc | 228 | |
0d2c5909 | 229 | // takes a number (uint64_t) and creates a binarray in dest. |
230 | void num_to_bytebits(uint64_t n, size_t len, uint8_t *dest) { | |
a126332a | 231 | while (len--) { |
232 | dest[len] = n & 1; | |
233 | n >>= 1; | |
234 | } | |
235 | } | |
1f1d974f | 236 | |
0d2c5909 | 237 | //least significant bit first |
c805748f | 238 | void num_to_bytebitsLSBF(uint64_t n, size_t len, uint8_t *dest) { |
0d2c5909 | 239 | for(int i = 0 ; i < len ; ++i) { |
240 | dest[i] = n & 1; | |
241 | n >>= 1; | |
242 | } | |
243 | } | |
244 | ||
e1c88b09 | 245 | // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp |
246 | // to | |
247 | // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii | |
248 | // up to 64 bytes or 512 bits | |
224e8c1a | 249 | uint8_t *SwapEndian64(const uint8_t *src, const size_t len, const uint8_t blockSize){ |
3f5bcc3b | 250 | static uint8_t buf[64]; |
224e8c1a | 251 | memset(buf, 0x00, 64); |
252 | uint8_t *tmp = buf; | |
253 | for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){ | |
254 | for (size_t i = 0; i < blockSize; i++){ | |
255 | tmp[i+(blockSize*block)] = src[(blockSize-1-i)+(blockSize*block)]; | |
e1c88b09 | 256 | } |
257 | } | |
3f5bcc3b | 258 | return buf; |
e1c88b09 | 259 | } |
260 | ||
0d2c5909 | 261 | // takes a uint8_t src array, for len items and reverses the byte order in blocksizes (8,16,32,64), |
262 | // returns: the dest array contains the reordered src array. | |
f4d0ffd1 | 263 | void SwapEndian64ex(const uint8_t *src, const size_t len, const uint8_t blockSize, uint8_t *dest){ |
264 | for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){ | |
265 | for (size_t i = 0; i < blockSize; i++){ | |
266 | dest[i+(blockSize*block)] = src[(blockSize-1-i)+(blockSize*block)]; | |
267 | } | |
268 | } | |
269 | } | |
270 | ||
9ca155ba M |
271 | // ------------------------------------------------------------------------- |
272 | // string parameters lib | |
273 | // ------------------------------------------------------------------------- | |
274 | ||
f397b5cc M |
275 | // ------------------------------------------------------------------------- |
276 | // line - param line | |
277 | // bg, en - symbol numbers in param line of beginning an ending parameter | |
278 | // paramnum - param number (from 0) | |
279 | // ------------------------------------------------------------------------- | |
280 | int param_getptr(const char *line, int *bg, int *en, int paramnum) | |
281 | { | |
282 | int i; | |
283 | int len = strlen(line); | |
284 | ||
285 | *bg = 0; | |
286 | *en = 0; | |
287 | ||
288 | // skip spaces | |
289 | while (line[*bg] ==' ' || line[*bg]=='\t') (*bg)++; | |
290 | if (*bg >= len) { | |
291 | return 1; | |
292 | } | |
293 | ||
294 | for (i = 0; i < paramnum; i++) { | |
295 | while (line[*bg]!=' ' && line[*bg]!='\t' && line[*bg] != '\0') (*bg)++; | |
296 | while (line[*bg]==' ' || line[*bg]=='\t') (*bg)++; | |
297 | ||
298 | if (line[*bg] == '\0') return 1; | |
299 | } | |
300 | ||
301 | *en = *bg; | |
302 | while (line[*en] != ' ' && line[*en] != '\t' && line[*en] != '\0') (*en)++; | |
303 | ||
304 | (*en)--; | |
305 | ||
306 | return 0; | |
307 | } | |
308 | ||
309 | char param_getchar(const char *line, int paramnum) | |
310 | { | |
311 | int bg, en; | |
f397b5cc | 312 | if (param_getptr(line, &bg, &en, paramnum)) return 0x00; |
f397b5cc M |
313 | return line[bg]; |
314 | } | |
315 | ||
316 | uint8_t param_get8(const char *line, int paramnum) | |
317 | { | |
60e86577 | 318 | return param_get8ex(line, paramnum, 0, 10); |
f397b5cc M |
319 | } |
320 | ||
f6d9fb17 | 321 | /** |
31abe49f | 322 | * @brief Reads a decimal integer (actually, 0-254, not 255) |
f6d9fb17 MHS |
323 | * @param line |
324 | * @param paramnum | |
31abe49f | 325 | * @return -1 if error |
f6d9fb17 MHS |
326 | */ |
327 | uint8_t param_getdec(const char *line, int paramnum, uint8_t *destination) | |
328 | { | |
31abe49f | 329 | uint8_t val = param_get8ex(line, paramnum, 255, 10); |
31abe49f | 330 | if( (int8_t) val == -1) return 1; |
f6d9fb17 MHS |
331 | (*destination) = val; |
332 | return 0; | |
333 | } | |
334 | /** | |
335 | * @brief Checks if param is decimal | |
336 | * @param line | |
337 | * @param paramnum | |
338 | * @return | |
339 | */ | |
340 | uint8_t param_isdec(const char *line, int paramnum) | |
341 | { | |
342 | int bg, en; | |
343 | //TODO, check more thorougly | |
344 | if (!param_getptr(line, &bg, &en, paramnum)) return 1; | |
345 | // return strtoul(&line[bg], NULL, 10) & 0xff; | |
346 | ||
347 | return 0; | |
348 | } | |
349 | ||
f397b5cc M |
350 | uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base) |
351 | { | |
352 | int bg, en; | |
f397b5cc | 353 | if (!param_getptr(line, &bg, &en, paramnum)) |
6c6d1ac1 | 354 | return strtoul(&line[bg], NULL, base) & 0xff; |
f397b5cc M |
355 | else |
356 | return deflt; | |
357 | } | |
358 | ||
359 | uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base) | |
360 | { | |
361 | int bg, en; | |
f397b5cc | 362 | if (!param_getptr(line, &bg, &en, paramnum)) |
6c6d1ac1 | 363 | return strtoul(&line[bg], NULL, base); |
f397b5cc M |
364 | else |
365 | return deflt; | |
366 | } | |
367 | ||
368 | uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base) | |
369 | { | |
370 | int bg, en; | |
f397b5cc | 371 | if (!param_getptr(line, &bg, &en, paramnum)) |
6c6d1ac1 | 372 | return strtoull(&line[bg], NULL, base); |
f397b5cc M |
373 | else |
374 | return deflt; | |
f397b5cc M |
375 | } |
376 | ||
377 | int param_gethex(const char *line, int paramnum, uint8_t * data, int hexcnt) | |
378 | { | |
379 | int bg, en, temp, i; | |
380 | ||
508b37ba | 381 | if (hexcnt & 1) return 1; |
f397b5cc M |
382 | |
383 | if (param_getptr(line, &bg, &en, paramnum)) return 1; | |
384 | ||
508b37ba | 385 | if (en - bg + 1 != hexcnt) return 1; |
f397b5cc M |
386 | |
387 | for(i = 0; i < hexcnt; i += 2) { | |
388 | if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1])) ) return 1; | |
389 | ||
390 | sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp); | |
391 | data[i / 2] = temp & 0xff; | |
392 | } | |
393 | ||
394 | return 0; | |
395 | } | |
e98572a1 | 396 | int param_gethex_ex(const char *line, int paramnum, uint8_t * data, int *hexcnt) |
397 | { | |
398 | int bg, en, temp, i; | |
399 | ||
400 | //if (hexcnt % 2) | |
401 | // return 1; | |
402 | ||
403 | if (param_getptr(line, &bg, &en, paramnum)) return 1; | |
404 | ||
405 | *hexcnt = en - bg + 1; | |
406 | if (*hexcnt % 2) //error if not complete hex bytes | |
407 | return 1; | |
aea4d766 | 408 | |
e98572a1 | 409 | for(i = 0; i < *hexcnt; i += 2) { |
3bc7b13d | 410 | if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1])) ) return 1; |
e98572a1 | 411 | |
412 | sscanf((char[]){line[bg + i], line[bg + i + 1], 0}, "%X", &temp); | |
413 | data[i / 2] = temp & 0xff; | |
414 | } | |
415 | ||
416 | return 0; | |
417 | } | |
aea4d766 | 418 | int param_getstr(const char *line, int paramnum, char * str) |
419 | { | |
420 | int bg, en; | |
aea4d766 | 421 | if (param_getptr(line, &bg, &en, paramnum)) return 0; |
422 | ||
423 | memcpy(str, line + bg, en - bg + 1); | |
424 | str[en - bg + 1] = 0; | |
425 | ||
426 | return en - bg + 1; | |
427 | } | |
79544b28 | 428 | |
429 | /* | |
430 | The following methods comes from Rfidler sourcecode. | |
431 | https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/ | |
432 | */ | |
433 | ||
434 | // convert hex to sequence of 0/1 bit values | |
435 | // returns number of bits converted | |
436 | int hextobinarray(char *target, char *source) | |
437 | { | |
438 | int length, i, count= 0; | |
439 | char x; | |
440 | ||
441 | length = strlen(source); | |
442 | // process 4 bits (1 hex digit) at a time | |
443 | while(length--) | |
444 | { | |
445 | x= *(source++); | |
446 | // capitalize | |
447 | if (x >= 'a' && x <= 'f') | |
448 | x -= 32; | |
449 | // convert to numeric value | |
450 | if (x >= '0' && x <= '9') | |
451 | x -= '0'; | |
452 | else if (x >= 'A' && x <= 'F') | |
453 | x -= 'A' - 10; | |
454 | else | |
455 | return 0; | |
456 | // output | |
457 | for(i= 0 ; i < 4 ; ++i, ++count) | |
458 | *(target++)= (x >> (3 - i)) & 1; | |
459 | } | |
460 | ||
461 | return count; | |
462 | } | |
463 | ||
464 | // convert hex to human readable binary string | |
465 | int hextobinstring(char *target, char *source) | |
466 | { | |
467 | int length; | |
468 | ||
469 | if(!(length= hextobinarray(target, source))) | |
470 | return 0; | |
471 | binarraytobinstring(target, target, length); | |
472 | return length; | |
473 | } | |
474 | ||
475 | // convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source) | |
476 | // return number of bits converted | |
477 | int binarraytohex(char *target, char *source, int length) | |
478 | { | |
479 | unsigned char i, x; | |
480 | int j = length; | |
481 | ||
482 | if(j % 4) | |
483 | return 0; | |
484 | ||
485 | while(j) | |
486 | { | |
487 | for(i= x= 0 ; i < 4 ; ++i) | |
488 | x += ( source[i] << (3 - i)); | |
489 | sprintf(target,"%X", x); | |
490 | ++target; | |
491 | source += 4; | |
492 | j -= 4; | |
493 | } | |
494 | return length; | |
495 | } | |
496 | ||
497 | // convert binary array to human readable binary | |
498 | void binarraytobinstring(char *target, char *source, int length) | |
499 | { | |
500 | int i; | |
501 | ||
502 | for(i= 0 ; i < length ; ++i) | |
503 | *(target++)= *(source++) + '0'; | |
504 | *target= '\0'; | |
505 | } | |
506 | ||
507 | // return parity bit required to match type | |
a126332a | 508 | uint8_t GetParity( uint8_t *bits, uint8_t type, int length) |
79544b28 | 509 | { |
510 | int x; | |
ab7bb494 | 511 | for( x = 0 ; length > 0 ; --length) |
79544b28 | 512 | x += bits[length - 1]; |
513 | x %= 2; | |
79544b28 | 514 | return x ^ type; |
515 | } | |
516 | ||
517 | // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half | |
a126332a | 518 | void wiegand_add_parity(uint8_t *target, uint8_t *source, uint8_t length) |
79544b28 | 519 | { |
520 | *(target++)= GetParity(source, EVEN, length / 2); | |
521 | memcpy(target, source, length); | |
522 | target += length; | |
523 | *(target)= GetParity(source + length / 2, ODD, length / 2); | |
524 | } | |
c3c241f3 | 525 | |
0d2c5909 | 526 | // xor two arrays together for len items. The dst array contains the new xored values. |
c3c241f3 | 527 | void xor(unsigned char * dst, unsigned char * src, size_t len) { |
528 | for( ; len > 0; len--,dst++,src++) | |
529 | *dst ^= *src; | |
530 | } | |
531 | ||
532 | int32_t le24toh (uint8_t data[3]) { | |
533 | return (data[2] << 16) | (data[1] << 8) | data[0]; | |
534 | } | |
c805748f | 535 | uint32_t le32toh (uint8_t *data) { |
536 | return (uint32_t)( (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0]); | |
537 | } | |
0d2c5909 | 538 | // Pack a bitarray into a uint32_t. |
ad6219fc | 539 | uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits) { |
9984b173 | 540 | |
541 | if (len > 32) return 0; | |
ad6219fc | 542 | |
543 | int i = start; | |
544 | int j = len-1; | |
ad6219fc | 545 | uint32_t tmp = 0; |
9984b173 | 546 | |
ad6219fc | 547 | for (; j >= 0; --j, ++i) |
548 | tmp |= bits[i] << j; | |
549 | ||
550 | return tmp; | |
551 | } | |
9984b173 | 552 | |
ab7bb494 | 553 | // RotateLeft - Ultralight, Desfire, works on byte level |
554 | // 00-01-02 >> 01-02-00 | |
6bb7609c | 555 | void rol(uint8_t *data, const size_t len){ |
9984b173 | 556 | uint8_t first = data[0]; |
557 | for (size_t i = 0; i < len-1; i++) { | |
558 | data[i] = data[i+1]; | |
559 | } | |
560 | data[len-1] = first; | |
6bb7609c | 561 | } |
562 | ||
0d2c5909 | 563 | // Swap bit order on a uint32_t value. Can be limited by nrbits just use say 8bits reversal |
1f1d974f | 564 | // And clears the rest of the bits. |
6bb7609c | 565 | uint32_t SwapBits(uint32_t value, int nrbits) { |
566 | uint32_t newvalue = 0; | |
567 | for(int i = 0; i < nrbits; i++) { | |
568 | newvalue ^= ((value >> i) & 1) << (nrbits - 1 - i); | |
569 | } | |
570 | return newvalue; | |
3e134b4c | 571 | } |
572 | /* | |
573 | ref http://www.csm.ornl.gov/~dunigan/crc.html | |
574 | Returns the value v with the bottom b [0,32] bits reflected. | |
575 | Example: reflect(0x3e23L,3) == 0x3e26 | |
576 | */ | |
577 | uint32_t reflect(uint32_t v, int b) { | |
578 | uint32_t t = v; | |
579 | for ( int i = 0; i < b; ++i) { | |
580 | if (t & 1) | |
581 | v |= BITMASK((b-1)-i); | |
582 | else | |
583 | v &= ~BITMASK((b-1)-i); | |
584 | t>>=1; | |
585 | } | |
586 | return v; | |
587 | } | |
dae31af2 | 588 | |
589 | uint64_t HornerScheme(uint64_t num, uint64_t divider, uint64_t factor) { | |
590 | uint64_t remainder=0, quotient=0, result=0; | |
591 | remainder = num % divider; | |
592 | quotient = num / divider; | |
593 | if(!(quotient == 0 && remainder == 0)) | |
594 | result += HornerScheme(quotient, divider, factor) * factor + remainder; | |
595 | return result; | |
596 | } |