common/commonutil.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 // Utility functions used in many places, not specific to any piece of code.
  17 //-----------------------------------------------------------------------------
  18 #include "commonutil.h"
  19 #include <string.h>
  20
  21 /* Similar to FpgaGatherVersion this formats stored version information
  22  * into a string representation. It takes a pointer to the struct version_information_t,
  23  * verifies the magic properties, then stores a formatted string, prefixed by
  24  * prefix in dst.
  25  */
  26 void FormatVersionInformation(char *dst, int len, const char *prefix, const void *version_info) {
  27     const struct version_information_t *v = (const struct version_information_t *)version_info;
  28     dst[0] = 0;
  29     strncat(dst, prefix, len - 1);
  30     if (v->magic != VERSION_INFORMATION_MAGIC) {
  31         strncat(dst, "Missing/Invalid version information", len - strlen(dst) - 1);
  32         return;
  33     }
  34     if (v->versionversion != 1) {
  35         strncat(dst, "Version information not understood", len - strlen(dst) - 1);
  36         return;
  37     }
  38     if (!v->present) {
  39         strncat(dst, "Version information not available", len - strlen(dst) - 1);
  40         return;
  41     }
  42
  43     strncat(dst, v->gitversion, len - strlen(dst) - 1);
  44     if (v->clean == 0) {
  45         strncat(dst, "-unclean", len - strlen(dst) - 1);
  46     } else if (v->clean == 2) {
  47         strncat(dst, "-suspect", len - strlen(dst) - 1);
  48     }
  49
  50     strncat(dst, " ", len - strlen(dst) - 1);
  51     strncat(dst, v->buildtime, len - strlen(dst) - 1);
  52     strncat(dst, " ", len - strlen(dst) - 1);
  53     strncat(dst, v->armsrc, len - strlen(dst) - 1);
  54 }
  55
  56 void format_version_information_short(char *dst, int len, const void *version_info) {
  57     const struct version_information_t *v = (const struct version_information_t *)version_info;
  58     dst[0] = 0;
  59     if (v->magic != VERSION_INFORMATION_MAGIC) {
  60         strncat(dst, "Missing/Invalid version information", len - strlen(dst) - 1);
  61         return;
  62     }
  63     if (v->versionversion != 1) {
  64         strncat(dst, "Version information not understood", len - strlen(dst) - 1);
  65         return;
  66     }
  67     if (!v->present) {
  68         strncat(dst, "Version information not available", len - strlen(dst) - 1);
  69         return;
  70     }
  71
  72     strncat(dst, v->gitversion, len - strlen(dst) - 1);
  73     strncat(dst, " ", len - strlen(dst) - 1);
  74     strncat(dst, v->buildtime, len - strlen(dst) - 1);
  75 }
  76
  77 /*
  78  ref  http://www.csm.ornl.gov/~dunigan/crc.html
  79  Returns the value v with the bottom b [0,32] bits reflected.
  80  Example: reflect(0x3e23L,3) == 0x3e26
  81 */
  82 uint32_t reflect(uint32_t v, int b) {
  83     uint32_t t = v;
  84     for (int i = 0; i < b; ++i) {
  85         if (t & 1)
  86             v |=  BITMASK((b - 1) - i);
  87         else
  88             v &= ~BITMASK((b - 1) - i);
  89         t >>= 1;
  90     }
  91     return v;
  92 }
  93
  94 // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
  95
  96 // Reverse the bits in a byte with 3 operations (64-bit multiply and modulus division):
  97 uint8_t reflect8(uint8_t b) {
  98     return (b * 0x0202020202ULL & 0x010884422010ULL) % 1023;
  99 }
 100
 101
 102 // Reverse the bits in a byte with 4 operations (64-bit multiply, no division):
 103 /*
 104 uint8_t reflect8(uint8_t b) {
 105     return ((b * 0x80200802ULL) & 0x0884422110ULL) * 0x0101010101ULL >> 32;
 106 }
 107 */
 108
 109 uint16_t reflect16(uint16_t b) {
 110     uint16_t v = 0;
 111     v |= (b & 0x8000) >> 15;
 112     v |= (b & 0x4000) >> 13;
 113     v |= (b & 0x2000) >> 11;
 114     v |= (b & 0x1000) >> 9;
 115     v |= (b & 0x0800) >> 7;
 116     v |= (b & 0x0400) >> 5;
 117     v |= (b & 0x0200) >> 3;
 118     v |= (b & 0x0100) >> 1;
 119
 120     v |= (b & 0x0080) << 1;
 121     v |= (b & 0x0040) << 3;
 122     v |= (b & 0x0020) << 5;
 123     v |= (b & 0x0010) << 7;
 124     v |= (b & 0x0008) << 9;
 125     v |= (b & 0x0004) << 11;
 126     v |= (b & 0x0002) << 13;
 127     v |= (b & 0x0001) << 15;
 128     return v;
 129 }
 130
 131 uint32_t reflect32(uint32_t b) {
 132     // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
 133     uint32_t v = b; // 32-bit word to reverse bit order
 134     // swap odd and even bits
 135     v = ((v >> 1) & 0x55555555) | ((v & 0x55555555) << 1);
 136     // swap consecutive pairs
 137     v = ((v >> 2) & 0x33333333) | ((v & 0x33333333) << 2);
 138     // swap nibbles ...
 139     v = ((v >> 4) & 0x0F0F0F0F) | ((v & 0x0F0F0F0F) << 4);
 140     // swap bytes
 141     v = ((v >> 8) & 0x00FF00FF) | ((v & 0x00FF00FF) << 8);
 142     // swap 2-byte long pairs
 143     v = (v >> 16) | (v               << 16);
 144     return v;
 145 }
 146
 147 uint64_t reflect64(uint64_t b) {
 148     // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
 149     uint64_t v = b; // 32-bit word to reverse bit order
 150     // swap 2-byte long pairs
 151     uint64_t v1 = reflect32(v >> 32);
 152     uint64_t v2 = reflect32(v);
 153     v = (v1 << 32) | (v2 & 0xFFFFFFFF);
 154     return v;
 155 }
 156
 157 void num_to_bytes(uint64_t n, size_t len, uint8_t *dest) {
 158     while (len--) {
 159         dest[len] = (uint8_t) n;
 160         n >>= 8;
 161     }
 162 }
 163
 164 uint64_t bytes_to_num(const uint8_t *src, size_t len) {
 165     uint64_t num = 0;
 166     while (len--) {
 167         num = (num << 8) | (*src);
 168         src++;
 169     }
 170     return num;
 171 }
 172
 173 uint16_t MemLeToUint2byte(const uint8_t *data) {
 174     return (uint16_t)(
 175                (((uint16_t)(data[1])) << 8) +
 176                (((uint16_t)(data[0])) << 0)
 177            );
 178 }
 179
 180 uint32_t MemLeToUint3byte(const uint8_t *data) {
 181     return (uint32_t)(
 182                (((uint32_t)(data[2])) << 16) +
 183                (((uint32_t)(data[1])) << 8) +
 184                (((uint32_t)(data[0])) << 0)
 185            );
 186 }
 187
 188 uint32_t MemLeToUint4byte(const uint8_t *data) {
 189     return (uint32_t)(
 190                (((uint32_t)(data[3])) << 24) +
 191                (((uint32_t)(data[2])) << 16) +
 192                (((uint32_t)(data[1])) << 8) +
 193                (((uint32_t)(data[0])) << 0)
 194            );
 195 }
 196
 197 uint64_t MemLeToUint5byte(const uint8_t *data) {
 198     return (uint64_t)(
 199                (((uint64_t)(data[4])) << 32) +
 200                (((uint64_t)(data[3])) << 24) +
 201                (((uint64_t)(data[2])) << 16) +
 202                (((uint64_t)(data[1])) << 8) +
 203                (((uint64_t)(data[0])) << 0)
 204            );
 205 }
 206
 207 uint64_t MemLeToUint6byte(const uint8_t *data) {
 208     return (uint64_t)(
 209                (((uint64_t)(data[5])) << 40) +
 210                (((uint64_t)(data[4])) << 32) +
 211                (((uint64_t)(data[3])) << 24) +
 212                (((uint64_t)(data[2])) << 16) +
 213                (((uint64_t)(data[1])) << 8) +
 214                (((uint64_t)(data[0])) << 0)
 215            );
 216 }
 217
 218 uint64_t MemLeToUint7byte(const uint8_t *data) {
 219     return (uint64_t)(
 220                (((uint64_t)(data[6])) << 48) +
 221                (((uint64_t)(data[5])) << 40) +
 222                (((uint64_t)(data[4])) << 32) +
 223                (((uint64_t)(data[3])) << 24) +
 224                (((uint64_t)(data[2])) << 16) +
 225                (((uint64_t)(data[1])) << 8) +
 226                (((uint64_t)(data[0])) << 0)
 227            );
 228 }
 229
 230 uint64_t MemLeToUint8byte(const uint8_t *data) {
 231     return (uint64_t)(
 232                (((uint64_t)(data[7])) << 56) +
 233                (((uint64_t)(data[6])) << 48) +
 234                (((uint64_t)(data[5])) << 40) +
 235                (((uint64_t)(data[4])) << 32) +
 236                (((uint64_t)(data[3])) << 24) +
 237                (((uint64_t)(data[2])) << 16) +
 238                (((uint64_t)(data[1])) << 8) +
 239                (((uint64_t)(data[0])) << 0)
 240            );
 241 }
 242
 243 uint16_t MemBeToUint2byte(const uint8_t *data) {
 244     return (uint16_t)(
 245                (((uint16_t)(data[0])) << 8) +
 246                (((uint16_t)(data[1])) << 0)
 247            );
 248 }
 249
 250 uint32_t MemBeToUint3byte(const uint8_t *data) {
 251     return (uint32_t)(
 252                (((uint32_t)(data[0])) << 16) +
 253                (((uint32_t)(data[1])) << 8) +
 254                (((uint32_t)(data[2])) << 0)
 255            );
 256 }
 257
 258 uint32_t MemBeToUint4byte(const uint8_t *data) {
 259     return (uint32_t)(
 260                (((uint32_t)(data[0])) << 24) +
 261                (((uint32_t)(data[1])) << 16) +
 262                (((uint32_t)(data[2])) << 8) +
 263                (((uint32_t)(data[3])) << 0)
 264            );
 265 }
 266
 267 uint64_t MemBeToUint5byte(const uint8_t *data) {
 268     return (uint64_t)(
 269                (((uint64_t)(data[0])) << 32) +
 270                (((uint64_t)(data[1])) << 24) +
 271                (((uint64_t)(data[2])) << 16) +
 272                (((uint64_t)(data[3])) << 8) +
 273                (((uint64_t)(data[4])) << 0)
 274            );
 275 }
 276
 277 uint64_t MemBeToUint6byte(const uint8_t *data) {
 278     return (uint64_t)(
 279                (((uint64_t)(data[0])) << 40) +
 280                (((uint64_t)(data[1])) << 32) +
 281                (((uint64_t)(data[2])) << 24) +
 282                (((uint64_t)(data[3])) << 16) +
 283                (((uint64_t)(data[4])) << 8) +
 284                (((uint64_t)(data[5])) << 0)
 285            );
 286 }
 287
 288 uint64_t MemBeToUint7byte(const uint8_t *data) {
 289     return (uint64_t)(
 290                (((uint64_t)(data[0])) << 48) +
 291                (((uint64_t)(data[1])) << 40) +
 292                (((uint64_t)(data[2])) << 32) +
 293                (((uint64_t)(data[3])) << 24) +
 294                (((uint64_t)(data[4])) << 16) +
 295                (((uint64_t)(data[5])) << 8) +
 296                (((uint64_t)(data[6])) << 0)
 297            );
 298 }
 299
 300 uint64_t MemBeToUint8byte(const uint8_t *data) {
 301     return (uint64_t)(
 302                (((uint64_t)(data[0])) << 56) +
 303                (((uint64_t)(data[1])) << 48) +
 304                (((uint64_t)(data[2])) << 40) +
 305                (((uint64_t)(data[3])) << 32) +
 306                (((uint64_t)(data[4])) << 24) +
 307                (((uint64_t)(data[5])) << 16) +
 308                (((uint64_t)(data[6])) << 8) +
 309                (((uint64_t)(data[7])) << 0)
 310            );
 311 }
 312
 313 void Uint2byteToMemLe(uint8_t *data, uint16_t value) {
 314     data[0] = (uint8_t)((value >> 0) & 0xffu);
 315     data[1] = (uint8_t)((value >> 8) & 0xffu);
 316 }
 317
 318 void Uint3byteToMemLe(uint8_t *data, uint32_t value) {
 319     data[0] = (uint8_t)((value >> 0) & 0xffu);
 320     data[1] = (uint8_t)((value >> 8) & 0xffu);
 321     data[2] = (uint8_t)((value >> 16) & 0xffu);
 322 }
 323
 324 void Uint4byteToMemLe(uint8_t *data, uint32_t value) {
 325     data[0] = (uint8_t)((value >> 0) & 0xffu);
 326     data[1] = (uint8_t)((value >> 8) & 0xffu);
 327     data[2] = (uint8_t)((value >> 16) & 0xffu);
 328     data[3] = (uint8_t)((value >> 24) & 0xffu);
 329 }
 330
 331 void Uint5byteToMemLe(uint8_t *data, uint64_t value) {
 332     data[0] = (uint8_t)((value >> 0) & 0xffu);
 333     data[1] = (uint8_t)((value >> 8) & 0xffu);
 334     data[2] = (uint8_t)((value >> 16) & 0xffu);
 335     data[3] = (uint8_t)((value >> 24) & 0xffu);
 336     data[4] = (uint8_t)((value >> 32) & 0xffu);
 337 }
 338
 339 void Uint6byteToMemLe(uint8_t *data, uint64_t value) {
 340     data[0] = (uint8_t)((value >> 0) & 0xffu);
 341     data[1] = (uint8_t)((value >> 8) & 0xffu);
 342     data[2] = (uint8_t)((value >> 16) & 0xffu);
 343     data[3] = (uint8_t)((value >> 24) & 0xffu);
 344     data[4] = (uint8_t)((value >> 32) & 0xffu);
 345     data[5] = (uint8_t)((value >> 40) & 0xffu);
 346 }
 347
 348 void Uint7byteToMemLe(uint8_t *data, uint64_t value) {
 349     data[0] = (uint8_t)((value >> 0) & 0xffu);
 350     data[1] = (uint8_t)((value >> 8) & 0xffu);
 351     data[2] = (uint8_t)((value >> 16) & 0xffu);
 352     data[3] = (uint8_t)((value >> 24) & 0xffu);
 353     data[4] = (uint8_t)((value >> 32) & 0xffu);
 354     data[5] = (uint8_t)((value >> 40) & 0xffu);
 355     data[6] = (uint8_t)((value >> 48) & 0xffu);
 356 }
 357
 358 void Uint8byteToMemLe(uint8_t *data, uint64_t value) {
 359     data[0] = (uint8_t)((value >> 0) & 0xffu);
 360     data[1] = (uint8_t)((value >> 8) & 0xffu);
 361     data[2] = (uint8_t)((value >> 16) & 0xffu);
 362     data[3] = (uint8_t)((value >> 24) & 0xffu);
 363     data[4] = (uint8_t)((value >> 32) & 0xffu);
 364     data[5] = (uint8_t)((value >> 40) & 0xffu);
 365     data[6] = (uint8_t)((value >> 48) & 0xffu);
 366     data[7] = (uint8_t)((value >> 56) & 0xffu);
 367 }
 368
 369 void Uint2byteToMemBe(uint8_t *data, uint16_t value) {
 370     data[0] = (uint8_t)((value >> 8) & 0xffu);
 371     data[1] = (uint8_t)((value >> 0) & 0xffu);
 372 }
 373
 374 void Uint3byteToMemBe(uint8_t *data, uint32_t value) {
 375     data[0] = (uint8_t)((value >> 16) & 0xffu);
 376     data[1] = (uint8_t)((value >> 8) & 0xffu);
 377     data[2] = (uint8_t)((value >> 0) & 0xffu);
 378 }
 379
 380 void Uint4byteToMemBe(uint8_t *data, uint32_t value) {
 381     data[0] = (uint8_t)((value >> 24) & 0xffu);
 382     data[1] = (uint8_t)((value >> 16) & 0xffu);
 383     data[2] = (uint8_t)((value >> 8) & 0xffu);
 384     data[3] = (uint8_t)((value >> 0) & 0xffu);
 385 }
 386
 387 void Uint5byteToMemBe(uint8_t *data, uint64_t value) {
 388     data[0] = (uint8_t)((value >> 32) & 0xffu);
 389     data[1] = (uint8_t)((value >> 24) & 0xffu);
 390     data[2] = (uint8_t)((value >> 16) & 0xffu);
 391     data[3] = (uint8_t)((value >> 8) & 0xffu);
 392     data[4] = (uint8_t)((value >> 0) & 0xffu);
 393 }
 394
 395 void Uint6byteToMemBe(uint8_t *data, uint64_t value) {
 396     data[0] = (uint8_t)((value >> 40) & 0xffu);
 397     data[1] = (uint8_t)((value >> 32) & 0xffu);
 398     data[2] = (uint8_t)((value >> 24) & 0xffu);
 399     data[3] = (uint8_t)((value >> 16) & 0xffu);
 400     data[4] = (uint8_t)((value >> 8) & 0xffu);
 401     data[5] = (uint8_t)((value >> 0) & 0xffu);
 402 }
 403
 404 void Uint7byteToMemBe(uint8_t *data, uint64_t value) {
 405     data[0] = (uint8_t)((value >> 48) & 0xffu);
 406     data[1] = (uint8_t)((value >> 40) & 0xffu);
 407     data[2] = (uint8_t)((value >> 32) & 0xffu);
 408     data[3] = (uint8_t)((value >> 24) & 0xffu);
 409     data[4] = (uint8_t)((value >> 16) & 0xffu);
 410     data[5] = (uint8_t)((value >> 8) & 0xffu);
 411     data[6] = (uint8_t)((value >> 0) & 0xffu);
 412 }
 413
 414 void Uint8byteToMemBe(uint8_t *data, uint64_t value) {
 415     data[0] = (uint8_t)((value >> 56) & 0xffu);
 416     data[1] = (uint8_t)((value >> 48) & 0xffu);
 417     data[2] = (uint8_t)((value >> 40) & 0xffu);
 418     data[3] = (uint8_t)((value >> 32) & 0xffu);
 419     data[4] = (uint8_t)((value >> 24) & 0xffu);
 420     data[5] = (uint8_t)((value >> 16) & 0xffu);
 421     data[6] = (uint8_t)((value >> 8) & 0xffu);
 422     data[7] = (uint8_t)((value >> 0) & 0xffu);
 423 }
 424
 425 // Rotate Left - Ultralight, Desfire
 426 void rol(uint8_t *data, const size_t len) {
 427     uint8_t first = data[0];
 428     for (size_t i = 0; i < len - 1; i++) {
 429         data[i] = data[i + 1];
 430     }
 431     data[len - 1] = first;
 432 }
 433
 434 // Rotate Right - Ultralight, Desfire
 435 void ror(uint8_t *data, const size_t len) {
 436     uint8_t last = data[len - 1];
 437
 438     for (int i = len - 1; i > 0; i--) {
 439         data[i] = data[i - 1];
 440     }
 441
 442     data[0] = last;
 443 }
 444
 445
 446 void lsl(uint8_t *data, size_t len) {
 447     for (size_t n = 0; n < len - 1; n++) {
 448         data[n] = (data[n] << 1) | (data[n + 1] >> 7);
 449     }
 450     data[len - 1] <<= 1;
 451 }
 452
 453
 454 // BSWAP24 of array[3]
 455 uint32_t le24toh(const uint8_t data[3]) {
 456     return (data[2] << 16) | (data[1] << 8) | data[0];
 457 }
 458
 459 // BSWAP24, take u32, output array
 460 void htole24(uint32_t val, uint8_t data[3]) {
 461     data[0] = (uint8_t) val;
 462     data[1] = (uint8_t)(val >> 8);
 463     data[2] = (uint8_t)(val >> 16);
 464 }
 465
 466
 467 // ROL on u32
 468 uint32_t rotl(uint32_t a, uint8_t n) {
 469     n &= 31;
 470     return (a << n) | (a >> (32 - n));
 471 }
 472
 473 // ROR on u32
 474 uint32_t rotr(uint32_t a, uint8_t n) {
 475     n &= 31;
 476     return (a >> n) | (a << (32 - n));
 477 }
 478
 479 uint16_t get_sw(const uint8_t *d, uint16_t n) {
 480     if (n < 2)
 481         return 0;
 482
 483     n -= 2;
 484     return (d[n] << 8 | d[n + 1]);
 485 }
 486
 487 // reverse same array
 488 void reverse_array(uint8_t *d, size_t n) {
 489     if (d == NULL || n < 2) {
 490         return;
 491     }
 492
 493     for (int i = 0, j = n - 1; i < j; ++i, --j) {
 494         d[i] ^= d[j];
 495         d[j] ^= d[i];
 496         d[i] ^= d[j];
 497     }
 498 }
 499
 500 // reverse src array into dest array
 501 void reverse_array_copy(const uint8_t *src, int src_len, uint8_t *dest) {
 502     if (src == NULL || src_len == 0 || dest == NULL) {
 503         return;
 504     }
 505
 506     for (int i = 0; i < src_len; i++) {
 507         dest[i] = src[(src_len - 1) - i];
 508     }
 509 }
 510
 511 static int hexchar_to_dec(char ch) {
 512     if (ch >= '0' && ch <= '9') {
 513         return ch - '0';
 514     }
 515     if (ch >= 'a' && ch <= 'f') {
 516         return ch - 'a' + 10;
 517     }
 518     if (ch >= 'A' && ch <= 'F') {
 519         return ch - 'A' + 10;
 520     }
 521     return -1;
 522 }
 523
 524 // no spaces allowed for input hex string
 525 bool hexstr_to_byte_array(const char *hexstr, uint8_t *d, size_t *n) {
 526
 527     size_t hexstr_len = strlen(hexstr);
 528     if (hexstr_len & 1) {
 529         return false;
 530     }
 531
 532     *n = (hexstr_len >> 1);
 533
 534     for (int i = 0; i < *n; i++) {
 535
 536         char c1 = *hexstr++;
 537         char c2 = *hexstr++;
 538
 539         if (c1 == '\0' || c2 == '\0') {
 540             return false;
 541         }
 542
 543         int b = (hexchar_to_dec(c1) << 4) | hexchar_to_dec(c2);
 544         if (b < 0) {
 545             // Error: invalid hex character
 546             return false;
 547         }
 548         d[i] = (uint8_t) b;
 549     }
 550     return true;
 551 }
 552
 553 void reverse_arraybytes(uint8_t *arr, size_t len) {
 554     size_t i;
 555     for (i = 0; i < len ; i++) {
 556         arr[i] = reflect8(arr[i]);
 557     }
 558 }
 559
 560 void reverse_arraybytes_copy(uint8_t *arr, uint8_t *dest, size_t len) {
 561     size_t i;
 562     for (i = 0; i < len ; i++) {
 563         dest[i] = reflect8(arr[i]);
 564     }
 565 }