client/src/util.c

   1 //-----------------------------------------------------------------------------
   2 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
   3 //
   4 // This program is free software: you can redistribute it and/or modify
   5 // it under the terms of the GNU General Public License as published by
   6 // the Free Software Foundation, either version 3 of the License, or
   7 // (at your option) any later version.
   8 //
   9 // This program is distributed in the hope that it will be useful,
  10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 // GNU General Public License for more details.
  13 //
  14 // See LICENSE.txt for the text of the license.
  15 //-----------------------------------------------------------------------------
  16 // utilities
  17 //-----------------------------------------------------------------------------
  18
  19 // ensure gmtime_r is available even with -std=c99; must be included before
  20 #if !defined(_WIN32) && !defined(__APPLE__)
  21 #define _POSIX_C_SOURCE 200112L
  22 #endif
  23
  24 #include "util.h"
  25
  26 #include <stdarg.h>
  27 #include <inttypes.h>
  28 #include <string.h>
  29 #include <ctype.h>
  30 #include <stdlib.h>
  31 #include <stdio.h>
  32 #include <time.h> // Mingw
  33
  34 #include "ui.h"     // PrintAndLog
  35
  36 #define UTIL_BUFFER_SIZE_SPRINT 8196
  37 // global client debug variable
  38 uint8_t g_debugMode = 0;
  39 // global client enable/disable printing/logging/grabbing variable
  40 uint8_t g_printAndLog = PRINTANDLOG_PRINT | PRINTANDLOG_LOG;
  41 // global pointer to grabbed output
  42 grabbed_output g_grabbed_output = {NULL, 0, 0};
  43 // global client tell if a pending prompt is present
  44 bool g_pendingPrompt = false;
  45 // global CPU core count override
  46 int g_numCPUs = 0;
  47
  48 #ifdef _WIN32
  49 #include <windows.h>
  50 #endif
  51
  52 #define MAX_BIN_BREAK_LENGTH   (3072 + 384 + 1)
  53
  54 #ifndef _WIN32
  55 #include <unistd.h>
  56 #include <fcntl.h>
  57
  58 int kbd_enter_pressed(void) {
  59     int flags;
  60     if ((flags = fcntl(STDIN_FILENO, F_GETFL, 0)) < 0) {
  61         PrintAndLogEx(ERR, "fcntl failed in kbd_enter_pressed");
  62         return -1;
  63     }
  64     //non-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     int c;
  71     int ret = 0;
  72     do { //get all available chars
  73         c = getchar();
  74         ret |= c == '\n';
  75     } while (c != EOF);
  76     //blocking
  77     flags &= ~O_NONBLOCK;
  78     if (fcntl(STDIN_FILENO, F_SETFL, flags) < 0) {
  79         PrintAndLogEx(ERR, "fcntl failed in kbd_enter_pressed");
  80         return -1;
  81     }
  82     return ret;
  83 }
  84
  85 #else
  86
  87 #include <conio.h>
  88 int kbd_enter_pressed(void) {
  89     int ret = 0;
  90     while (kbhit()) {
  91         ret |= getch() == '\r';
  92     }
  93     return ret;
  94 }
  95 #endif
  96
  97 static char inv_b2s(char v, bool uppercase) {
  98
  99     if (isxdigit(v) == 0) {
 100         return '.';
 101     }
 102
 103     uint8_t lut[] = {
 104         'f', 'e', 'd', 'c',
 105         'b', 'a', '9', '8',
 106         '7', '6', '5', '4',
 107         '3', '2', '1', '0'
 108     };
 109
 110     uint8_t tmp = (tolower(v) - 'a' + 10);
 111     if (isdigit(v)) {
 112         tmp = (v - 0x30);
 113     }
 114
 115     if (uppercase)
 116         return toupper(lut[tmp]);
 117     else
 118         return lut[tmp];
 119 }
 120
 121 static char b2s(uint8_t v, bool uppercase) {
 122     // clear higher bits
 123     v &= 0xF;
 124
 125     switch (v) {
 126         case 0xA :
 127             return (uppercase ? 'A' : 'a') ;
 128         case 0xB :
 129             return (uppercase ? 'B' : 'b') ;
 130         case 0xC :
 131             return (uppercase ? 'C' : 'c') ;
 132         case 0xD :
 133             return (uppercase ? 'D' : 'd') ;
 134         case 0xE :
 135             return (uppercase ? 'E' : 'e') ;
 136         case 0xF :
 137             return (uppercase ? 'F' : 'f') ;
 138         default:
 139             return (char)(v + 0x30);
 140     }
 141 }
 142
 143 // create filename on hex uid.
 144 // param *fn   -  pointer to filename char array
 145 // param *uid  -  pointer to uid byte array
 146 // param *ext  -  ".log"
 147 // param uidlen - length of uid array.
 148 void FillFileNameByUID(char *filenamePrefix, const uint8_t *uid, const char *ext, const int uidlen) {
 149     if (filenamePrefix == NULL || uid == NULL || ext == NULL) {
 150         return;
 151     }
 152
 153     int len = strlen(filenamePrefix);
 154
 155     for (int j = 0; j < uidlen; j++) {
 156         // This is technically not the safest option, but there is no way to make this work without changing the function signature
 157         // Possibly todo for future PR, but given UID lenghts are defined by program and not variable, should not be an issue
 158         snprintf(filenamePrefix + len + j * 2, 3, "%02X", uid[j]);
 159     }
 160
 161     strcat(filenamePrefix, ext);
 162 }
 163
 164 // fill buffer from structure [{uint8_t data, size_t length},...]
 165 int FillBuffer(uint8_t *data, size_t maxDataLength, size_t *dataLength, ...) {
 166     *dataLength = 0;
 167     va_list valist;
 168     va_start(valist, dataLength);
 169
 170     uint8_t *vdata = NULL;
 171
 172     do {
 173         vdata = va_arg(valist, uint8_t *);
 174         if (!vdata)
 175             break;
 176
 177         size_t vlength = va_arg(valist, size_t);
 178         if (*dataLength + vlength >  maxDataLength) {
 179             va_end(valist);
 180             return 1;
 181         }
 182
 183         memcpy(&data[*dataLength], vdata, vlength);
 184         *dataLength += vlength;
 185
 186     } while (vdata);
 187
 188     va_end(valist);
 189
 190     return 0;
 191 }
 192
 193 bool CheckStringIsHEXValue(const char *value) {
 194     for (size_t i = 0; i < strlen(value); i++)
 195         if (!isxdigit(value[i]))
 196             return false;
 197
 198     if (strlen(value) % 2)
 199         return false;
 200
 201     return true;
 202 }
 203
 204 void ascii_to_buffer(uint8_t *buf, const uint8_t *hex_data, const size_t hex_len,
 205                      const size_t hex_max_len, const size_t min_str_len) {
 206
 207     if (buf == NULL) return;
 208
 209     char *tmp_base = (char *)buf;
 210     char *tmp = tmp_base;
 211
 212     size_t max_len = (hex_len > hex_max_len) ? hex_max_len : hex_len;
 213
 214     size_t i = 0;
 215     for (i = 0; i < max_len; ++i, tmp++) {
 216         char c = hex_data[i];
 217         *tmp = ((c < 32) || (c == 127)) ? '.' : c;
 218     }
 219
 220     size_t m = (min_str_len > i) ? min_str_len : 0;
 221     if (m > hex_max_len)
 222         m = hex_max_len;
 223
 224     for (; i < m; i++, tmp++)
 225         *tmp = ' ';
 226
 227     // remove last space
 228     *tmp = '\0';
 229 }
 230
 231 void hex_to_buffer(uint8_t *buf, const uint8_t *hex_data, const size_t hex_len, const size_t hex_max_len,
 232                    const size_t min_str_len, const size_t spaces_between, bool uppercase) {
 233
 234     // sanity check
 235     if (buf == NULL || hex_len < 1)
 236         return;
 237
 238     // 1. hex string length.
 239     // 2. byte array to be converted to string
 240     //
 241
 242     size_t max_byte_len = (hex_len > hex_max_len) ? hex_max_len : hex_len;
 243     size_t max_str_len = (max_byte_len * (2 + spaces_between)) + 1;
 244     char *tmp_base = (char *)buf;
 245     char *tmp = tmp_base;
 246
 247     size_t i;
 248     for (i = 0; (i < max_byte_len) && (max_str_len > strlen(tmp_base)) ; ++i) {
 249
 250         *(tmp++) = b2s((hex_data[i] >> 4), uppercase);
 251         *(tmp++) = b2s(hex_data[i], uppercase);
 252
 253         for (size_t j = 0; j < spaces_between; j++)
 254             *(tmp++) = ' ';
 255     }
 256
 257     i *= (2 + spaces_between);
 258
 259     size_t m = (min_str_len > i) ? min_str_len : 0;
 260     if (m > hex_max_len)
 261         m = hex_max_len;
 262
 263     while (m--)
 264         *(tmp++) = ' ';
 265
 266     // remove last space
 267     *tmp = '\0';
 268
 269 }
 270
 271 // printing and converting functions
 272 void print_hex(const uint8_t *data, const size_t len) {
 273     if (data == NULL || len == 0) return;
 274
 275     for (size_t i = 0; i < len; i++)
 276         PrintAndLogEx(NORMAL, "%02x " NOLF, data[i]);
 277
 278     PrintAndLogEx(NORMAL, "");
 279 }
 280
 281 void print_hex_break(const uint8_t *data, const size_t len, uint8_t breaks) {
 282     if (data == NULL || len == 0 || breaks == 0) return;
 283
 284     uint16_t rownum = 0;
 285     int i;
 286     for (i = 0; i < len; i += breaks, rownum++) {
 287         if (len - i < breaks) { // incomplete block, will be treated out of the loop
 288             break;
 289         }
 290         PrintAndLogEx(INFO, "%02u | %s", rownum, sprint_hex_ascii(data + i, breaks));
 291     }
 292
 293     // the last odd bytes
 294     uint8_t mod = len % breaks;
 295
 296     if (mod) {
 297         char buf[UTIL_BUFFER_SIZE_SPRINT + 3] = {0};
 298         hex_to_buffer((uint8_t *)buf, data + i, mod, (sizeof(buf) - 1), 0, 1, true);
 299
 300         // add the spaces...
 301         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%*s", ((breaks - mod) * 3), " ");
 302         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "| %s", sprint_ascii(data + i, mod));
 303         PrintAndLogEx(INFO, "%02u | %s", rownum, buf);
 304     }
 305 }
 306
 307 void print_hex_noascii_break(const uint8_t *data, const size_t len, uint8_t breaks) {
 308     if (data == NULL || len == 0 || breaks == 0) return;
 309
 310     int i;
 311     for (i = 0; i < len; i += breaks) {
 312         if (len - i < breaks) { // incomplete block, will be treated out of the loop
 313             break;
 314         }
 315         PrintAndLogEx(INFO, "%s", sprint_hex_inrow_spaces(data + i, breaks, 0));
 316     }
 317
 318     // the last odd bytes
 319     uint8_t mod = len % breaks;
 320
 321     if (mod) {
 322         char buf[UTIL_BUFFER_SIZE_SPRINT + 3] = {0};
 323         hex_to_buffer((uint8_t *)buf, data + i, mod, (sizeof(buf) - 1), 0, 0, true);
 324
 325         // add the spaces...
 326         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%*s", ((breaks - mod) * 3), " ");
 327         PrintAndLogEx(INFO, "%s", buf);
 328     }
 329 }
 330
 331 static void print_buffer_ex(const uint8_t *data, const size_t len, int level, uint8_t breaks) {
 332
 333     // sanity checks
 334     if ((data == NULL) || (len < 1))
 335         return;
 336
 337     char buf[UTIL_BUFFER_SIZE_SPRINT + 3] = {0};
 338     int i;
 339     for (i = 0; i < len; i += breaks) {
 340
 341         memset(buf, 0x00, sizeof(buf));
 342
 343         if (len - i < breaks) { // incomplete block, will be treated out of the loop
 344             break;
 345         }
 346
 347         // (16 * 3) + (16) +  + 1
 348         snprintf(buf, sizeof(buf), "%*s%02x: ", (level * 4), " ", i);
 349
 350         hex_to_buffer((uint8_t *)(buf + strlen(buf)), data + i, breaks, (sizeof(buf) - strlen(buf) - 1), 0, 1, true);
 351
 352         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "| %s", sprint_ascii(data + i, breaks));
 353
 354         PrintAndLogEx(INFO, "%s", buf);
 355     }
 356
 357     // the last odd bytes
 358     uint8_t mod = len % breaks;
 359
 360     if (mod) {
 361         snprintf(buf, sizeof(buf), "%*s%02x: ", (level * 4), " ", i);
 362         hex_to_buffer((uint8_t *)(buf + strlen(buf)), data + i, mod, (sizeof(buf) - strlen(buf) - 1), 0, 1, true);
 363
 364         // add the spaces...
 365         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%*s", ((breaks - mod) * 3), " ");
 366
 367         snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "| %s", sprint_ascii(data + i, mod));
 368         PrintAndLogEx(INFO, "%s", buf);
 369     }
 370 }
 371
 372 void print_buffer(const uint8_t *data, const size_t len, int level) {
 373     print_buffer_ex(data, len, level, 16);
 374 }
 375
 376 void print_buffer_with_offset(const uint8_t *data, const size_t len, int offset, bool print_header) {
 377     if (print_header) {
 378         PrintAndLogEx(INFO, " Offset  | Data                                            | Ascii");
 379         PrintAndLogEx(INFO, "----------------------------------------------------------------------------");
 380     }
 381
 382     for (uint32_t i = 0; i < len; i += 16) {
 383         uint32_t l = len - i;
 384         PrintAndLogEx(INFO, "%3d/0x%02X | %s" NOLF, offset + i, offset + i, sprint_hex(&data[i], l > 16 ? 16 : l));
 385         if (l < 16)
 386             PrintAndLogEx(NORMAL, "%*s" NOLF, 3 * (16 - l), " ");
 387         PrintAndLogEx(NORMAL, "| %s", sprint_ascii(&data[i], l > 16 ? 16 : l));
 388     }
 389 }
 390
 391 void print_blocks(uint32_t *data, size_t len) {
 392     PrintAndLogEx(SUCCESS, "Blk | Data ");
 393     PrintAndLogEx(SUCCESS, "----+------------");
 394
 395     if (!data) {
 396         PrintAndLogEx(ERR, "..empty data");
 397     } else {
 398         for (size_t i = 0; i < len; i++)
 399             PrintAndLogEx(SUCCESS, " %02zd | %08X", i, data[i]);
 400     }
 401 }
 402
 403 char *sprint_hex(const uint8_t *data, const size_t len) {
 404     static char buf[UTIL_BUFFER_SIZE_SPRINT] = {0};
 405     memset(buf, 0x00, sizeof(buf));
 406     hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, 0, 1, true);
 407     return buf;
 408 }
 409
 410 char *sprint_hex_inrow_ex(const uint8_t *data, const size_t len, const size_t min_str_len) {
 411     static char buf[UTIL_BUFFER_SIZE_SPRINT] = {0};
 412     memset(buf, 0x00, sizeof(buf));
 413     hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, min_str_len, 0, true);
 414     return buf;
 415 }
 416
 417 char *sprint_hex_inrow(const uint8_t *data, const size_t len) {
 418     return sprint_hex_inrow_ex(data, len, 0);
 419 }
 420
 421 char *sprint_hex_inrow_spaces(const uint8_t *data, const size_t len, size_t spaces_between) {
 422     static char buf[UTIL_BUFFER_SIZE_SPRINT] = {0};
 423     memset(buf, 0x00, sizeof(buf));
 424     hex_to_buffer((uint8_t *)buf, data, len, sizeof(buf) - 1, 0, spaces_between, true);
 425     return buf;
 426 }
 427
 428 char *sprint_bytebits_bin_break(const uint8_t *data, const size_t len, const uint8_t breaks) {
 429
 430     // make sure we don't go beyond our char array memory
 431     size_t rowlen = (len > MAX_BIN_BREAK_LENGTH) ? MAX_BIN_BREAK_LENGTH : len;
 432
 433     // 3072 + end of line characters if broken at 8 bits
 434     static char buf[MAX_BIN_BREAK_LENGTH] = {0};
 435     memset(buf, 0, sizeof(buf));
 436
 437     char *tmp = buf;
 438
 439     // loop through the out_index to make sure we don't go too far
 440     for (int i = 0; i < rowlen; i++) {
 441
 442         char c = data[i];
 443         // manchester wrong bit marker
 444         if (c == 7) {
 445             c = '.';
 446         } else if (c < 2) {
 447             c += '0';
 448         } else {
 449             PrintAndLogEx(ERR, "Invalid data passed to sprint_bytebits_bin_break()");
 450             return buf;
 451         }
 452
 453         *(tmp++) = c;
 454
 455         // check if a line break is needed and we have room to print it in our array
 456         if (breaks > 1) {
 457             if (((i + 1) % breaks) == 0) {
 458
 459                 *(tmp++) = '\n';
 460             }
 461         }
 462     }
 463     return buf;
 464 }
 465 /*
 466 void sprint_bin_break_ex(uint8_t *src, size_t srclen, char *dest , uint8_t breaks) {
 467     if ( src == NULL ) return;
 468     if ( srclen < 1 ) return;
 469
 470     // make sure we don't go beyond our char array memory
 471     size_t in_index = 0, out_index = 0;
 472     int rowlen;
 473     if (breaks==0)
 474         rowlen = ( len > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len;
 475     else
 476         rowlen = ( len+(len/breaks) > MAX_BIN_BREAK_LENGTH ) ? MAX_BIN_BREAK_LENGTH : len+(len/breaks);
 477
 478     PrintAndLogEx(NORMAL, "(sprint_bin_break) rowlen %d", rowlen);
 479
 480     // 3072 + end of line characters if broken at 8 bits
 481     dest = (char *)calloc(MAX_BIN_BREAK_LENGTH, sizeof(uint8_t));
 482     if (dest == NULL) return;
 483
 484     //clear memory
 485     memset(dest, 0x00, sizeof(dest));
 486
 487     // loop through the out_index to make sure we don't go too far
 488     for (out_index=0; out_index < rowlen-1; out_index++) {
 489         // set character
 490         sprintf(dest++, "%u", src[in_index]);
 491         // check if a line break is needed and we have room to print it in our array
 492         if ( (breaks > 0) && !((in_index+1) % breaks) && (out_index+1 != rowlen) ) {
 493             // increment and print line break
 494             out_index++;
 495             sprintf(dest++, "%s","\n");
 496         }
 497         in_index++;
 498     }
 499     // last char.
 500     sprintf(dest++, "%u", src[in_index]);
 501 }
 502 */
 503
 504 char *sprint_bytebits_bin(const uint8_t *data, const size_t len) {
 505     return sprint_bytebits_bin_break(data, len, 0);
 506 }
 507
 508 char *sprint_bin(const uint8_t *data, const size_t len) {
 509     size_t binlen = (len * 8 > MAX_BIN_BREAK_LENGTH) ? MAX_BIN_BREAK_LENGTH : len * 8;
 510     static uint8_t buf[MAX_BIN_BREAK_LENGTH] = {0};
 511     bytes_to_bytebits(data, binlen / 8, buf);
 512     return sprint_bytebits_bin_break(buf, binlen, 0);
 513 }
 514
 515 char *sprint_hex_ascii(const uint8_t *data, const size_t len) {
 516     static char buf[UTIL_BUFFER_SIZE_SPRINT + 20] = {0};
 517     memset(buf, 0x00, sizeof(buf));
 518
 519     char *tmp = buf;
 520     size_t max_len = (len > 1010) ? 1010 : len;
 521
 522     int ret = snprintf(buf, sizeof(buf) - 1, "%s| ", sprint_hex(data, max_len));
 523     if (ret < 0) {
 524         goto out;
 525     }
 526
 527     size_t i = 0;
 528     size_t pos = (max_len * 3) + 2;
 529
 530     while (i < max_len) {
 531         unsigned char c = (unsigned char)data[i];
 532         tmp[pos + i]  = (isprint(c) && c != 0xff) ? c : '.';
 533         ++i;
 534     }
 535 out:
 536     return buf;
 537 }
 538
 539 char *sprint_ascii_ex(const uint8_t *data, const size_t len, const size_t min_str_len) {
 540     static char buf[UTIL_BUFFER_SIZE_SPRINT] = {0};
 541     memset(buf, 0x00, sizeof(buf));
 542
 543     char *tmp = buf;
 544     size_t max_len = (len > 1010) ? 1010 : len;
 545     size_t i = 0;
 546
 547     while (i < max_len) {
 548         unsigned char c = (unsigned char)data[i];
 549         tmp[i]  = (isprint(c) && c != 0xff) ? c : '.';
 550         ++i;
 551     }
 552
 553     size_t m = min_str_len > i ? min_str_len : 0;
 554     for (; i < m; ++i) {
 555         tmp[i] = ' ';
 556     }
 557     return buf;
 558 }
 559 char *sprint_ascii(const uint8_t *data, const size_t len) {
 560     return sprint_ascii_ex(data, len, 0);
 561 }
 562
 563 char *sprint_breakdown_bin(color_t color, const char *bs, int width, int padn, int bits, const char *msg) {
 564
 565     if (bs == NULL || width > 32) {
 566         return NULL;
 567     }
 568
 569     const char *prepad     = "................................";
 570     const char *postmarker = " ................................";
 571     static char buf[32 + 120] = {0};
 572     memset(buf, 0, sizeof(buf));
 573
 574     int8_t end = (width - padn - bits);
 575     if (end < 0) {
 576         end = 0;
 577     }
 578
 579     switch (color) {
 580         case C_GREEN: {
 581             snprintf(buf, sizeof(buf), "%.*s" _GREEN_("%.*s") "%.*s - " _GREEN_("%s")
 582                      , padn, prepad
 583                      , bits, bs + padn
 584                      , end, postmarker
 585                      , msg
 586                     );
 587             break;
 588         }
 589         case C_RED: {
 590             snprintf(buf, sizeof(buf), "%.*s" _RED_("%.*s") "%.*s - " _RED_("%s")
 591                      , padn, prepad
 592                      , bits, bs + padn
 593                      , end, postmarker
 594                      , msg
 595                     );
 596             break;
 597         }
 598         case C_YELLOW: {
 599             snprintf(buf, sizeof(buf), "%.*s" _YELLOW_("%.*s") "%.*s - " _YELLOW_("%s")
 600                      , padn, prepad
 601                      , bits, bs + padn
 602                      , end, postmarker
 603                      , msg
 604                     );
 605             break;
 606         }
 607         case C_NONE:
 608         default: {
 609             snprintf(buf, sizeof(buf), "%.*s%.*s%.*s - %s"
 610                      , padn, prepad
 611                      , bits, bs + padn
 612                      , end, postmarker
 613                      , msg
 614                     );
 615             break;
 616         }
 617     }
 618     return buf;
 619 }
 620
 621 int hex_to_bytes(const char *hexValue, uint8_t *bytesValue, size_t maxBytesValueLen) {
 622     char buf[4] = {0};
 623     int indx = 0;
 624     int bytesValueLen = 0;
 625     while (hexValue[indx]) {
 626         if (hexValue[indx] == '\t' || hexValue[indx] == ' ') {
 627             indx++;
 628             continue;
 629         }
 630
 631         if (isxdigit(hexValue[indx])) {
 632             buf[strlen(buf)] = hexValue[indx];
 633         } else {
 634             // if we have symbols other than spaces and hex
 635             return -1;
 636         }
 637
 638         if (maxBytesValueLen && bytesValueLen >= maxBytesValueLen) {
 639             // if we don't have space in buffer and have symbols to translate
 640             return -2;
 641         }
 642
 643         if (strlen(buf) >= 2) {
 644             uint32_t temp = 0;
 645             sscanf(buf, "%x", &temp);
 646             bytesValue[bytesValueLen] = (uint8_t)(temp & 0xff);
 647             memset(buf, 0, sizeof(buf));
 648             bytesValueLen++;
 649         }
 650
 651         indx++;
 652     }
 653
 654     if (strlen(buf) > 0) {
 655         //error when not completed hex bytes
 656         return -3;
 657     }
 658
 659     return bytesValueLen;
 660 }
 661
 662 // takes a number (uint64_t) and creates a binarray in dest.
 663 void num_to_bytebits(uint64_t n, size_t len, uint8_t *dest) {
 664     while (len--) {
 665         dest[len] = n & 1;
 666         n >>= 1;
 667     }
 668 }
 669
 670 // least significant bit (lsb) first
 671 void num_to_bytebitsLSBF(uint64_t n, size_t len, uint8_t *dest) {
 672     for (size_t i = 0 ; i < len ; ++i) {
 673         dest[i] =  n & 1;
 674         n >>= 1;
 675     }
 676 }
 677
 678 void bytes_to_bytebits(const void *src, const size_t srclen, void *dest) {
 679
 680     uint8_t *s = (uint8_t *)src;
 681     uint8_t *d = (uint8_t *)dest;
 682
 683     uint32_t i = srclen * 8;
 684     size_t j = srclen;
 685     while (j--) {
 686         uint8_t b = s[j];
 687         d[--i] = (b >> 0) & 1;
 688         d[--i] = (b >> 1) & 1;
 689         d[--i] = (b >> 2) & 1;
 690         d[--i] = (b >> 3) & 1;
 691         d[--i] = (b >> 4) & 1;
 692         d[--i] = (b >> 5) & 1;
 693         d[--i] = (b >> 6) & 1;
 694         d[--i] = (b >> 7) & 1;
 695     }
 696 }
 697
 698 // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
 699 // to
 700 // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
 701 // up to 64 bytes or 512 bits
 702 uint8_t *SwapEndian64(const uint8_t *src, const size_t len, const uint8_t blockSize) {
 703     static uint8_t buf[64] = {0};
 704     memset(buf, 0x00, 64);
 705     uint8_t *tmp = buf;
 706     for (uint8_t block = 0; block < (uint8_t)(len / blockSize); block++) {
 707         for (size_t i = 0; i < blockSize; i++) {
 708             tmp[i + (blockSize * block)] = src[(blockSize - 1 - i) + (blockSize * block)];
 709         }
 710     }
 711     return buf;
 712 }
 713
 714 // takes a uint8_t src array, for len items and reverses the byte order in blocksizes (8,16,32,64),
 715 // returns: the dest array contains the reordered src array.
 716 void SwapEndian64ex(const uint8_t *src, const size_t len, const uint8_t blockSize, uint8_t *dest) {
 717     for (uint8_t block = 0; block < (uint8_t)(len / blockSize); block++) {
 718         for (size_t i = 0; i < blockSize; i++) {
 719             dest[i + (blockSize * block)] = src[(blockSize - 1 - i) + (blockSize * block)];
 720         }
 721     }
 722 }
 723
 724 //  -------------------------------------------------------------------------
 725 //  string parameters lib
 726 //  -------------------------------------------------------------------------
 727
 728 //  -------------------------------------------------------------------------
 729 //  line     - param line
 730 //  bg, en   - symbol numbers in param line of beginning and ending parameter
 731 //  paramnum - param number (from 0)
 732 //  -------------------------------------------------------------------------
 733 int param_getptr(const char *line, int *bg, int *en, int paramnum) {
 734     int i;
 735     int len = strlen(line);
 736
 737     *bg = 0;
 738     *en = 0;
 739
 740     // skip spaces
 741     while (line[*bg] == ' ' || line[*bg] == '\t')(*bg)++;
 742     if (*bg >= len) {
 743         return 1;
 744     }
 745
 746     for (i = 0; i < paramnum; i++) {
 747         while (line[*bg] != ' ' && line[*bg] != '\t' && line[*bg] != '\0')(*bg)++;
 748         while (line[*bg] == ' ' || line[*bg] == '\t')(*bg)++;
 749
 750         if (line[*bg] == '\0') return 1;
 751     }
 752
 753     *en = *bg;
 754     while (line[*en] != ' ' && line[*en] != '\t' && line[*en] != '\0')(*en)++;
 755
 756     (*en)--;
 757
 758     return 0;
 759 }
 760
 761 int param_getlength(const char *line, int paramnum) {
 762     int bg, en;
 763
 764     if (param_getptr(line, &bg, &en, paramnum)) return 0;
 765
 766     return en - bg + 1;
 767 }
 768
 769 char param_getchar(const char *line, int paramnum) {
 770     return param_getchar_indx(line, 0, paramnum);
 771 }
 772
 773 char param_getchar_indx(const char *line, int indx, int paramnum) {
 774     int bg, en;
 775
 776     if (param_getptr(line, &bg, &en, paramnum)) return 0x00;
 777
 778     if (bg + indx > en)
 779         return '\0';
 780
 781     return line[bg + indx];
 782 }
 783
 784 uint8_t param_get8(const char *line, int paramnum) {
 785     return param_get8ex(line, paramnum, 0, 10);
 786 }
 787
 788 /**
 789  * @brief Reads a decimal integer (actually, 0-254, not 255)
 790  * @param line
 791  * @param paramnum
 792  * @return -1 if error
 793  */
 794 uint8_t param_getdec(const char *line, int paramnum, uint8_t *destination) {
 795     uint8_t val =  param_get8ex(line, paramnum, 255, 10);
 796     if ((int8_t) val == -1) return 1;
 797     (*destination) = val;
 798     return 0;
 799 }
 800 /**
 801  * @brief Checks if param is decimal
 802  * @param line
 803  * @param paramnum
 804  * @return
 805  */
 806 uint8_t param_isdec(const char *line, int paramnum) {
 807     int bg, en;
 808     //TODO, check more thorougly
 809     if (!param_getptr(line, &bg, &en, paramnum)) return 1;
 810     // return strtoul(&line[bg], NULL, 10) & 0xff;
 811
 812     return 0;
 813 }
 814
 815 uint8_t param_get8ex(const char *line, int paramnum, int deflt, int base) {
 816     int bg, en;
 817     if (!param_getptr(line, &bg, &en, paramnum))
 818         return strtoul(&line[bg], NULL, base) & 0xff;
 819     else
 820         return deflt;
 821 }
 822
 823 uint32_t param_get32ex(const char *line, int paramnum, int deflt, int base) {
 824     int bg, en;
 825     if (!param_getptr(line, &bg, &en, paramnum))
 826         return strtoul(&line[bg], NULL, base);
 827     else
 828         return deflt;
 829 }
 830
 831 uint64_t param_get64ex(const char *line, int paramnum, int deflt, int base) {
 832     int bg, en;
 833     if (!param_getptr(line, &bg, &en, paramnum))
 834         return strtoull(&line[bg], NULL, base);
 835     else
 836         return deflt;
 837 }
 838
 839 float param_getfloat(const char *line, int paramnum, float deflt) {
 840     int bg, en;
 841     if (!param_getptr(line, &bg, &en, paramnum))
 842         return strtof(&line[bg], NULL);
 843     else
 844         return deflt;
 845 }
 846
 847 int param_gethex_ex(const char *line, int paramnum, uint8_t *data, int *hexcnt) {
 848     int bg, en, i;
 849     uint32_t temp;
 850
 851     if (param_getptr(line, &bg, &en, paramnum)) return 1;
 852
 853     *hexcnt = en - bg + 1;
 854
 855     // error if not complete hex bytes
 856     if (*hexcnt & 1) {
 857         return 1;
 858     }
 859
 860     for (i = 0; i < *hexcnt; i += 2) {
 861         if (!(isxdigit(line[bg + i]) && isxdigit(line[bg + i + 1]))) return 1;
 862
 863         sscanf((char[]) {line[bg + i], line[bg + i + 1], 0}, "%X", &temp);
 864         data[i / 2] = temp & 0xff;
 865     }
 866
 867     return 0;
 868 }
 869
 870 int param_gethex_to_eol(const char *line, int paramnum, uint8_t *data, int maxdatalen, int *datalen) {
 871
 872     int bg, en;
 873
 874     if (param_getptr(line, &bg, &en, paramnum))
 875         return 1;
 876
 877     *datalen = 0;
 878     char buf[5] = {0};
 879
 880     int indx = bg;
 881     while (line[indx]) {
 882         if (line[indx] == '\t' || line[indx] == ' ') {
 883             indx++;
 884             continue;
 885         }
 886
 887         if (isxdigit(line[indx])) {
 888             buf[strlen(buf) + 1] = 0x00;
 889             buf[strlen(buf)] = line[indx];
 890         } else {
 891             // if we have symbols other than spaces and hex
 892             return 1;
 893         }
 894
 895         if (*datalen >= maxdatalen) {
 896             // if we don't have space in buffer and have symbols to translate
 897             return 2;
 898         }
 899
 900         if (strlen(buf) >= 2) {
 901             uint32_t temp = 0;
 902             sscanf(buf, "%x", &temp);
 903             data[*datalen] = (uint8_t)(temp & 0xFF);
 904             *buf = 0;
 905             (*datalen)++;
 906         }
 907
 908         indx++;
 909     }
 910
 911     if (strlen(buf) > 0)
 912         //error when not completed hex bytes
 913         return 3;
 914
 915     return 0;
 916 }
 917
 918 int param_getbin_to_eol(const char *line, int paramnum, uint8_t *data, int maxdatalen, int *datalen) {
 919     int bg, en;
 920     if (param_getptr(line, &bg, &en, paramnum)) {
 921         return 1;
 922     }
 923
 924     *datalen = 0;
 925     char buf[5] = {0};
 926     int indx = bg;
 927     while (line[indx]) {
 928         if (line[indx] == '\t' || line[indx] == ' ') {
 929             indx++;
 930             continue;
 931         }
 932
 933         if (line[indx] == '0' || line[indx] == '1') {
 934             buf[strlen(buf) + 1] = 0x00;
 935             buf[strlen(buf)] = line[indx];
 936         } else {
 937             // if we have symbols other than spaces and 0/1
 938             return 1;
 939         }
 940
 941         if (*datalen >= maxdatalen) {
 942             // if we don't have space in buffer and have symbols to translate
 943             return 2;
 944         }
 945
 946         if (strlen(buf) > 0) {
 947             uint32_t temp = 0;
 948             sscanf(buf, "%u", &temp);
 949             data[*datalen] = (uint8_t)(temp & 0xff);
 950             *buf = 0;
 951             (*datalen)++;
 952         }
 953
 954         indx++;
 955     }
 956     return 0;
 957 }
 958
 959 int param_getstr(const char *line, int paramnum, char *str, size_t buffersize) {
 960     int bg, en;
 961
 962     if (param_getptr(line, &bg, &en, paramnum)) {
 963         return 0;
 964     }
 965
 966     // Prevent out of bounds errors
 967     if (en - bg + 1 >= buffersize) {
 968         PrintAndLogEx(ERR, "out of bounds error: want %d bytes have %zu bytes\n", en - bg + 1 + 1, buffersize);
 969         return 0;
 970     }
 971
 972     memcpy(str, line + bg, en - bg + 1);
 973     str[en - bg + 1] = 0;
 974
 975     return en - bg + 1;
 976 }
 977
 978 /*
 979 The following methods comes from Rfidler sourcecode.
 980 https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
 981 */
 982 // convert hex to sequence of 0/1 bit values
 983 // returns number of bits converted
 984 int hextobinarray(char *target, char *source) {
 985     return hextobinarray_n(target, source, strlen(source));
 986 }
 987
 988 int hextobinarray_n(char *target, char *source, int sourcelen) {
 989     int i, count = 0;
 990     char *start = source;
 991     // process 4 bits (1 hex digit) at a time
 992     while (sourcelen--) {
 993         char x = *(source++);
 994         // capitalize
 995         if (x >= 'a' && x <= 'f') {
 996             x -= 32;
 997         }
 998         // convert to numeric value
 999         if (x >= '0' && x <= '9') {
1000             x -= '0';
1001         } else if (x >= 'A' && x <= 'F') {
1002             x -= 'A' - 10;
1003         } else {
1004             PrintAndLogEx(INFO, "(hextobinarray) discovered unknown character %c %d at idx %d of %s", x, x, (int16_t)(source - start), start);
1005             return 0;
1006         }
1007         // output
1008         for (i = 0 ; i < 4 ; ++i, ++count) {
1009             *(target++) = (x >> (3 - i)) & 1;
1010         }
1011     }
1012
1013     return count;
1014 }
1015
1016 // convert hexstring to human readable binary string
1017 int hextobinstring(char *target, char *source) {
1018     return hextobinstring_n(target, source, strlen(source));
1019 }
1020 // convert hexstring to human readable binary string
1021 int hextobinstring_n(char *target, char *source, int sourcelen) {
1022     int length = hextobinarray_n(target, source, sourcelen);
1023     if (length == 0) {
1024         return 0;
1025     }
1026     binarray_2_binstr(target, target, length);
1027     return length;
1028 }
1029
1030 // convert bytes to binary string
1031 void bytes_2_binstr(char *target,  const uint8_t *source, size_t sourcelen) {
1032     //uint8_t *p = *source;
1033     for (int i = 0 ; i < sourcelen; ++i) {
1034         uint8_t b = *(source++);
1035         *(target++) = ((b >> 7) & 0x1) + '0';
1036         *(target++) = ((b >> 6) & 0x1) + '0';
1037         *(target++) = ((b >> 5) & 0x1) + '0';
1038         *(target++) = ((b >> 4) & 0x1) + '0';
1039         *(target++) = ((b >> 3) & 0x1) + '0';
1040         *(target++) = ((b >> 2) & 0x1) + '0';
1041         *(target++) = ((b >> 1) & 0x1) + '0';
1042         *(target++) = (b & 0x1) + '0';
1043     }
1044     *target = '\0';
1045 }
1046
1047 // convert binary array of 0x00/0x01 values to hex
1048 // return number of bits converted
1049 int binarray_2_hex(char *target, const size_t targetlen, const char *source, size_t srclen) {
1050     uint8_t i = 0, x = 0;
1051     uint32_t t = 0; // written target chars
1052     uint32_t r = 0; // consumed bits
1053     uint8_t w = 0; // wrong bits separator printed
1054     for (size_t s = 0 ; s < srclen; s++) {
1055         if ((source[s] == 0) || (source[s] == 1)) {
1056             w = 0;
1057             x += (source[s] << (3 - i));
1058             i++;
1059             if (i == 4) {
1060                 if (t >= targetlen - 2) {
1061                     return r;
1062                 }
1063                 snprintf(target + t, targetlen - t, "%X", x);
1064                 t++;
1065                 r += 4;
1066                 x = 0;
1067                 i = 0;
1068             }
1069         } else {
1070             if (i > 0) {
1071                 if (t >= targetlen - 5) {
1072                     return r;
1073                 }
1074                 snprintf(target + t, targetlen - t, "%X[%i]", x, i);
1075                 t += 4;
1076                 r += i;
1077                 x = 0;
1078                 i = 0;
1079                 w = 1;
1080             }
1081             if (w == 0) {
1082                 if (t >= targetlen - 2) {
1083                     return r;
1084                 }
1085                 snprintf(target + t, targetlen - t, " ");
1086                 t++;
1087             }
1088             r++;
1089         }
1090     }
1091     return r;
1092 }
1093
1094 // convert binary array to human readable binary
1095 void binarray_2_binstr(char *target, char *source,  int length) {
1096     for (int i = 0 ; i < length; ++i) {
1097         *(target++) = *(source++) + '0';
1098     }
1099     *target = '\0';
1100 }
1101
1102 int binstr_2_binarray(uint8_t *target, char *source, int length) {
1103     int count = 0;
1104     char *start = source;
1105     while (length--) {
1106         char x = *(source++);
1107         // convert from binary value
1108         if (x >= '0' && x <= '1')
1109             x -= '0';
1110         else {
1111             PrintAndLogEx(WARNING, "(binstring2binarray) discovered unknown character %c %d at idx %d of %s", x, x, (int16_t)(source - start), start);
1112             return 0;
1113         }
1114         *(target++) = x;
1115         count++;
1116     }
1117     return count;
1118 }
1119
1120 void binstr_2_bytes(uint8_t *target, size_t *targetlen, const char *src) {
1121     size_t binlen = strlen(src);
1122     if (binlen == 0) {
1123         *targetlen = 0;
1124         return;
1125     }
1126
1127     // Calculate padding needed
1128     size_t padding = (8 - (binlen % 8)) % 8;
1129
1130     // Determine the size of the hexadecimal array
1131     *targetlen = (binlen + padding) / 8;
1132
1133     uint8_t b = 0;
1134     size_t bit_cnt = padding;
1135     size_t idx = 0;
1136
1137     // Process binary string
1138     for (size_t i = 0; i < binlen; ++i) {
1139         b = (b << 1) | (src[i] == '1');
1140         ++bit_cnt;
1141
1142         if (bit_cnt == 8) {
1143             target[idx++] = b;
1144             b = 0;
1145             bit_cnt = 0;
1146         }
1147     }
1148 }
1149
1150 void hex_xor(uint8_t *d, const uint8_t *x, int n) {
1151     while (n--) {
1152         d[n] ^= x[n];
1153     }
1154 }
1155
1156 void hex_xor_token(uint8_t *d, const uint8_t *x, int dn, int xn) {
1157     while (dn--) {
1158         d[dn] ^= x[dn % xn];
1159     }
1160 }
1161
1162
1163 // return parity bit required to match type
1164 uint8_t GetParity(const uint8_t *bits, uint8_t type, int length) {
1165     int x;
1166     for (x = 0 ; length > 0 ; --length)
1167         x += bits[length - 1];
1168     x %= 2;
1169     return x ^ type;
1170 }
1171
1172 // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
1173 void wiegand_add_parity(uint8_t *target, const uint8_t *source, uint8_t length) {
1174     *(target++) = GetParity(source, EVEN, length / 2);
1175     memcpy(target, source, length);
1176     target += length;
1177     *(target) = GetParity(source + length / 2, ODD, length / 2);
1178 }
1179
1180 // add HID parity to binary array: ODD prefix for 1st half of ID, EVEN suffix for 2nd half
1181 void wiegand_add_parity_swapped(uint8_t *target, const uint8_t *source, uint8_t length) {
1182     *(target++) = GetParity(source, ODD, length / 2);
1183     memcpy(target, source, length);
1184     target += length;
1185     *(target) = GetParity(source + length / 2, EVEN, length / 2);
1186 }
1187
1188 // Pack a bitarray into a uint32_t.
1189 uint32_t PackBits(uint8_t start, uint8_t len, const uint8_t *bits) {
1190
1191     if (len > 32) return 0;
1192
1193     int i = start;
1194     int j = len - 1;
1195     uint32_t tmp = 0;
1196
1197     for (; j >= 0; --j, ++i)
1198         tmp |= bits[i] << j;
1199
1200     return tmp;
1201 }
1202
1203 uint64_t HornerScheme(uint64_t num, uint64_t divider, uint64_t factor) {
1204     uint64_t remaind = 0, quotient = 0, result = 0;
1205     remaind = num % divider;
1206     quotient = num / divider;
1207     if (!(quotient == 0 && remaind == 0))
1208         result += HornerScheme(quotient, divider, factor) * factor + remaind;
1209     return result;
1210 }
1211
1212 int num_CPUs(void) {
1213     if (g_numCPUs > 0) {
1214         return g_numCPUs;
1215     }
1216
1217     return detect_num_CPUs();
1218 }
1219
1220 // determine number of logical CPU cores (use for multithreaded functions)
1221 int detect_num_CPUs(void) {
1222 #if defined(_WIN32)
1223 #include <sysinfoapi.h>
1224     SYSTEM_INFO sysinfo;
1225     GetSystemInfo(&sysinfo);
1226     return sysinfo.dwNumberOfProcessors;
1227 #else
1228     int count = sysconf(_SC_NPROCESSORS_ONLN);
1229     if (count <= 0)
1230         count = 1;
1231     return count;
1232 #endif
1233 }
1234
1235 void str_lower(char *s) {
1236     for (size_t i = 0; i < strlen(s); i++)
1237         s[i] = tolower(s[i]);
1238 }
1239
1240 void str_upper(char *s) {
1241     strn_upper(s, strlen(s));
1242 }
1243
1244 void strn_upper(char *s, size_t n) {
1245     for (size_t i = 0; i < n; i++)
1246         s[i] = toupper(s[i]);
1247 }
1248 // check for prefix in string
1249 bool str_startswith(const char *s,  const char *pre) {
1250     return strncmp(pre, s, strlen(pre)) == 0;
1251 }
1252
1253 // check for suffix in string
1254 bool str_endswith(const char *s,  const char *suffix) {
1255     size_t ls = strlen(s);
1256     size_t lsuffix = strlen(suffix);
1257     if (ls >= lsuffix) {
1258         return strncmp(suffix, s + (ls - lsuffix), lsuffix) == 0;
1259     }
1260     return false;
1261 }
1262
1263 // Replace unprintable characters with a dot in char buffer
1264 void clean_ascii(unsigned char *buf, size_t len) {
1265     for (size_t i = 0; i < len; i++) {
1266         if (!isprint(buf[i]))
1267             buf[i] = '.';
1268     }
1269 }
1270
1271 // replace \r \n to \0
1272 void str_cleanrn(char *buf, size_t len) {
1273     str_creplace(buf, len, '\n', '\0');
1274     str_creplace(buf, len, '\r', '\0');
1275 }
1276
1277 // replace char in buffer
1278 void str_creplace(char *buf, size_t len, char from, char to) {
1279     for (size_t i = 0; i < len; i++) {
1280         if (buf[i] == from)
1281             buf[i] = to;
1282     }
1283 }
1284
1285
1286 char *str_dup(const char *src) {
1287     return str_ndup(src, strlen(src));
1288 }
1289 char *str_ndup(const char *src, size_t len) {
1290
1291     char *dest = (char *) calloc(len + 1, sizeof(uint8_t));
1292     if (dest != NULL) {
1293         memcpy(dest, src, len);
1294         dest[len] = '\0';
1295     }
1296     return dest;
1297 }
1298
1299 size_t str_nlen(const char *src, size_t maxlen) {
1300     size_t len = 0;
1301     if (src) {
1302         for (char c = *src; (len < maxlen && c != '\0'); c = *++src) {
1303             len++;
1304         }
1305     }
1306     return len;
1307 }
1308
1309 void str_reverse(char *buf,  size_t len) {
1310     for (size_t i = 0; i < (len >> 1); i++) {
1311         char tmp = buf[i];
1312         buf[i] = buf[len - i - 1];
1313         buf[len - i - 1] = tmp;
1314     }
1315 }
1316
1317 void str_inverse_hex(char *buf, size_t len) {
1318     for (size_t i = 0; i < len; i++) {
1319         buf[i] = inv_b2s(buf[i], true);
1320     }
1321 }
1322
1323 void str_inverse_bin(char *buf, size_t len) {
1324     for (size_t i = 0; i < len; i++) {
1325
1326         char c = buf[i];
1327         if (c == '1')
1328             buf[i] = '0';
1329         else if (c == '0')
1330             buf[i] = '1';
1331         else
1332             buf[i] = '.';
1333     }
1334 }
1335
1336
1337 /**
1338  * Converts a hex string to component "hi2", "hi" and "lo" 32-bit integers
1339  * one nibble at a time.
1340  *
1341  * Returns the number of nibbles (4 bits) entered.
1342  */
1343 int hexstring_to_u96(uint32_t *hi2, uint32_t *hi, uint32_t *lo, const char *str) {
1344     uint32_t n = 0, i = 0;
1345
1346     while (sscanf(&str[i++], "%1x", &n) == 1) {
1347         *hi2 = (*hi2 << 4) | (*hi >> 28);
1348         *hi = (*hi << 4) | (*lo >> 28);
1349         *lo = (*lo << 4) | (n & 0xf);
1350     }
1351     return i - 1;
1352 }
1353
1354 /**
1355  * Converts a binary string to component "hi2", "hi" and "lo" 32-bit integers,
1356  * one bit at a time.
1357  *
1358  * Returns the number of bits entered.
1359  */
1360 int binstring_to_u96(uint32_t *hi2, uint32_t *hi, uint32_t *lo, const char *str) {
1361     uint32_t n = 0, i = 0;
1362
1363     for (;;) {
1364
1365         int res = sscanf(&str[i], "%1u", &n);
1366         if ((res != 1) || (n > 1))
1367             break;
1368
1369         *hi2 = (*hi2 << 1) | (*hi >> 31);
1370         *hi = (*hi << 1) | (*lo >> 31);
1371         *lo = (*lo << 1) | (n & 0x1);
1372
1373         i++;
1374     }
1375     return i;
1376 }
1377
1378
1379 /**
1380  * Converts a binary array to component "hi2", "hi" and "lo" 32-bit integers,
1381  * one bit at a time.
1382  *
1383  * Returns the number of bits entered.
1384  */
1385 int binarray_to_u96(uint32_t *hi2, uint32_t *hi, uint32_t *lo, const uint8_t *arr, int arrlen) {
1386     int i = 0;
1387     for (; i < arrlen; i++) {
1388         uint8_t n = arr[i];
1389         if (n > 1)
1390             break;
1391
1392         *hi2 = (*hi2 << 1) | (*hi >> 31);
1393         *hi = (*hi << 1) | (*lo >> 31);
1394         *lo = (*lo << 1) | (n & 0x1);
1395     }
1396     return i;
1397 }
1398
1399 inline uint32_t bitcount32(uint32_t a) {
1400 #if defined __GNUC__
1401     return __builtin_popcountl(a);
1402 #else
1403     a = a - ((a >> 1) & 0x55555555);
1404     a = (a & 0x33333333) + ((a >> 2) & 0x33333333);
1405     return (((a + (a >> 4)) & 0x0f0f0f0f) * 0x01010101) >> 24;
1406 #endif
1407 }
1408
1409 inline uint64_t bitcount64(uint64_t a) {
1410 #if defined __GNUC__
1411     return __builtin_popcountll(a);
1412 #else
1413     PrintAndLogEx(FAILED, "Was not compiled with fct bitcount64");
1414     return 0;
1415 #endif
1416 }
1417
1418 inline uint32_t leadingzeros32(uint32_t a) {
1419 #if defined __GNUC__
1420     return __builtin_clz(a);
1421 #else
1422     PrintAndLogEx(FAILED, "Was not compiled with fct bitcount64");
1423     return 0;
1424 #endif
1425 }
1426
1427 inline uint64_t leadingzeros64(uint64_t a) {
1428 #if defined __GNUC__
1429     return __builtin_clzll(a);
1430 #else
1431     PrintAndLogEx(FAILED, "Was not compiled with fct bitcount64");
1432     return 0;
1433 #endif
1434 }
1435
1436
1437 // byte_strstr searches for the first occurrence of pattern in src
1438 // returns the byte offset the pattern is found at, or -1 if not found
1439 int byte_strstr(const uint8_t *src, size_t srclen, const uint8_t *pattern, size_t plen) {
1440
1441     size_t max = srclen - plen + 1;
1442
1443     for (size_t i = 0; i < max; i++) {
1444
1445         // compare only first byte
1446         if (src[i] != pattern[0])
1447             continue;
1448
1449         // try to match rest of the pattern
1450         for (int j = plen - 1; j >= 1; j--) {
1451
1452             if (src[i + j] != pattern[j])
1453                 break;
1454
1455             if (j == 1)
1456                 return i;
1457         }
1458     }
1459     return -1;
1460 }
1461
1462 // byte_strrstr is like byte_strstr except searches in reverse
1463 // ie it returns the last occurrence of the pattern in src instead of the first
1464 // returns the byte offset the pattern is found at, or -1 if not found
1465 int byte_strrstr(const uint8_t *src, size_t srclen, const uint8_t *pattern, size_t plen) {
1466     for (int i = srclen - plen; i >= 0; i--) {
1467         // compare only first byte
1468         if (src[i] != pattern[0])
1469             continue;
1470
1471         // try to match rest of the pattern
1472         for (int j = plen - 1; j >= 1; j--) {
1473
1474             if (src[i + j] != pattern[j])
1475                 break;
1476
1477             if (j == 1)
1478                 return i;
1479         }
1480     }
1481     return -1;
1482 }
1483
1484 void sb_append_char(smartbuf *sb, unsigned char c) {
1485     if (sb->idx >= sb->size) {
1486         sb->size *= 2;
1487         sb->ptr = realloc(sb->ptr, sb->size);
1488     }
1489     sb->ptr[sb->idx] = c;
1490     sb->idx++;
1491 }
1492
1493 uint8_t get_highest_frequency(const uint8_t *d, uint8_t n) {
1494
1495     uint8_t frequency[256] = {0};
1496     uint8_t highest = 0;
1497     uint8_t v = 0;
1498
1499     // Count the frequency of each byte
1500     for (uint8_t i = 0; i < n; i++) {
1501         frequency[d[i]]++;
1502
1503         if (frequency[d[i]] > highest) {
1504             highest = frequency[d[i]];
1505             v = d[i];
1506         }
1507     }
1508     PrintAndLogEx(DEBUG, "highest occurance... %u  xor byte... 0x%02X", highest, v);
1509     return v;
1510 }