client/src/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 // ensure gmtime_r is available even with -std=c99; must be included before
  12 #if !defined(_WIN32) && !defined(__APPLE__)
  13 #define _POSIX_C_SOURCE 200112L
  14 #endif
  15
  16 #include "util.h"
  17
  18 #include <stdarg.h>
  19 #include <inttypes.h>
  20 #include <string.h>
  21 #include <ctype.h>
  22 #include <stdlib.h>
  23 #include <stdio.h>
  24 #include <time.h> // Mingw
  25
  26 #include "ui.h"     // PrintAndLog
  27
  28 #define UTIL_BUFFER_SIZE_SPRINT 4097
  29 // global client debug variable
  30 uint8_t g_debugMode = 0;
  31 // global client disable logging variable
  32 uint8_t g_printAndLog = PRINTANDLOG_PRINT | PRINTANDLOG_LOG;
  33 // global client tell if a pending prompt is present
  34 bool g_pendingPrompt = false;
  35
  36 #ifdef _WIN32
  37 #include <windows.h>
  38 #endif
  39
  40 #define MAX_BIN_BREAK_LENGTH   (3072+384+1)
  41
  42 #ifndef _WIN32
  43 #include <unistd.h>
  44 #include <fcntl.h>
  45
  46 int kbd_enter_pressed(void) {
  47     int flags;
  48     if ((flags = fcntl(STDIN_FILENO, F_GETFL, 0)) < 0) {
  49         PrintAndLogEx(ERR, "fcntl failed in kbd_enter_pressed");
  50         return -1;
  51     }
  52     //non-blocking
  53     flags |= O_NONBLOCK;
  54     if (fcntl(STDIN_FILENO, F_SETFL, flags) < 0) {
  55         PrintAndLogEx(ERR, "fcntl failed in kbd_enter_pressed");
  56         return -1;
  57     }
  58     int c;
  59     int ret = 0;
  60     do { //get all available chars
  61         c = getchar();
  62         ret |= c == '\n';
  63     } while (c != EOF);
  64     //blocking
  65     flags &= ~O_NONBLOCK;
  66     if (fcntl(STDIN_FILENO, F_SETFL, flags) < 0) {
  67         PrintAndLogEx(ERR, "fcntl failed in kbd_enter_pressed");
  68         return -1;
  69     }
  70     return ret;
  71 }
  72
  73 #else
  74
  75 #include <conio.h>
  76 int kbd_enter_pressed(void) {
  77     int ret = 0;
  78     while (kbhit()) {
  79         ret |= getch() == '\r';
  80     }
  81     return ret;
  82 }
  83 #endif
  84
  85 // create filename on hex uid.
  86 // param *fn   -  pointer to filename char array
  87 // param *uid  -  pointer to uid byte array
  88 // param *ext  -  ".log"
  89 // param uidlen - length of uid array.
  90 void FillFileNameByUID(char *filenamePrefix, const uint8_t *uid, const char *ext, const int uidlen) {
  91     if (filenamePrefix == NULL || uid == NULL || ext == NULL) {
  92         return;
  93     }
  94
  95     int len = strlen(filenamePrefix);
  96
  97     for (int j = 0; j < uidlen; j++)
  98         sprintf(filenamePrefix + len + j * 2, "%02X", uid[j]);
  99
 100     strcat(filenamePrefix, ext);
 101 }
 102
 103 // fill buffer from structure [{uint8_t data, size_t length},...]
 104 int FillBuffer(uint8_t *data, size_t maxDataLength, size_t *dataLength, ...) {
 105     *dataLength = 0;
 106     va_list valist;
 107     va_start(valist, dataLength);
 108
 109     uint8_t *vdata = NULL;
 110
 111     do {
 112         vdata = va_arg(valist, uint8_t *);
 113         if (!vdata)
 114             break;
 115
 116         size_t vlength = va_arg(valist, size_t);
 117         if (*dataLength + vlength >  maxDataLength) {
 118             va_end(valist);
 119             return 1;
 120         }
 121
 122         memcpy(&data[*dataLength], vdata, vlength);
 123         *dataLength += vlength;
 124
 125     } while (vdata);
 126
 127     va_end(valist);
 128
 129     return 0;
 130 }
 131
 132 bool CheckStringIsHEXValue(const char *value) {
 133     for (size_t i = 0; i < strlen(value); i++)
 134         if (!isxdigit(value[i]))
 135             return false;
 136
 137     if (strlen(value) % 2)
 138         return false;
 139
 140     return true;
 141 }
 142
 143 void hex_to_buffer(const uint8_t *buf, const uint8_t *hex_data, const size_t hex_len, const size_t hex_max_len,
 144                    const size_t min_str_len, const size_t spaces_between, bool uppercase) {
 145
 146     if (buf == NULL) return;
 147
 148     char *tmp = (char *)buf;
 149     size_t i;
 150     memset(tmp, 0x00, hex_max_len);
 151
 152     size_t max_len = (hex_len > hex_max_len) ? hex_max_len : hex_len;
 153
 154     for (i = 0; i < max_len; ++i, tmp += 2 + spaces_between) {
 155         sprintf(tmp, (uppercase) ? "%02X" : "%02x", (unsigned int) hex_data[i]);
 156
 157         for (size_t j = 0; j < spaces_between; j++)
 158             sprintf(tmp + 2 + j, " ");
 159     }
 160
 161     i *= (2 + spaces_between);
 162
 163     size_t mlen = min_str_len > i ? min_str_len : 0;
 164     if (mlen > hex_max_len)
 165         mlen = hex_max_len;
 166
 167     for (; i < mlen; i++, tmp += 1)
 168         sprintf(tmp, " ");
 169
 170     // remove last space
 171     *tmp = '\0';
 172     return;
 173 }
 174
 175 // printing and converting functions
 176 void print_hex(const uint8_t *data, const size_t len) {
 177     if (data == NULL || len == 0) return;
 178
 179     for (size_t i = 0; i < len; i++)
 180         PrintAndLogEx(NORMAL, "%02x " NOLF, data[i]);
 181
 182     PrintAndLogEx(NORMAL, "");
 183 }
 184
 185 void print_hex_break(const uint8_t *data, const size_t len, uint8_t breaks) {
 186     if (data == NULL || len == 0 || breaks == 0) return;
 187
 188     uint16_t rownum = 0;
 189     int i;
 190     for (i = 0; i < len; i += breaks, rownum++) {
 191         if (len - i < breaks) { // incomplete block, will be treated out of the loop
 192             break;
 193         }
 194         PrintAndLogEx(INFO, "%02u | %s", rownum, sprint_hex_ascii(data + i, breaks));
 195     }
 196
 197     // the last odd bytes
 198     uint8_t mod = len % breaks;
 199
 200     if (mod) {
 201         char buf[UTIL_BUFFER_SIZE_SPRINT + 3];
 202         memset(buf, 0, sizeof(buf));
 203         hex_to_buffer((uint8_t *)buf, data + i, mod, (sizeof(buf) - 1), 0, 1, true);
 204
 205         // add the spaces...
 206         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%*s", ((breaks - mod) * 3), " ");
 207         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "| %s", sprint_ascii(data + i, mod));
 208         PrintAndLogEx(INFO, "%02u | %s", rownum, buf);
 209     }
 210 }
 211
 212 static void print_buffer_ex(const uint8_t *data, const size_t len, int level, uint8_t breaks) {
 213
 214     if (len < 1)
 215         return;
 216
 217     char buf[UTIL_BUFFER_SIZE_SPRINT + 3];
 218     int i;
 219     for (i = 0; i < len; i += breaks) {
 220         if (len - i < breaks) { // incomplete block, will be treated out of the loop
 221             break;
 222         }
 223         // (16 * 3) + (16) +  + 1
 224         memset(buf, 0, sizeof(buf));
 225         sprintf(buf, "%*s%02x: ", (level * 4), " ", i);
 226
 227         hex_to_buffer((uint8_t *)(buf + strlen(buf)), data + i, breaks, (sizeof(buf) - strlen(buf) - 1), 0, 1, true);
 228         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "| %s", sprint_ascii(data + i, breaks));
 229         PrintAndLogEx(INFO, "%s", buf);
 230     }
 231
 232     // the last odd bytes
 233     uint8_t mod = len % breaks;
 234
 235     if (mod) {
 236         memset(buf, 0, sizeof(buf));
 237         sprintf(buf, "%*s%02x: ", (level * 4), " ", i);
 238         hex_to_buffer((uint8_t *)(buf + strlen(buf)), data + i, mod, (sizeof(buf) - strlen(buf) - 1), 0, 1, true);
 239
 240         // add the spaces...
 241         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%*s", ((breaks - mod) * 3), " ");
 242
 243         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "| %s", sprint_ascii(data + i, mod));
 244         PrintAndLogEx(INFO, "%s", buf);
 245     }
 246 }
 247
 248 void print_buffer(const uint8_t *data, const size_t len, int level) {
 249     print_buffer_ex(data, len, level, 16);
 250 }
 251
 252 void print_blocks(uint32_t *data, size_t len) {
 253     PrintAndLogEx(SUCCESS, "Blk | Data ");
 254     PrintAndLogEx(SUCCESS, "----+------------");
 255
 256     if (!data) {
 257         PrintAndLogEx(ERR, "..empty data");
 258     } else {
 259         for (uint8_t i = 0; i < len; i++)
 260             PrintAndLogEx(SUCCESS, " %02d | %08X", i, data[i]);
 261     }
 262 }
 263
 264 char *sprint_hex(const uint8_t *data, const size_t len) {
 265     static char buf[UTIL_BUFFER_SIZE_SPRINT - 3] = {0};
 266     hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, 0, 1, true);
 267     return buf;
 268 }
 269
 270 char *sprint_hex_inrow_ex(const uint8_t *data, const size_t len, const size_t min_str_len) {
 271     static char buf[UTIL_BUFFER_SIZE_SPRINT] = {0};
 272     hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, min_str_len, 0, true);
 273     return buf;
 274 }
 275
 276 char *sprint_hex_inrow(const uint8_t *data, const size_t len) {
 277     return sprint_hex_inrow_ex(data, len, 0);
 278 }
 279 char *sprint_hex_inrow_spaces(const uint8_t *data, const size_t len, size_t spaces_between) {
 280     static char buf[UTIL_BUFFER_SIZE_SPRINT] = {0};
 281     hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, 0, spaces_between, true);
 282     return buf;
 283 }
 284
 285 char *sprint_bin_break(const uint8_t *data, const size_t len, const uint8_t breaks) {
 286
 287     // make sure we don't go beyond our char array memory
 288     size_t rowlen = (len > MAX_BIN_BREAK_LENGTH) ? MAX_BIN_BREAK_LENGTH : len;
 289
 290     // 3072 + end of line characters if broken at 8 bits
 291     static char buf[MAX_BIN_BREAK_LENGTH];
 292     memset(buf, 0x00, sizeof(buf));
 293     char *tmp = buf;
 294
 295     // loop through the out_index to make sure we don't go too far
 296     for (int i = 0; i < rowlen; i++) {
 297
 298         char c = data[i];
 299         // manchester wrong bit marker
 300         if (c == 7)
 301             c = '.';
 302         else
 303             c += '0';
 304
 305         *(tmp++) = c;
 306
 307         // check if a line break is needed and we have room to print it in our array
 308         if (breaks > 1) {
 309             if (((i + 1) % breaks) == 0) {
 310
 311                 *(tmp++) = '\n';
 312             }
 313         }
 314     }
 315     return buf;
 316 }
 317 /*
 318 void sprint_bin_break_ex(uint8_t *src, size_t srclen, char *dest , uint8_t breaks) {
 319     if ( src == NULL ) return;
 320     if ( srclen < 1 ) return;
 321
 322     // make sure we don't go beyond our char array memory
 323     size_t in_index = 0, out_index = 0;
 324     int rowlen;
 325     if (breaks==0)
 326         rowlen = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len;
 327     else
 328         rowlen = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks);
 329
 330     PrintAndLogEx(NORMAL, "(sprint_bin_break) rowlen %d", rowlen);
 331
 332     // 3072 + end of line characters if broken at 8 bits
 333     dest = (char *)calloc(MAX_BIN_BREAK_LENGTH, sizeof(uint8_t));
 334     if (dest == NULL) return;
 335
 336     //clear memory
 337     memset(dest, 0x00, sizeof(dest));
 338
 339     // loop through the out_index to make sure we don't go too far
 340     for (out_index=0; out_index < rowlen-1; out_index++) {
 341         // set character
 342         sprintf(dest++, "%u", src[in_index]);
 343         // check if a line break is needed and we have room to print it in our array
 344         if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 != rowlen) ) {
 345             // increment and print line break
 346             out_index++;
 347             sprintf(dest++, "%s","\n");
 348         }
 349         in_index++;
 350     }
 351     // last char.
 352     sprintf(dest++, "%u", src[in_index]);
 353 }
 354 */
 355
 356 char *sprint_bin(const uint8_t *data, const size_t len) {
 357     return sprint_bin_break(data, len, 0);
 358 }
 359
 360 char *sprint_hex_ascii(const uint8_t *data, const size_t len) {
 361     static char buf[UTIL_BUFFER_SIZE_SPRINT];
 362     char *tmp = buf;
 363     memset(buf, 0x00, UTIL_BUFFER_SIZE_SPRINT);
 364     size_t max_len = (len > 1010) ? 1010 : len;
 365
 366     snprintf(tmp, UTIL_BUFFER_SIZE_SPRINT, "%s| ", sprint_hex(data, max_len));
 367
 368     size_t i = 0;
 369     size_t pos = (max_len * 3) + 2;
 370
 371     while (i < max_len) {
 372
 373         char c = data[i];
 374         if ((c < 32) || (c == 127))
 375             c = '.';
 376
 377         sprintf(tmp + pos + i, "%c",  c);
 378         ++i;
 379     }
 380     return buf;
 381 }
 382
 383 char *sprint_ascii_ex(const uint8_t *data, const size_t len, const size_t min_str_len) {
 384     static char buf[UTIL_BUFFER_SIZE_SPRINT];
 385     char *tmp = buf;
 386     memset(buf, 0x00, UTIL_BUFFER_SIZE_SPRINT);
 387     size_t max_len = (len > 1010) ? 1010 : len;
 388     size_t i = 0;
 389     while (i < max_len) {
 390         char c = data[i];
 391         tmp[i] = ((c < 32) || (c == 127)) ? '.' : c;
 392         ++i;
 393     }
 394
 395     size_t m = min_str_len > i ? min_str_len : 0;
 396     for (; i < m; ++i)
 397         tmp[i] = ' ';
 398
 399     return buf;
 400 }
 401 char *sprint_ascii(const uint8_t *data, const size_t len) {
 402     return sprint_ascii_ex(data, len, 0);
 403 }
 404
 405 int hex_to_bytes(const char *hexValue, uint8_t *bytesValue, size_t maxBytesValueLen) {
 406     char buf[4] = {0};
 407     int indx = 0;
 408     int bytesValueLen = 0;
 409     while (hexValue[indx]) {
 410         if (hexValue[indx] == '\t' || hexValue[indx] == ' ') {
 411             indx++;
 412             continue;
 413         }
 414
 415         if (isxdigit(hexValue[indx])) {
 416             buf[strlen(buf)] = hexValue[indx];
 417         } else {
 418             // if we have symbols other than spaces and hex
 419             return -1;
 420         }
 421
 422         if (maxBytesValueLen && bytesValueLen >= maxBytesValueLen) {
 423             // if we dont have space in buffer and have symbols to translate
 424             return -2;
 425         }
 426
 427         if (strlen(buf) >= 2) {
 428             uint32_t temp = 0;
 429             sscanf(buf, "%x", &temp);
 430             bytesValue[bytesValueLen] = (uint8_t)(temp & 0xff);
 431             memset(buf, 0, sizeof(buf));
 432             bytesValueLen++;
 433         }
 434
 435         indx++;
 436     }
 437
 438     if (strlen(buf) > 0)
 439         //error when not completed hex bytes
 440         return -3;
 441
 442     return bytesValueLen;
 443 }
 444
 445 // takes a number (uint64_t) and creates a binarray in dest.
 446 void num_to_bytebits(uint64_t n, size_t len, uint8_t *dest) {
 447     while (len--) {
 448         dest[len] = n & 1;
 449         n >>= 1;
 450     }
 451 }
 452
 453 //least significant bit first
 454 void num_to_bytebitsLSBF(uint64_t n, size_t len, uint8_t *dest) {
 455     for (size_t i = 0 ; i < len ; ++i) {
 456         dest[i] =  n & 1;
 457         n >>= 1;
 458     }
 459 }
 460
 461 void bytes_to_bytebits(void *src, size_t srclen, void *dest) {
 462
 463     uint8_t *s = (uint8_t *)src;
 464     uint8_t *d = (uint8_t *)dest;
 465
 466     uint32_t i = srclen * 8;
 467     while (srclen--) {
 468         uint8_t b = s[srclen];
 469         d[--i] = (b >> 0) & 1;
 470         d[--i] = (b >> 1) & 1;
 471         d[--i] = (b >> 2) & 1;
 472         d[--i] = (b >> 3) & 1;
 473         d[--i] = (b >> 4) & 1;
 474         d[--i] = (b >> 5) & 1;
 475         d[--i] = (b >> 6) & 1;
 476         d[--i] = (b >> 7) & 1;
 477     }
 478 }
 479
 480 // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
 481 // to
 482 // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
 483 // up to 64 bytes or 512 bits
 484 uint8_t *SwapEndian64(const uint8_t *src, const size_t len, const uint8_t blockSize) {
 485     static uint8_t buf[64];
 486     memset(buf, 0x00, 64);
 487     uint8_t *tmp = buf;
 488     for (uint8_t block = 0; block < (uint8_t)(len / blockSize); block++) {
 489         for (size_t i = 0; i < blockSize; i++) {
 490             tmp[i + (blockSize * block)] = src[(blockSize - 1 - i) + (blockSize * block)];
 491         }
 492     }
 493     return buf;
 494 }
 495
 496 // takes a uint8_t src array, for len items and reverses the byte order in blocksizes (8,16,32,64),
 497 // returns: the dest array contains the reordered src array.
 498 void SwapEndian64ex(const uint8_t *src, const size_t len, const uint8_t blockSize, uint8_t *dest) {
 499     for (uint8_t block = 0; block < (uint8_t)(len / blockSize); block++) {
 500         for (size_t i = 0; i < blockSize; i++) {
 501             dest[i + (blockSize * block)] = src[(blockSize - 1 - i) + (blockSize * block)];
 502         }
 503     }
 504 }
 505
 506 //  -------------------------------------------------------------------------
 507 //  string parameters lib
 508 //  -------------------------------------------------------------------------
 509
 510 //  -------------------------------------------------------------------------
 511 //  line     - param line
 512 //  bg, en   - symbol numbers in param line of beginning and ending parameter
 513 //  paramnum - param number (from 0)
 514 //  -------------------------------------------------------------------------
 515 int param_getptr(const char *line, int *bg, int *en, int paramnum) {
 516     int i;
 517     int len = strlen(line);
 518
 519     *bg = 0;
 520     *en = 0;
 521
 522     // skip spaces
 523     while (line[*bg] == ' ' || line[*bg] == '\t')(*bg)++;
 524     if (*bg >= len) {
 525         return 1;
 526     }
 527
 528     for (i = 0; i < paramnum; i++) {
 529         while (line[*bg] != ' ' && line[*bg] != '\t' && line[*bg] != '\0')(*bg)++;
 530         while (line[*bg] == ' ' || line[*bg] == '\t')(*bg)++;
 531
 532         if (line[*bg] == '\0') return 1;
 533     }
 534
 535     *en = *bg;
 536     while (line[*en] != ' ' && line[*en] != '\t' && line[*en] != '\0')(*en)++;
 537
 538     (*en)--;
 539
 540     return 0;
 541 }
 542
 543 int param_getlength(const char *line, int paramnum) {
 544     int bg, en;
 545
 546     if (param_getptr(line, &bg, &en, paramnum)) return 0;
 547
 548     return en - bg + 1;
 549 }
 550
 551 char param_getchar(const char *line, int paramnum) {
 552     return param_getchar_indx(line, 0, paramnum);
 553 }
 554
 555 char param_getchar_indx(const char *line, int indx, int paramnum) {
 556     int bg, en;
 557
 558     if (param_getptr(line, &bg, &en, paramnum)) return 0x00;
 559
 560     if (bg + indx > en)
 561         return '\0';
 562
 563     return line[bg + indx];
 564 }
 565
 566 uint8_t param_get8(const char *line, int paramnum) {
 567     return param_get8ex(line, paramnum, 0, 10);
 568 }
 569
 570 /**
 571  * @brief Reads a decimal integer (actually, 0-254, not 255)
 572  * @param line
 573  * @param paramnum
 574  * @return -1 if error
 575  */
 576 uint8_t param_getdec(const char *line, int paramnum, uint8_t *destination) {
 577     uint8_t val =  param_get8ex(line, paramnum, 255, 10);
 578     if ((int8_t) val == -1) return 1;
 579     (*destination) = val;
 580     return 0;
 581 }
 582 /**
 583  * @brief Checks if param is decimal
 584  * @param line
 585  * @param paramnum
 586  * @return
 587  */
 588 uint8_t param_isdec(const char *line, int paramnum) {
 589     int bg, en;
 590     //TODO, check more thorougly
 591     if (!param_getptr(line, &bg, &en, paramnum)) return 1;
 592     // return strtoul(&line[bg], NULL, 10) & 0xff;
 593
 594     return 0;
 595 }
 596
 597 uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base) {
 598     int bg, en;
 599     if (!param_getptr(line, &bg, &en, paramnum))
 600         return strtoul(&line[bg], NULL, base) & 0xff;
 601     else
 602         return deflt;
 603 }
 604
 605 uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base) {
 606     int bg, en;
 607     if (!param_getptr(line, &bg, &en, paramnum))
 608         return strtoul(&line[bg], NULL, base);
 609     else
 610         return deflt;
 611 }
 612
 613 uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base) {
 614     int bg, en;
 615     if (!param_getptr(line, &bg, &en, paramnum))
 616         return strtoull(&line[bg], NULL, base);
 617     else
 618         return deflt;
 619 }
 620
 621 float param_getfloat(const char *line, int paramnum, float deflt) {
 622     int bg, en;
 623     if (!param_getptr(line, &bg, &en, paramnum))
 624         return strtof(&line[bg], NULL);
 625     else
 626         return deflt;
 627 }
 628
 629 int param_gethex(const char *line, int paramnum, uint8_t *data, int hexcnt) {
 630     int bg, en, i;
 631     uint32_t temp;
 632
 633     if (hexcnt & 1) return 1;
 634
 635     if (param_getptr(line, &bg, &en, paramnum)) return 1;
 636
 637     if (en - bg + 1 != hexcnt) return 1;
 638
 639     for (i = 0; i < hexcnt; i += 2) {
 640         if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1]))) return 1;
 641
 642         sscanf((char[]) {line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
 643         data[i / 2] = temp & 0xff;
 644     }
 645
 646     return 0;
 647 }
 648 int param_gethex_ex(const char *line, int paramnum, uint8_t *data, int *hexcnt) {
 649     int bg, en, i;
 650     uint32_t temp;
 651
 652     if (param_getptr(line, &bg, &en, paramnum)) return 1;
 653
 654     *hexcnt = en - bg + 1;
 655     if (*hexcnt % 2) //error if not complete hex bytes
 656         return 1;
 657
 658     for (i = 0; i < *hexcnt; i += 2) {
 659         if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1]))) return 1;
 660
 661         sscanf((char[]) {line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
 662         data[i / 2] = temp & 0xff;
 663     }
 664
 665     return 0;
 666 }
 667
 668 int param_gethex_to_eol(const char *line, int paramnum, uint8_t *data, int maxdatalen, int *datalen) {
 669     int bg, en;
 670     uint32_t temp;
 671     char buf[5] = {0};
 672
 673     if (param_getptr(line, &bg, &en, paramnum)) return 1;
 674
 675     *datalen = 0;
 676
 677     int indx = bg;
 678     while (line[indx]) {
 679         if (line[indx] == '\t' || line[indx] == ' ') {
 680             indx++;
 681             continue;
 682         }
 683
 684         if (isxdigit(line[indx])) {
 685             buf[strlen(buf) + 1] = 0x00;
 686             buf[strlen(buf)] = line[indx];
 687         } else {
 688             // if we have symbols other than spaces and hex
 689             return 1;
 690         }
 691
 692         if (*datalen >= maxdatalen) {
 693             // if we dont have space in buffer and have symbols to translate
 694             return 2;
 695         }
 696
 697         if (strlen(buf) >= 2) {
 698             sscanf(buf, "%x", &temp);
 699             data[*datalen] = (uint8_t)(temp & 0xff);
 700             *buf = 0;
 701             (*datalen)++;
 702         }
 703
 704         indx++;
 705     }
 706
 707     if (strlen(buf) > 0)
 708         //error when not completed hex bytes
 709         return 3;
 710
 711     return 0;
 712 }
 713
 714 int param_getbin_to_eol(const char *line, int paramnum, uint8_t *data, int maxdatalen, int *datalen) {
 715     int bg, en;
 716     if (param_getptr(line, &bg, &en, paramnum)) {
 717         return 1;
 718     }
 719
 720     *datalen = 0;
 721     char buf[5] = {0};
 722     int indx = bg;
 723     while (line[indx]) {
 724         if (line[indx] == '\t' || line[indx] == ' ') {
 725             indx++;
 726             continue;
 727         }
 728
 729         if (line[indx] == '0' || line[indx] == '1') {
 730             buf[strlen(buf) + 1] = 0x00;
 731             buf[strlen(buf)] = line[indx];
 732         } else {
 733             // if we have symbols other than spaces and 0/1
 734             return 1;
 735         }
 736
 737         if (*datalen >= maxdatalen) {
 738             // if we dont have space in buffer and have symbols to translate
 739             return 2;
 740         }
 741
 742         if (strlen(buf) > 0) {
 743             uint32_t temp = 0;
 744             sscanf(buf, "%d", &temp);
 745             data[*datalen] = (uint8_t)(temp & 0xff);
 746             *buf = 0;
 747             (*datalen)++;
 748         }
 749
 750         indx++;
 751     }
 752     return 0;
 753 }
 754
 755 int param_getstr(const char *line, int paramnum, char *str, size_t buffersize) {
 756     int bg, en;
 757
 758     if (param_getptr(line, &bg, &en, paramnum)) {
 759         return 0;
 760     }
 761
 762     // Prevent out of bounds errors
 763     if (en - bg + 1 >= buffersize) {
 764         PrintAndLogEx(ERR, "out of bounds error: want %d bytes have %zu bytes\n", en - bg + 1 + 1, buffersize);
 765         return 0;
 766     }
 767
 768     memcpy(str, line + bg, en - bg + 1);
 769     str[en - bg + 1] = 0;
 770
 771     return en - bg + 1;
 772 }
 773
 774 /*
 775 The following methods comes from Rfidler sourcecode.
 776 https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
 777 */
 778 // convert hex to sequence of 0/1 bit values
 779 // returns number of bits converted
 780 int hextobinarray(char *target, char *source) {
 781     int length, i, count = 0;
 782     char *start = source;
 783     length = strlen(source);
 784     // process 4 bits (1 hex digit) at a time
 785     while (length--) {
 786         char x = *(source++);
 787         // capitalize
 788         if (x >= 'a' && x <= 'f')
 789             x -= 32;
 790         // convert to numeric value
 791         if (x >= '0' && x <= '9')
 792             x -= '0';
 793         else if (x >= 'A' && x <= 'F')
 794             x -= 'A' - 10;
 795         else {
 796             PrintAndLogEx(INFO, "(hextobinarray) discovered unknown character %c %d at idx %d of %s", x, x, (int16_t)(source - start), start);
 797             return 0;
 798         }
 799         // output
 800         for (i = 0 ; i < 4 ; ++i, ++count)
 801             *(target++) = (x >> (3 - i)) & 1;
 802     }
 803
 804     return count;
 805 }
 806
 807 // convert hex to human readable binary string
 808 int hextobinstring(char *target, char *source) {
 809     int length = hextobinarray(target, source);
 810     if (length == 0)
 811         return 0;
 812     binarraytobinstring(target, target, length);
 813     return length;
 814 }
 815
 816 // convert binary array of 0x00/0x01 values to hex
 817 // return number of bits converted
 818 int binarraytohex(char *target, const size_t targetlen, char *source, size_t srclen) {
 819     uint8_t i = 0, x = 0;
 820     uint32_t t = 0; // written target chars
 821     uint32_t r = 0; // consumed bits
 822     uint8_t w = 0; // wrong bits separator printed
 823     for (size_t s = 0 ; s < srclen; s++) {
 824         if ((source[s] == 0) || (source[s] == 1)) {
 825             w = 0;
 826             x += (source[s] << (3 - i));
 827             i++;
 828             if (i == 4) {
 829                 if (t >= targetlen - 2) {
 830                     return r;
 831                 }
 832                 sprintf(target + t, "%X", x);
 833                 t++;
 834                 r += 4;
 835                 x = 0;
 836                 i = 0;
 837             }
 838         } else {
 839             if (i > 0) {
 840                 if (t >= targetlen - 5) {
 841                     return r;
 842                 }
 843                 sprintf(target + t, "%X[%i]", x, i);
 844                 t += 4;
 845                 r += i;
 846                 x = 0;
 847                 i = 0;
 848                 w = 1;
 849             }
 850             if (w == 0) {
 851                 if (t >= targetlen - 2) {
 852                     return r;
 853                 }
 854                 sprintf(target + t, " ");
 855                 t++;
 856             }
 857             r++;
 858         }
 859     }
 860     return r;
 861 }
 862
 863 // convert binary array to human readable binary
 864 void binarraytobinstring(char *target, char *source,  int length) {
 865     for (int i = 0 ; i < length; ++i)
 866         *(target++) = *(source++) + '0';
 867     *target = '\0';
 868 }
 869
 870 int binstring2binarray(uint8_t *target, char *source, int length) {
 871     int count = 0;
 872     char *start = source;
 873     while (length--) {
 874         char x = *(source++);
 875         // convert from binary value
 876         if (x >= '0' && x <= '1')
 877             x -= '0';
 878         else {
 879             PrintAndLogEx(WARNING, "(binstring2binarray) discovered unknown character %c %d at idx %d of %s", x, x, (int16_t)(source - start), start);
 880             return 0;
 881         }
 882         *(target++) = x;
 883         count++;
 884     }
 885     return count;
 886 }
 887
 888 // return parity bit required to match type
 889 uint8_t GetParity(uint8_t *bits, uint8_t type, int length) {
 890     int x;
 891     for (x = 0 ; length > 0 ; --length)
 892         x += bits[length - 1];
 893     x %= 2;
 894     return x ^ type;
 895 }
 896
 897 // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
 898 void wiegand_add_parity(uint8_t *target, uint8_t *source, uint8_t length) {
 899     *(target++) = GetParity(source, EVEN, length / 2);
 900     memcpy(target, source, length);
 901     target += length;
 902     *(target) = GetParity(source + length / 2, ODD, length / 2);
 903 }
 904
 905 // add HID parity to binary array: ODD prefix for 1st half of ID, EVEN suffix for 2nd half
 906 void wiegand_add_parity_swapped(uint8_t *target, uint8_t *source, uint8_t length) {
 907     *(target++) = GetParity(source, ODD, length / 2);
 908     memcpy(target, source, length);
 909     target += length;
 910     *(target) = GetParity(source + length / 2, EVEN, length / 2);
 911 }
 912
 913 // Pack a bitarray into a uint32_t.
 914 uint32_t PackBits(uint8_t start, uint8_t len, uint8_t *bits) {
 915
 916     if (len > 32) return 0;
 917
 918     int i = start;
 919     int j = len - 1;
 920     uint32_t tmp = 0;
 921
 922     for (; j >= 0; --j, ++i)
 923         tmp |= bits[i] << j;
 924
 925     return tmp;
 926 }
 927
 928 uint64_t HornerScheme(uint64_t num, uint64_t divider, uint64_t factor) {
 929     uint64_t remaind = 0, quotient = 0, result = 0;
 930     remaind = num % divider;
 931     quotient = num / divider;
 932     if (!(quotient == 0 && remaind == 0))
 933         result += HornerScheme(quotient, divider, factor) * factor + remaind;
 934     return result;
 935 }
 936
 937 // determine number of logical CPU cores (use for multithreaded functions)
 938 int num_CPUs(void) {
 939 #if defined(_WIN32)
 940 #include <sysinfoapi.h>
 941     SYSTEM_INFO sysinfo;
 942     GetSystemInfo(&sysinfo);
 943     return sysinfo.dwNumberOfProcessors;
 944 #else
 945 #include <unistd.h>
 946     int count = sysconf(_SC_NPROCESSORS_ONLN);
 947     if (count <= 0)
 948         count = 1;
 949     return count;
 950 #endif
 951 }
 952
 953 void str_lower(char *s) {
 954     for (size_t i = 0; i < strlen(s); i++)
 955         s[i] = tolower(s[i]);
 956 }
 957
 958 // check for prefix in string
 959 bool str_startswith(const char *s,  const char *pre) {
 960     return strncmp(pre, s, strlen(pre)) == 0;
 961 }
 962
 963 // check for suffix in string
 964 bool str_endswith(const char *s,  const char *suffix) {
 965     size_t ls = strlen(s);
 966     size_t lsuffix = strlen(suffix);
 967     if (ls >= lsuffix) {
 968         return strncmp(suffix, s + (ls - lsuffix), lsuffix) == 0;
 969     }
 970     return false;
 971 }
 972
 973 // Replace unprintable characters with a dot in char buffer
 974 void clean_ascii(unsigned char *buf, size_t len) {
 975     for (size_t i = 0; i < len; i++) {
 976         if (!isprint(buf[i]))
 977             buf[i] = '.';
 978     }
 979 }
 980
 981 // replace \r \n to \0
 982 void strcleanrn(char *buf, size_t len) {
 983     strcreplace(buf, len, '\n', '\0');
 984     strcreplace(buf, len, '\r', '\0');
 985 }
 986
 987 // replace char in buffer
 988 void strcreplace(char *buf, size_t len, char from, char to) {
 989     for (size_t i = 0; i < len; i++) {
 990         if (buf[i] == from)
 991             buf[i] = to;
 992     }
 993 }
 994
 995
 996 char *str_dup(const char *src) {
 997     return str_ndup(src, strlen(src));
 998 }
 999 char *str_ndup(const char *src, size_t len) {
1000
1001     char *dest = (char *) calloc(len + 1, sizeof(uint8_t));
1002     if (dest != NULL) {
1003         memcpy(dest, src, len);
1004         dest[len] = '\0';
1005     }
1006     return dest;
1007 }
1008
1009 /**
1010  * Converts a hex string to component "hi2", "hi" and "lo" 32-bit integers
1011  * one nibble at a time.
1012  *
1013  * Returns the number of nibbles (4 bits) entered.
1014  */
1015 int hexstring_to_u96(uint32_t *hi2, uint32_t *hi, uint32_t *lo, const char *str) {
1016     uint32_t n = 0, i = 0;
1017
1018     while (sscanf(&str[i++], "%1x", &n) == 1) {
1019         *hi2 = (*hi2 << 4) | (*hi >> 28);
1020         *hi = (*hi << 4) | (*lo >> 28);
1021         *lo = (*lo << 4) | (n & 0xf);
1022     }
1023     return i - 1;
1024 }
1025
1026 /**
1027  * Converts a binary string to component "hi2", "hi" and "lo" 32-bit integers,
1028  * one bit at a time.
1029  *
1030  * Returns the number of bits entered.
1031  */
1032 int binstring_to_u96(uint32_t *hi2, uint32_t *hi, uint32_t *lo, const char *str) {
1033     uint32_t n = 0, i = 0;
1034
1035     for (;;) {
1036
1037         int res = sscanf(&str[i], "%1u", &n);
1038         if ((res != 1) || (n > 1))
1039             break;
1040
1041         *hi2 = (*hi2 << 1) | (*hi >> 31);
1042         *hi = (*hi << 1) | (*lo >> 31);
1043         *lo = (*lo << 1) | (n & 0x1);
1044
1045         i++;
1046     }
1047     return i;
1048 }
1049
1050
1051 /**
1052  * Converts a binary array to component "hi2", "hi" and "lo" 32-bit integers,
1053  * one bit at a time.
1054  *
1055  * Returns the number of bits entered.
1056  */
1057 int binarray_to_u96(uint32_t *hi2, uint32_t *hi, uint32_t *lo, uint8_t *arr, int arrlen) {
1058     int i = 0;
1059     for (; i < arrlen; i++) {
1060         uint8_t n = arr[i];
1061         if (n > 1)
1062             break;
1063
1064         *hi2 = (*hi2 << 1) | (*hi >> 31);
1065         *hi = (*hi << 1) | (*lo >> 31);
1066         *lo = (*lo << 1) | (n & 0x1);
1067     }
1068     return i;
1069 }
1070
1071 inline uint32_t bitcount32(uint32_t a) {
1072 #if defined __GNUC__
1073     return __builtin_popcountl(a);
1074 #else
1075     a = a - ((a >> 1) & 0x55555555);
1076     a = (a & 0x33333333) + ((a >> 2) & 0x33333333);
1077     return (((a + (a >> 4)) & 0x0f0f0f0f) * 0x01010101) >> 24;
1078 #endif
1079 }
1080
1081 inline uint64_t bitcount64(uint64_t a) {
1082 #if defined __GNUC__
1083     return __builtin_popcountll(a);
1084 #else
1085     PrintAndLogEx(FAILED, "Was not compiled with fct bitcount64");
1086     return 0;
1087 #endif
1088 }
1089
1090 inline uint32_t leadingzeros32(uint32_t a) {
1091 #if defined __GNUC__
1092     return __builtin_clzl(a);
1093 #else
1094     PrintAndLogEx(FAILED, "Was not compiled with fct bitcount64");
1095     return 0;
1096 #endif
1097 }
1098
1099 inline uint64_t leadingzeros64(uint64_t a) {
1100 #if defined __GNUC__
1101     return __builtin_clzll(a);
1102 #else
1103     PrintAndLogEx(FAILED, "Was not compiled with fct bitcount64");
1104     return 0;
1105 #endif
1106 }