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