client/src/cmdlffdxb.c

   1 //-----------------------------------------------------------------------------
   2 //
   3 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   4 // at your option, any later version. See the LICENSE.txt file for the text of
   5 // the license.
   6 //-----------------------------------------------------------------------------
   7 // Low frequency fdx-b tag commands
   8 // Differential Biphase, rf/32, 128 bits (known)
   9 //-----------------------------------------------------------------------------
  10
  11 #include "cmdlffdxb.h"
  12 #include <inttypes.h>
  13 #include <string.h>
  14 #include <stdlib.h>
  15 #include <ctype.h>        // tolower
  16 #include "cmdparser.h"    // command_t
  17 #include "comms.h"
  18 #include "commonutil.h"
  19 #include "ui.h"           // PrintAndLog
  20 #include "cmddata.h"
  21 #include "cmdlf.h"        // lf read
  22 #include "crc16.h"        // for checksum crc-16_ccitt
  23 #include "protocols.h"    // for T55xx config register definitions
  24 #include "lfdemod.h"      // parityTest
  25 #include "cmdlft55xx.h"   // verifywrite
  26 #include "cliparser.h"
  27 #include "cmdlfem4x05.h"  // EM defines
  28
  29 /*
  30     FDX-B ISO11784/85 demod  (aka animal tag)  BIPHASE, inverted, rf/32,  with preamble of 00000000001 (128bits)
  31     8 databits + 1 parity (1)
  32     CIITT 16 checksum
  33     NATIONAL CODE, ICAR database
  34     COUNTRY CODE (ISO3166) or http://cms.abvma.ca/uploads/ManufacturersISOsandCountryCodes.pdf
  35     FLAG (animal/non-animal)
  36
  37     38 IDbits
  38     10 country code
  39     1 extra app bit
  40     14 reserved bits
  41     1 animal bit
  42     16 ccitt CRC chksum over 64bit ID CODE.
  43     24 appli bits.
  44
  45     sample: 985121004515220  [ 37FF65B88EF94 ]
  46 */
  47
  48 static int CmdHelp(const char *Cmd);
  49
  50 static int getFDXBBits(uint64_t national_code, uint16_t country_code, uint8_t is_animal, uint8_t is_extended, uint32_t extended, uint8_t *bits) {
  51
  52     // add preamble ten 0x00 and one 0x01
  53     memset(bits, 0x00, 10);
  54     bits[10] = 1;
  55
  56     // 128bits
  57     // every 9th bit is 0x01, but we can just fill the rest with 0x01 and overwrite
  58     memset(bits, 0x01, 128);
  59
  60     // add preamble ten 0x00 and one 0x01
  61     memset(bits, 0x00, 10);
  62
  63     // add reserved
  64     num_to_bytebitsLSBF(0x00, 7, bits + 66);
  65     num_to_bytebitsLSBF(0x00 >> 7, 7, bits + 74);
  66
  67     // add animal flag - OK
  68     bits[81] = is_animal;
  69
  70     // add extended flag - OK
  71     bits[65] = is_extended;
  72
  73     // add national code 40bits - OK
  74     num_to_bytebitsLSBF(national_code >> 0, 8, bits + 11);
  75     num_to_bytebitsLSBF(national_code >> 8, 8, bits + 20);
  76     num_to_bytebitsLSBF(national_code >> 16, 8, bits + 29);
  77     num_to_bytebitsLSBF(national_code >> 24, 8, bits + 38);
  78     num_to_bytebitsLSBF(national_code >> 32, 6, bits + 47);
  79
  80     // add country code - OK
  81     num_to_bytebitsLSBF(country_code >> 0, 2, bits + 53);
  82     num_to_bytebitsLSBF(country_code >> 2, 8, bits + 56);
  83
  84     // add crc-16 - OK
  85     uint8_t raw[8];
  86     for (uint8_t i = 0; i < 8; ++i)
  87         raw[i] = bytebits_to_byte(bits + 11 + i * 9, 8);
  88
  89     init_table(CRC_11784);
  90     uint16_t crc = crc16_fdxb(raw, 8);
  91     num_to_bytebitsLSBF(crc >> 0, 8, bits + 83);
  92     num_to_bytebitsLSBF(crc >> 8, 8, bits + 92);
  93
  94     // extended data - OK
  95     num_to_bytebitsLSBF(extended >> 0, 8, bits + 101);
  96     num_to_bytebitsLSBF(extended >> 8, 8, bits + 110);
  97     num_to_bytebitsLSBF(extended >> 16, 8, bits + 119);
  98
  99     // 8  16 24 32 40 48 49
 100     // A8 28 0C 92 EA 6F 00 01
 101     // A8 28 0C 92 EA 6F 80 00
 102     return PM3_SUCCESS;
 103 }
 104
 105 // clearing the topbit needed for the preambl detection.
 106 static void verify_values(uint64_t *animalid, uint32_t *countryid, uint32_t *extended) {
 107     if ((*animalid & 0x3FFFFFFFFF) != *animalid) {
 108         *animalid &= 0x3FFFFFFFFF;
 109         PrintAndLogEx(INFO, "Animal ID truncated to 38bits: " _YELLOW_("%"PRIx64), *animalid);
 110     }
 111     if ((*countryid & 0x3FF) != *countryid) {
 112         *countryid &= 0x3FF;
 113         PrintAndLogEx(INFO, "Country ID truncated to 10bits:" _YELLOW_("%03d"), *countryid);
 114     }
 115     if ((*extended & 0xFFFFFF) != *extended) {
 116         *extended &= 0xFFFFFF;
 117         PrintAndLogEx(INFO, "Extended truncated to 24bits: " _YELLOW_("0x%03X"), *extended);
 118     }
 119 }
 120
 121 // FDX-B ISO11784/85 demod  (aka animal tag)  BIPHASE, inverted, rf/32,  with preamble of 00000000001 (128bits)
 122 // 8 databits + 1 parity (1)
 123 // CIITT 16 chksum
 124 // NATIONAL CODE, ICAR database
 125 // COUNTRY CODE (ISO3166) or http://cms.abvma.ca/uploads/ManufacturersISOsandCountryCodes.pdf
 126 // FLAG (animal/non-animal)
 127 /*
 128 38 IDbits
 129 10 country code
 130 1 extra app bit
 131 14 reserved bits
 132 1 animal bit
 133 16 ccitt CRC chksum over 64bit ID CODE.
 134 24 appli bits.
 135
 136 -- sample: 985121004515220  [ 37FF65B88EF94 ]
 137 */
 138 /*
 139 static int CmdFDXBdemodBI(const char *Cmd) {
 140     (void)Cmd; // Cmd is not used so far
 141
 142     int clk = 32;
 143     int invert = 1, errCnt = 0, offset = 0, maxErr = 100;
 144     uint8_t bs[MAX_DEMOD_BUF_LEN];
 145     size_t size = getFromGraphBuf(bs);
 146
 147     errCnt = askdemod(bs, &size, &clk, &invert, maxErr, 0, 0);
 148     if (errCnt < 0 || errCnt > maxErr) {
 149         PrintAndLogEx(DEBUG, "DEBUG: Error - FDXB no data or error found %d, clock: %d", errCnt, clk);
 150         return PM3_ESOFT;
 151     }
 152
 153     errCnt = BiphaseRawDecode(bs, &size, &offset, 1);
 154     if (errCnt < 0 || errCnt > maxErr) {
 155         PrintAndLogEx(DEBUG, "DEBUG: Error - FDXB BiphaseRawDecode: %d", errCnt);
 156         return PM3_ESOFT;
 157     }
 158
 159     int preambleIndex = detectFDXB(bs, &size);
 160     if (preambleIndex < 0) {
 161         PrintAndLogEx(DEBUG, "DEBUG: Error - FDXB preamble not found :: %d", preambleIndex);
 162         return PM3_ESOFT;
 163     }
 164     if (size != 128) {
 165         PrintAndLogEx(DEBUG, "DEBUG: Error - FDXB incorrect data length found");
 166         return PM3_ESOFT;
 167     }
 168
 169     setDemodBuff(bs, 128, preambleIndex);
 170
 171     // remove marker bits (1's every 9th digit after preamble) (pType = 2)
 172     size = removeParity(bs, preambleIndex + 11, 9, 2, 117);
 173     if (size != 104) {
 174         PrintAndLogEx(DEBUG, "DEBUG: Error - FDXB error removeParity:: %d", size);
 175         return PM3_ESOFT;
 176     }
 177     PrintAndLogEx(SUCCESS, "\nFDX-B / ISO 11784/5 Animal Tag ID Found:");
 178
 179     //got a good demod
 180     uint64_t NationalCode = ((uint64_t)(bytebits_to_byteLSBF(bs + 32, 6)) << 32) | bytebits_to_byteLSBF(bs, 32);
 181     uint32_t countryCode = bytebits_to_byteLSBF(bs + 38, 10);
 182     uint8_t dataBlockBit = bs[48];
 183     uint32_t reservedCode = bytebits_to_byteLSBF(bs + 49, 14);
 184     uint8_t animalBit = bs[63];
 185     uint32_t crc_16 = bytebits_to_byteLSBF(bs + 64, 16);
 186     uint32_t extended = bytebits_to_byteLSBF(bs + 80, 24);
 187
 188     uint64_t rawid = ((uint64_t)bytebits_to_byte(bs, 32) << 32) | bytebits_to_byte(bs + 32, 32);
 189     uint8_t raw[8];
 190     num_to_bytes(rawid, 8, raw);
 191
 192     PrintAndLogEx(SUCCESS, "Raw ID Hex: %s", sprint_hex(raw, 8));
 193
 194     uint16_t calcCrc = crc16_kermit(raw, 8);
 195     PrintAndLogEx(SUCCESS, "Animal ID:     %04u-%012" PRIu64, countryCode, NationalCode);
 196     PrintAndLogEx(SUCCESS, "National Code: %012" PRIu64, NationalCode);
 197     PrintAndLogEx(SUCCESS, "CountryCode:   %04u", countryCode);
 198
 199     PrintAndLogEx(SUCCESS, "Reserved/RFU:      %u", reservedCode);
 200     PrintAndLogEx(SUCCESS, "Animal Tag:        %s", animalBit ? _YELLOW_("True") : "False");
 201     PrintAndLogEx(SUCCESS, "Has extended data: %s [0x%X]", dataBlockBit ? _YELLOW_("True") : "False", extended);
 202     PrintAndLogEx(SUCCESS, "CRC:           0x%04X - [%04X] - %s", crc_16, calcCrc, (calcCrc == crc_16) ? _GREEN_("Passed") : _RED_("Fail") );
 203
 204     if (g_debugMode) {
 205         PrintAndLogEx(DEBUG, "Start marker %d;   Size %d", preambleIndex, size);
 206         char *bin = sprint_bin_break(bs, size, 16);
 207         PrintAndLogEx(DEBUG, "DEBUG BinStream:\n%s", bin);
 208     }
 209     return PM3_SUCCESS;
 210 }
 211 */
 212
 213 // For the country part:
 214 // wget -q -O - "https://en.wikipedia.org/w/index.php?title=List_of_ISO_3166_country_codes&action=raw" | awk '/id=/{match($0, /\[\[([^\]|]*)/, a); name=a[1];getline;getline;getline;getline;getline;match($0, /numeric#([0-9]*)/, a);num=a[1]; if (num != "") {printf "    { %3u, \"%s\" },\n", num, name}}'
 215 // Beware the bottom of the list contains also Manufacturers list
 216 const fdxbCountryMapping_t fdxbCountryMapping[] = {
 217     {   4, "Afghanistan" },
 218     {   8, "Albania" },
 219     {  12, "Algeria" },
 220     {  16, "American Samoa" },
 221     {  20, "Andorra" },
 222     {  24, "Angola" },
 223     { 660, "Anguilla" },
 224     {  10, "Antarctica" },
 225     {  28, "Antigua and Barbuda" },
 226     {  32, "Argentina" },
 227     {  51, "Armenia" },
 228     { 533, "Aruba" },
 229     {  40, "Austria" },
 230     {  31, "Azerbaijan" },
 231     {  44, "The Bahamas" },
 232     {  48, "Bahrain" },
 233     {  50, "Bangladesh" },
 234     {  52, "Barbados" },
 235     { 112, "Belarus" },
 236     {  56, "Belgium" },
 237     {  84, "Belize" },
 238     { 204, "Benin" },
 239     {  60, "Bermuda" },
 240     {  64, "Bhutan" },
 241     {  68, "Bolivia" },
 242     { 535, "Bonaire" },
 243     {  70, "Bosnia and Herzegovina" },
 244     {  72, "Botswana" },
 245     {  74, "Bouvet Island" },
 246     {  76, "Brazil" },
 247     {  86, "British Indian Ocean Territory" },
 248     { 100, "Bulgaria" },
 249     { 854, "Burkina Faso" },
 250     { 132, "Cape Verde" },
 251     { 116, "Cambodia" },
 252     { 120, "Cameroon" },
 253     { 124, "Canada" },
 254     { 140, "Central African Republic" },
 255     { 148, "Chad" },
 256     { 152, "Chile" },
 257     { 156, "China" },
 258     { 170, "Colombia" },
 259     { 174, "Comoros" },
 260     { 180, "Democratic Republic of the Congo" },
 261     { 178, "Republic of the Congo" },
 262     { 184, "Cook Islands" },
 263     { 384, "Ivory Coast" },
 264     { 191, "Croatia" },
 265     { 192, "Cuba" },
 266     { 531, "Curaçao" },
 267     { 196, "Cyprus" },
 268     { 203, "Czech Republic" },
 269     { 262, "Djibouti" },
 270     { 212, "Dominica" },
 271     { 214, "Dominican Republic" },
 272     { 818, "Egypt" },
 273     { 222, "El Salvador" },
 274     { 232, "Eritrea" },
 275     { 233, "Estonia" },
 276     { 748, "Eswatini" },
 277     { 231, "Ethiopia" },
 278     { 238, "Falkland Islands" },
 279     { 234, "Faroe Islands" },
 280     { 242, "Fiji" },
 281     { 246, "Finland" },
 282     { 250, "France" },
 283     { 254, "French Guiana" },
 284     { 258, "French Polynesia" },
 285     { 260, "French Southern Territories" },
 286     { 266, "Gabon" },
 287     { 270, "The Gambia" },
 288     { 268, "Georgia (country)" },
 289     { 276, "Germany" },
 290     { 288, "Ghana" },
 291     { 292, "Gibraltar" },
 292     { 304, "Greenland" },
 293     { 308, "Grenada" },
 294     { 312, "Guadeloupe" },
 295     { 316, "Guam" },
 296     { 320, "Guatemala" },
 297     { 831, "Bailiwick of Guernsey" },
 298     { 324, "Guinea" },
 299     { 624, "Guinea-Bissau" },
 300     { 328, "Guyana" },
 301     { 332, "Haiti" },
 302     { 336, "Holy See" },
 303     { 340, "Honduras" },
 304     { 344, "Hong Kong" },
 305     { 348, "Hungary" },
 306     { 352, "Iceland" },
 307     { 356, "India" },
 308     { 360, "Indonesia" },
 309     { 364, "Iran (Islamic Republic of)" },
 310     { 368, "Iraq" },
 311     { 372, "Republic of Ireland" },
 312     { 833, "Isle of Man" },
 313     { 376, "Israel" },
 314     { 380, "Italy" },
 315     { 832, "Jersey" },
 316     { 400, "Jordan" },
 317     { 398, "Kazakhstan" },
 318     { 404, "Kenya" },
 319     { 296, "Kiribati" },
 320     { 408, "North Korea" },
 321     { 410, "South Korea" },
 322     { 414, "Kuwait" },
 323     { 417, "Kyrgyzstan" },
 324     { 418, "Laos" },
 325     { 428, "Latvia" },
 326     { 422, "Lebanon" },
 327     { 426, "Lesotho" },
 328     { 430, "Liberia" },
 329     { 434, "Libya" },
 330     { 438, "Liechtenstein" },
 331     { 440, "Lithuania" },
 332     { 442, "Luxembourg" },
 333     { 446, "Macau" },
 334     { 807, "North Macedonia" },
 335     { 450, "Madagascar" },
 336     { 454, "Malawi" },
 337     { 458, "Malaysia" },
 338     { 462, "Maldives" },
 339     { 466, "Mali" },
 340     { 470, "Malta" },
 341     { 584, "Marshall Islands" },
 342     { 474, "Martinique" },
 343     { 478, "Mauritania" },
 344     { 480, "Mauritius" },
 345     { 175, "Mayotte" },
 346     { 484, "Mexico" },
 347     { 583, "Federated States of Micronesia" },
 348     { 498, "Moldova" },
 349     { 492, "Monaco" },
 350     { 496, "Mongolia" },
 351     { 499, "Montenegro" },
 352     { 500, "Montserrat" },
 353     { 504, "Morocco" },
 354     { 508, "Mozambique" },
 355     { 104, "Myanmar" },
 356     { 516, "Namibia" },
 357     { 520, "Nauru" },
 358     { 524, "Nepal" },
 359     { 528, "Kingdom of the Netherlands" },
 360     { 540, "New Caledonia" },
 361     { 554, "New Zealand" },
 362     { 558, "Nicaragua" },
 363     { 562, "Niger" },
 364     { 566, "Nigeria" },
 365     { 570, "Niue" },
 366     { 574, "Norfolk Island" },
 367     { 578, "Norway" },
 368     { 512, "Oman" },
 369     { 586, "Pakistan" },
 370     { 585, "Palau" },
 371     { 275, "State of Palestine" },
 372     { 591, "Panama" },
 373     { 598, "Papua New Guinea" },
 374     { 600, "Paraguay" },
 375     { 608, "Philippines" },
 376     { 612, "Pitcairn Islands" },
 377     { 616, "Poland" },
 378     { 620, "Portugal" },
 379     { 630, "Puerto Rico" },
 380     { 634, "Qatar" },
 381     { 638, "Réunion" },
 382     { 642, "Romania" },
 383     { 643, "Russia" },
 384     { 646, "Rwanda" },
 385     { 654, "Saint Helena" },
 386     { 659, "Saint Kitts and Nevis" },
 387     { 662, "Saint Lucia" },
 388     { 663, "Collectivity of Saint Martin" },
 389     { 666, "Saint Pierre and Miquelon" },
 390     { 670, "Saint Vincent and the Grenadines" },
 391     { 882, "Samoa" },
 392     { 674, "San Marino" },
 393     { 678, "São Tomé and Príncipe" },
 394     { 682, "Saudi Arabia" },
 395     { 688, "Serbia" },
 396     { 690, "Seychelles" },
 397     { 694, "Sierra Leone" },
 398     { 702, "Singapore" },
 399     { 703, "Slovakia" },
 400     { 705, "Slovenia" },
 401     {  90, "Solomon Islands" },
 402     { 706, "Somalia" },
 403     { 710, "South Africa" },
 404     { 239, "South Georgia and the South Sandwich Islands" },
 405     { 724, "Spain" },
 406     { 144, "Sri Lanka" },
 407     { 729, "Sudan" },
 408     { 740, "Suriname" },
 409     { 744, "Svalbard" },
 410     { 752, "Sweden" },
 411     { 756, "Switzerland" },
 412     { 760, "Syria" },
 413     { 158, "Taiwan" },
 414     { 762, "Tajikistan" },
 415     { 834, "Tanzania" },
 416     { 764, "Thailand" },
 417     { 626, "East Timor" },
 418     { 768, "Togo" },
 419     { 772, "Tokelau" },
 420     { 776, "Tonga" },
 421     { 780, "Trinidad and Tobago" },
 422     { 788, "Tunisia" },
 423     { 792, "Turkey" },
 424     { 795, "Turkmenistan" },
 425     { 796, "Turks and Caicos Islands" },
 426     { 798, "Tuvalu" },
 427     { 800, "Uganda" },
 428     { 804, "Ukraine" },
 429     { 784, "United Arab Emirates" },
 430     { 826, "United Kingdom" },
 431     { 581, "United States Minor Outlying Islands" },
 432     { 840, "United States" },
 433     { 860, "Uzbekistan" },
 434     { 548, "Vanuatu" },
 435     { 704, "Vietnam" },
 436     {  92, "British Virgin Islands" },
 437     { 850, "United States Virgin Islands" },
 438     { 732, "Western Sahara" },
 439     { 887, "Yemen" },
 440     { 894, "Zambia" },
 441     { 716, "Zimbabwe" },
 442
 443     // Manufacturers list:
 444     { 952, "JECTA" },
 445     { 953, "Cromasa Identificacion electronica S.A."},
 446     { 955, "Reseaumatique" },
 447     { 956, "Trovan Ltd. (ACK Reunite)" },
 448     { 958, "Pet ID" },
 449     { 959, "Global ID Technologies" },
 450     { 961, "Mannings I.A.I.D." },
 451     { 963, "Korth Eletro Mecanica LTDA" },
 452     { 965, "4D Technology Co. Ltd" },
 453     { 966, "PetCode" },
 454     { 967, "Rfdynamics / M4S ID in Canada" },
 455     { 968, "AEG / EIDAP in Canada" },
 456     { 972, "Planet ID" },
 457     { 975, "Sokymat" },
 458     { 977, "AVID" },
 459     { 978, "Ordicam" },
 460     { 981, "Microfindr, Datamars, Found Animals, Crystal Tag, Banfield, Bayer resQ, Peeva" },
 461     { 982, "24 Pet Watch (Allflex)" },
 462     { 985, "HomeAgain (Destron Fearing/Digital Angel)" },
 463     { 991, "Peeva" },
 464     { 999, "Test range" },
 465     { 0,   "N/A" } // must be the last entry
 466 };
 467
 468 static const char *mapFDBX(uint16_t countryCode) {
 469     uint16_t i = 0;
 470     while (fdxbCountryMapping[i].code > 0) {
 471         if (countryCode == fdxbCountryMapping[i].code) {
 472             return fdxbCountryMapping[i].desc;
 473         }
 474         i++;
 475     }
 476     return fdxbCountryMapping[i].desc;
 477 }
 478
 479 //see ASKDemod for what args are accepted
 480 //almost the same demod as cmddata.c/CmdFDXBdemodBI
 481 int demodFDXB(bool verbose) {
 482     //Differential Biphase / di-phase (inverted biphase)
 483     //get binary from ask wave
 484     if (ASKbiphaseDemod(0, 32, 1, 100, false) != PM3_SUCCESS) {
 485         PrintAndLogEx(DEBUG, "DEBUG: Error - FDX-B ASKbiphaseDemod failed");
 486         return PM3_ESOFT;
 487     }
 488     size_t size = DemodBufferLen;
 489     int preambleIndex = detectFDXB(DemodBuffer, &size);
 490     if (preambleIndex < 0) {
 491
 492         if (preambleIndex == -1)
 493             PrintAndLogEx(DEBUG, "DEBUG: Error - FDX-B too few bits found");
 494         else if (preambleIndex == -2)
 495             PrintAndLogEx(DEBUG, "DEBUG: Error - FDX-B preamble not found");
 496         else if (preambleIndex == -3)
 497             PrintAndLogEx(DEBUG, "DEBUG: Error - FDX-B Size not correct: %zu", size);
 498         else
 499             PrintAndLogEx(DEBUG, "DEBUG: Error - FDX-B ans: %d", preambleIndex);
 500         return PM3_ESOFT;
 501     }
 502
 503     // set and leave DemodBuffer intact
 504     setDemodBuff(DemodBuffer, 128, preambleIndex);
 505     setClockGrid(g_DemodClock, g_DemodStartIdx + (preambleIndex * g_DemodClock));
 506
 507
 508     // remove marker bits (1's every 9th digit after preamble) (pType = 2)
 509     size = removeParity(DemodBuffer, 11, 9, 2, 117);
 510     if (size != 104) {
 511         PrintAndLogEx(DEBUG, "DEBUG: Error - FDX-B error removeParity: %zu", size);
 512         return PM3_ESOFT;
 513     }
 514
 515     //got a good demod
 516     uint8_t offset;
 517     // ISO: bits 27..64
 518     uint64_t NationalCode = ((uint64_t)(bytebits_to_byteLSBF(DemodBuffer + 32, 6)) << 32) | bytebits_to_byteLSBF(DemodBuffer, 32);
 519
 520     offset = 38;
 521     // ISO: bits 17..26
 522     uint16_t countryCode = bytebits_to_byteLSBF(DemodBuffer + offset, 10);
 523
 524     offset += 10;
 525     // ISO: bits 16
 526     uint8_t dataBlockBit = DemodBuffer[offset];
 527
 528     offset++;
 529     // ISO: bits 15
 530     uint8_t rudiBit = DemodBuffer[offset];
 531
 532     offset++;
 533     // ISO: bits 10..14
 534     uint32_t reservedCode = bytebits_to_byteLSBF(DemodBuffer + offset, 5);
 535
 536     offset += 5;
 537     // ISO: bits 5..9
 538     uint32_t userInfo = bytebits_to_byteLSBF(DemodBuffer + offset, 5);
 539
 540     offset += 5;
 541     // ISO: bits 2..4
 542     uint32_t replacementNr = bytebits_to_byteLSBF(DemodBuffer + offset, 3);
 543
 544     offset += 3;
 545     uint8_t animalBit = DemodBuffer[offset];
 546
 547     offset++;
 548     uint16_t crc = bytebits_to_byteLSBF(DemodBuffer + offset, 16);
 549
 550     offset += 16;
 551     uint32_t extended = bytebits_to_byteLSBF(DemodBuffer + offset, 24);
 552
 553     uint64_t rawid = (uint64_t)(bytebits_to_byte(DemodBuffer, 32)) << 32 | bytebits_to_byte(DemodBuffer + 32, 32);
 554     uint8_t raw[8];
 555     num_to_bytes(rawid, 8, raw);
 556
 557     if (!verbose) {
 558         PROMPT_CLEARLINE;
 559         PrintAndLogEx(SUCCESS, "Animal ID          " _GREEN_("%04u-%012"PRIu64), countryCode, NationalCode);
 560         return PM3_SUCCESS;
 561     }
 562     PrintAndLogEx(SUCCESS, "FDX-B / ISO 11784/5 Animal");
 563     PrintAndLogEx(SUCCESS, "Animal ID          " _GREEN_("%03u-%012"PRIu64), countryCode, NationalCode);
 564     PrintAndLogEx(SUCCESS, "National Code      " _GREEN_("%012" PRIu64) " (0x%" PRIX64 ")", NationalCode, NationalCode);
 565     PrintAndLogEx(SUCCESS, "Country Code       " _GREEN_("%03u") " - %s", countryCode, mapFDBX(countryCode));
 566     PrintAndLogEx(SUCCESS, "Reserved/RFU       %u (0x%04X)", reservedCode,  reservedCode);
 567     PrintAndLogEx(SUCCESS, "  Animal bit set?  %s", animalBit ? _YELLOW_("True") : "False");
 568     PrintAndLogEx(SUCCESS, "      Data block?  %s  [value 0x%X]", dataBlockBit ? _YELLOW_("True") : "False", extended);
 569     PrintAndLogEx(SUCCESS, "        RUDI bit?  %s", rudiBit ? _YELLOW_("True") " (advanced transponder)" : "False");
 570     PrintAndLogEx(SUCCESS, "       User Info?  %u %s", userInfo, userInfo == 0 ? "(RFU)" : "");
 571     PrintAndLogEx(SUCCESS, "  Replacement No?  %u %s", replacementNr, replacementNr == 0 ? "(RFU)" : "");
 572
 573     uint8_t c[] = {0, 0};
 574     compute_crc(CRC_11784, raw, sizeof(raw), &c[0], &c[1]);
 575     PrintAndLogEx(SUCCESS, "CRC-16             0x%04X (%s)", crc, (crc == (c[1] << 8 | c[0])) ? _GREEN_("ok") : _RED_("fail"));
 576     // iceman: crc doesn't protect the extended data?
 577     PrintAndLogEx(SUCCESS, "Raw                " _GREEN_("%s"), sprint_hex(raw, 8));
 578
 579     if (g_debugMode) {
 580         PrintAndLogEx(DEBUG, "Start marker %d;   Size %zu", preambleIndex, size);
 581         char *bin = sprint_bin_break(DemodBuffer, size, 16);
 582         PrintAndLogEx(DEBUG, "DEBUG bin stream:\n%s", bin);
 583     }
 584
 585     uint8_t bt_par = (extended & 0x100) >> 8;
 586     uint8_t bt_temperature = extended & 0xff;
 587     uint8_t bt_calc_parity = (bitcount32(bt_temperature) & 0x1) ? 0 : 1;
 588     uint8_t is_bt_temperature = (bt_calc_parity == bt_par) && !(extended & 0xe00) ;
 589
 590     if (is_bt_temperature) {
 591         float bt_F = 74 + bt_temperature * 0.2;
 592         float bt_C = (bt_F - 32) / 1.8;
 593         PrintAndLogEx(NORMAL, "");
 594         PrintAndLogEx(SUCCESS, "Bio-Thermo detected");
 595         PrintAndLogEx(INFO, "   temperature     " _GREEN_("%.1f")" F / " _GREEN_("%.1f") " C", bt_F, bt_C);
 596     }
 597
 598     // set block 0 for later
 599     //g_DemodConfig = T55x7_MODULATION_DIPHASE | T55x7_BITRATE_RF_32 | 4 << T55x7_MAXBLOCK_SHIFT;
 600
 601     return PM3_SUCCESS;
 602 }
 603
 604 static int CmdFdxBDemod(const char *Cmd) {
 605     CLIParserContext *ctx;
 606     CLIParserInit(&ctx, "lf fdxb demod",
 607                   "Try to find FDX-B preamble, if found decode / descramble data",
 608                   "lf fdxb demod"
 609                  );
 610
 611     void *argtable[] = {
 612         arg_param_begin,
 613         arg_param_end
 614     };
 615     CLIExecWithReturn(ctx, Cmd, argtable, true);
 616     CLIParserFree(ctx);
 617     return demodFDXB(true);
 618 }
 619
 620 static int CmdFdxBReader(const char *Cmd) {
 621
 622     CLIParserContext *ctx;
 623     CLIParserInit(&ctx, "lf fdxb reader",
 624                   "read a FDX-B animal tag\n"
 625                   "Note that the continuous mode is less verbose",
 626                   "lf fdxb reader -@   -> continuous reader mode"
 627                  );
 628
 629     void *argtable[] = {
 630         arg_param_begin,
 631         arg_lit0("@", NULL, "optional - continuous reader mode"),
 632         arg_param_end
 633     };
 634     CLIExecWithReturn(ctx, Cmd, argtable, true);
 635     bool cm = arg_get_lit(ctx, 1);
 636     CLIParserFree(ctx);
 637
 638     sample_config config;
 639     memset(&config, 0, sizeof(sample_config));
 640     int res = lf_getconfig(&config);
 641     if (res != PM3_SUCCESS) {
 642         PrintAndLogEx(ERR, "failed to get current device LF config");
 643         return res;
 644     }
 645
 646     config.verbose = false;
 647
 648     int16_t old_div = config.divisor;
 649     int16_t curr_div = config.divisor;
 650
 651     if (cm) {
 652         PrintAndLogEx(INFO, "Press " _GREEN_("<Enter>") " to exit");
 653     }
 654
 655     int ret = PM3_SUCCESS;
 656     do {
 657
 658         if (curr_div == LF_DIVISOR_125) {
 659             config.divisor = LF_DIVISOR_134;
 660             res = lf_config(&config);
 661             if (res != PM3_SUCCESS) {
 662                 PrintAndLogEx(ERR, "failed to change to 134 KHz LF configuration");
 663                 return res;
 664             }
 665         } else {
 666             config.divisor = LF_DIVISOR_125;
 667             res = lf_config(&config);
 668             if (res != PM3_SUCCESS) {
 669                 PrintAndLogEx(ERR, "failed to change to 125 KHz LF configuration");
 670                 return res;
 671             }
 672         }
 673         curr_div = config.divisor;
 674
 675         lf_read(false, 10000);
 676         ret = demodFDXB(!cm); // be verbose only if not in continuous mode
 677
 678     } while (cm && !kbd_enter_pressed());
 679
 680
 681     if (old_div != curr_div) {
 682         config.divisor = old_div;
 683         res = lf_config(&config);
 684         if (res != PM3_SUCCESS) {
 685             PrintAndLogEx(ERR, "failed to restore LF configuration");
 686             return res;
 687         }
 688     }
 689     return ret;
 690 }
 691
 692 static int CmdFdxBClone(const char *Cmd) {
 693
 694     CLIParserContext *ctx;
 695     CLIParserInit(&ctx, "lf fdxb clone",
 696                   "clone a FDX-B tag to a T55x7, Q5/T5555 or EM4305/4469 tag.",
 697                   "lf fdxb clone --country 999 --national 1337 --animal\n"
 698                   "lf fdxb clone --country 999 --national 1337 --extended 016A\n"
 699                   "lf fdxb clone --q5 --country 999 --national 1337   -> encode for Q5/T5555 tag\n"
 700                   "lf fdxb clone --em --country 999 --national 1337   -> encode for EM4305/4469"
 701                  );
 702
 703     void *argtable[] = {
 704         arg_param_begin,
 705         arg_u64_1("c", "country", "<dec>", "country code"),
 706         arg_u64_1("n", "national", "<dec>", "national code"),
 707         arg_str0(NULL, "extended", "<hex>", "extended data"),
 708         arg_lit0("a", "animal", "optional - set animal bit"),
 709         arg_lit0(NULL, "q5", "optional - specify writing to Q5/T5555 tag"),
 710         arg_lit0(NULL, "em", "optional - specify writing to EM4305/4469 tag"),
 711         arg_param_end
 712     };
 713     CLIExecWithReturn(ctx, Cmd, argtable, false);
 714
 715     uint32_t country_code = arg_get_u32_def(ctx, 1, 0);
 716     uint64_t national_code = arg_get_u64_def(ctx, 2, 0);
 717
 718     int extended_len = 0;
 719     uint8_t edata[3] = {0};
 720     CLIGetHexWithReturn(ctx, 3, edata, &extended_len);
 721
 722     bool is_animal = arg_get_lit(ctx, 4);
 723     bool q5 = arg_get_lit(ctx, 5);
 724     bool em = arg_get_lit(ctx, 6);
 725     CLIParserFree(ctx);
 726
 727     if (q5 && em) {
 728         PrintAndLogEx(FAILED, "Can't specify both Q5 and EM4305 at the same time");
 729         return PM3_EINVARG;
 730     }
 731
 732     uint32_t extended = 0;
 733     bool has_extended = false;
 734     if (extended_len) {
 735         extended = bytes_to_num(edata, extended_len);
 736         has_extended = true;
 737     }
 738
 739     verify_values(&national_code, &country_code, &extended);
 740
 741     PrintAndLogEx(INFO, "Country code........ %"PRIu32, country_code);
 742     PrintAndLogEx(INFO, "National code....... %"PRIu64, national_code);
 743     PrintAndLogEx(INFO, "Set animal bit...... %c", (is_animal) ? 'Y' : 'N');
 744     PrintAndLogEx(INFO, "Set data block bit.. %c", (has_extended) ? 'Y' : 'N');
 745     PrintAndLogEx(INFO, "Extended data....... 0x%"PRIX32, extended);
 746     PrintAndLogEx(INFO, "RFU................. 0");
 747
 748     uint8_t *bs = calloc(128, sizeof(uint8_t));
 749     if (getFDXBBits(national_code, country_code, is_animal, has_extended, extended, bs) != PM3_SUCCESS) {
 750         PrintAndLogEx(ERR, "Error with tag bitstream generation.");
 751         free(bs);
 752         return PM3_ESOFT;
 753     }
 754
 755     uint32_t blocks[5] = {T55x7_MODULATION_DIPHASE | T55x7_BITRATE_RF_32 | 4 << T55x7_MAXBLOCK_SHIFT, 0, 0, 0, 0};
 756     char cardtype[16] = {"T55x7"};
 757
 758     // Q5
 759     if (q5) {
 760         blocks[0] = T5555_FIXED | T5555_MODULATION_BIPHASE | T5555_INVERT_OUTPUT | T5555_SET_BITRATE(32) | 4 << T5555_MAXBLOCK_SHIFT;
 761         snprintf(cardtype, sizeof(cardtype), "Q5/T5555");
 762     }
 763
 764     // EM4305
 765     if (em) {
 766         blocks[0] = EM4305_FDXB_CONFIG_BLOCK;
 767         snprintf(cardtype, sizeof(cardtype), "EM4305/4469");
 768     }
 769
 770     // convert from bit stream to block data
 771     blocks[1] = bytebits_to_byte(bs, 32);
 772     blocks[2] = bytebits_to_byte(bs + 32, 32);
 773     blocks[3] = bytebits_to_byte(bs + 64, 32);
 774     blocks[4] = bytebits_to_byte(bs + 96, 32);
 775
 776     free(bs);
 777
 778     PrintAndLogEx(INFO, "Preparing to clone FDX-B to " _YELLOW_("%s") " with animal ID: " _GREEN_("%04u-%"PRIu64)
 779                   , cardtype
 780                   , country_code
 781                   , national_code
 782                  );
 783     print_blocks(blocks,  ARRAYLEN(blocks));
 784
 785     int res;
 786     if (em) {
 787         res = em4x05_clone_tag(blocks, ARRAYLEN(blocks), 0, false);
 788     } else {
 789         res = clone_t55xx_tag(blocks, ARRAYLEN(blocks));
 790     }
 791     PrintAndLogEx(SUCCESS, "Done");
 792     PrintAndLogEx(HINT, "Hint: try " _YELLOW_("`lf fdxb reader`") " to verify");
 793     return res;
 794 }
 795
 796 static int CmdFdxBSim(const char *Cmd) {
 797
 798     CLIParserContext *ctx;
 799     CLIParserInit(&ctx, "lf fdxb sim",
 800                   "Enables simulation of  FDX-B animal tag.\n"
 801                   "Simulation runs until the button is pressed or another USB command is issued.",
 802                   "lf fdxb sim --country 999 --national 1337 --animal\n"
 803                   "lf fdxb sim --country 999 --national 1337 --extended 016A\n"
 804                  );
 805
 806     void *argtable[] = {
 807         arg_param_begin,
 808         arg_u64_1("c", "country", "<dec>", "country code"),
 809         arg_u64_1("n", "national", "<dec>", "national code"),
 810         arg_str0(NULL, "extended", "<hex>", "extended data"),
 811         arg_lit0("a", "animal", "optional - set animal bit"),
 812         arg_param_end
 813     };
 814     CLIExecWithReturn(ctx, Cmd, argtable, false);
 815
 816     uint32_t country_code = arg_get_u32_def(ctx, 1, 0);
 817     uint64_t national_code = arg_get_u64_def(ctx, 2, 0);
 818     int extended_len = 0;
 819     uint8_t edata[3] = {0};
 820     CLIGetHexWithReturn(ctx, 3, edata, &extended_len);
 821
 822     bool is_animal = arg_get_lit(ctx, 4);
 823     CLIParserFree(ctx);
 824
 825     uint32_t extended = 0;
 826     bool has_extended = false;
 827     if (extended_len) {
 828         extended = bytes_to_num(edata, extended_len);
 829         has_extended = true;
 830     }
 831
 832     verify_values(&national_code, &country_code, &extended);
 833
 834     PrintAndLogEx(INFO, "Country code........ %"PRIu32, country_code);
 835     PrintAndLogEx(INFO, "National code....... %"PRIu64, national_code);
 836     PrintAndLogEx(INFO, "Set animal bit...... %c", (is_animal) ? 'Y' : 'N');
 837     PrintAndLogEx(INFO, "Set data block bit.. %c", (has_extended) ? 'Y' : 'N');
 838     PrintAndLogEx(INFO, "Extended data....... 0x%"PRIX16, extended);
 839     PrintAndLogEx(INFO, "RFU................. 0");
 840
 841     PrintAndLogEx(SUCCESS, "Simulating FDX-B animal ID: " _YELLOW_("%04u-%"PRIu64), country_code, national_code);
 842
 843     uint8_t *bs = calloc(128, sizeof(uint8_t));
 844     if (getFDXBBits(national_code, country_code, is_animal, (extended > 0), extended, bs) != PM3_SUCCESS) {
 845         PrintAndLogEx(ERR, "Error with tag bitstream generation.");
 846         free(bs);
 847         return PM3_ESOFT;
 848     }
 849
 850     // 32, no STT, BIPHASE INVERTED == diphase
 851     lf_asksim_t *payload = calloc(1, sizeof(lf_asksim_t) + 128);
 852     payload->encoding = 2;
 853     payload->invert = 1;
 854     payload->separator = 0;
 855     payload->clock = 32;
 856     memcpy(payload->data, bs, 128);
 857
 858     clearCommandBuffer();
 859     SendCommandNG(CMD_LF_ASK_SIMULATE, (uint8_t *)payload,  sizeof(lf_asksim_t) + 128);
 860
 861     free(bs);
 862     free(payload);
 863
 864     PacketResponseNG resp;
 865     WaitForResponse(CMD_LF_ASK_SIMULATE, &resp);
 866
 867     PrintAndLogEx(INFO, "Done");
 868     if (resp.status != PM3_EOPABORTED)
 869         return resp.status;
 870
 871     return PM3_SUCCESS;
 872 }
 873
 874 static command_t CommandTable[] = {
 875     {"help",    CmdHelp,      AlwaysAvailable, "this help"},
 876     {"demod",   CmdFdxBDemod,  AlwaysAvailable, "demodulate a FDX-B ISO11784/85 tag from the GraphBuffer"},
 877     {"reader",  CmdFdxBReader, IfPm3Lf,         "attempt to read at 134kHz and extract tag data"},
 878     {"clone",   CmdFdxBClone,  IfPm3Lf,         "clone animal ID tag to T55x7 or Q5/T5555"},
 879     {"sim",     CmdFdxBSim,    IfPm3Lf,         "simulate Animal ID tag"},
 880     {NULL, NULL, NULL, NULL}
 881 };
 882
 883 static int CmdHelp(const char *Cmd) {
 884     (void)Cmd; // Cmd is not used so far
 885     CmdsHelp(CommandTable);
 886     return PM3_SUCCESS;
 887 }
 888
 889 int CmdLFFdxB(const char *Cmd) {
 890     clearCommandBuffer();
 891     return CmdsParse(CommandTable, Cmd);
 892 }
 893
 894 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
 895 // BitStream must contain previously askrawdemod and biphasedemoded data
 896 int detectFDXB(uint8_t *dest, size_t *size) {
 897     //make sure buffer has enough data
 898     if (*size < 128 * 2) return -1;
 899     size_t startIdx = 0;
 900     uint8_t preamble[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
 901     if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
 902         return -2; //preamble not found
 903     if (*size < 128) return -3; //wrong demoded size
 904     //return start position
 905     return (int)startIdx;
 906 }