client/src/cmdlfpac.c

   1 //-----------------------------------------------------------------------------
   2 // by marshmellow
   3 // by danshuk
   4 //
   5 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   6 // at your option, any later version. See the LICENSE.txt file for the text of
   7 // the license.
   8 //-----------------------------------------------------------------------------
   9 // Low frequency PAC/Stanley tag commands
  10 // NRZ, RF/32, 128 bits long
  11 //-----------------------------------------------------------------------------
  12 #include "cmdlfpac.h"
  13
  14 #include <ctype.h>      // tolower
  15 #include <string.h>
  16 #include <stdlib.h>
  17 #include "commonutil.h" // ARRAYLEN
  18 #include "common.h"
  19 #include "cmdparser.h"  // command_t
  20 #include "comms.h"
  21 #include "ui.h"
  22 #include "cmddata.h"
  23 #include "cmdlf.h"
  24 #include "lfdemod.h"    // preamble test
  25 #include "protocols.h"  // t55xx defines
  26 #include "cmdlft55xx.h" // clone
  27 #include "parity.h"
  28 #include "cmdlfem4x05.h"   //
  29 #include "cliparser.h"
  30
  31 static int CmdHelp(const char *Cmd);
  32
  33 // PAC_8byte format: preamble (8 mark/idle bits), ascii STX (02), ascii '2' (32), ascii '0' (30), ascii bytes 0..7 (cardid), then xor checksum of cardid bytes
  34 // all bytes following 8 bit preamble are one start bit (0), 7 data bits (lsb first), odd parity bit, and one stop bit (1)
  35 static int pac_buf_to_cardid(uint8_t *src, const size_t src_size, uint8_t *dst, const size_t dst_size) {
  36     const size_t byteLength = 10; // start bit, 7 data bits, parity bit, stop bit
  37     const size_t startIndex = 8 + (3 * byteLength) + 1; // skip 8 bits preamble, STX, '2', '0', and first start bit
  38     const size_t dataLength = 9;
  39
  40     if (startIndex + byteLength * (dataLength - 1) > src_size) {
  41         PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: Source buffer too small");
  42         return PM3_EOVFLOW;
  43     }
  44     if (dataLength > dst_size) {
  45         PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: Destination buffer too small");
  46         return PM3_EOVFLOW;
  47     }
  48
  49     uint8_t checksum = 0;
  50     for (size_t idx = 0; idx < dataLength; idx++) {
  51         uint8_t byte = (uint8_t)bytebits_to_byteLSBF(src + startIndex + (byteLength * idx), 8);
  52         dst[idx] = byte & 0x7F; // discard the parity bit
  53         if (oddparity8(dst[idx]) != (byte & 0x80) >> 7) {
  54             PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: Parity check failed");
  55             return PM3_ESOFT;
  56         }
  57         if (idx < dataLength - 1) checksum ^= byte;
  58     }
  59     if (dst[dataLength - 1] != checksum) {
  60         PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: Bad checksum - expected: %02X, actual: %02X", dst[dataLength - 1], checksum);
  61         return PM3_ESOFT;
  62     }
  63
  64     // overwrite checksum byte with null terminator
  65     dst[dataLength - 1] = 0;
  66
  67     return PM3_SUCCESS;
  68 }
  69
  70 // convert a 16 byte array of raw demod data (FF204990XX...) to 8 bytes of PAC_8byte ID
  71 // performs no parity or checksum validation
  72 static void pac_raw_to_cardid(const uint8_t *src, uint8_t *dst) {
  73     for (int i = 4; i < 12; i++) {
  74         uint8_t shift = 7 - (i + 3) % 4 * 2;
  75         size_t index = i + (i - 1) / 4;
  76
  77         dst[i - 4] = reflect8((((src[index] << 8) | (src[index + 1])) >> shift) & 0xFE);
  78     }
  79 }
  80
  81 // convert 8 bytes of PAC_8byte ID to 16 byte array of raw data (FF204990XX...)
  82 static void pac_cardid_to_raw(const char *src, uint8_t *dst) {
  83     uint8_t idbytes[10];
  84
  85     // prepend PAC_8byte card type "20"
  86     idbytes[0] = '2';
  87     idbytes[1] = '0';
  88     for (size_t i = 0; i < 8; i++)
  89         idbytes[i + 2] = toupper(src[i]);
  90
  91     // initialise array with start and stop bits
  92     for (size_t i = 0; i < 16; i++)
  93         dst[i] = 0x40 >> (i + 3) % 5 * 2;
  94
  95     dst[0] = 0xFF; // mark + stop
  96     dst[1] = 0x20; // start + reflect8(STX)
  97
  98     uint8_t checksum = 0;
  99     for (size_t i = 2; i < 13; i++) {
 100         uint8_t shift = 7 - (i + 3) % 4 * 2;
 101         uint8_t index = i + (i - 1) / 4;
 102
 103         uint16_t pattern;
 104         if (i < 12) {
 105             pattern = reflect8(idbytes[i - 2]);
 106             pattern |= oddparity8(pattern);
 107             if (i > 3) checksum ^= idbytes[i - 2];
 108         } else {
 109             pattern = (reflect8(checksum) & 0xFE) | (oddparity8(checksum));
 110         }
 111         pattern <<= shift;
 112
 113         dst[index] |= pattern >> 8 & 0xFF;
 114         dst[index + 1] |= pattern & 0xFF;
 115     }
 116 }
 117
 118 //see NRZDemod for what args are accepted
 119 int demodPac(bool verbose) {
 120     (void) verbose; // unused so far
 121     //NRZ
 122     if (NRZrawDemod(0, 0, 100, false) != PM3_SUCCESS) {
 123         PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: NRZ Demod failed");
 124         return PM3_ESOFT;
 125     }
 126     bool invert = false;
 127     size_t size = DemodBufferLen;
 128     int ans = detectPac(DemodBuffer, &size, &invert);
 129     if (ans < 0) {
 130         if (ans == -1)
 131             PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: too few bits found");
 132         else if (ans == -2)
 133             PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: preamble not found");
 134         else if (ans == -3)
 135             PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: Size not correct: %zu", size);
 136         else
 137             PrintAndLogEx(DEBUG, "DEBUG: Error - PAC: ans: %d", ans);
 138
 139         return PM3_ESOFT;
 140     }
 141
 142     if (invert) {
 143         for (size_t i = ans; i < ans + 128; i++) {
 144             DemodBuffer[i] ^= 1;
 145         }
 146     }
 147     setDemodBuff(DemodBuffer, 128, ans);
 148     setClockGrid(g_DemodClock, g_DemodStartIdx + (ans * g_DemodClock));
 149
 150     //got a good demod
 151     uint32_t raw1 = bytebits_to_byte(DemodBuffer, 32);
 152     uint32_t raw2 = bytebits_to_byte(DemodBuffer + 32, 32);
 153     uint32_t raw3 = bytebits_to_byte(DemodBuffer + 64, 32);
 154     uint32_t raw4 = bytebits_to_byte(DemodBuffer + 96, 32);
 155
 156     const size_t idLen = 9; // 8 bytes + null terminator
 157     uint8_t cardid[idLen];
 158     int retval = pac_buf_to_cardid(DemodBuffer, DemodBufferLen, cardid, sizeof(cardid));
 159
 160     if (retval == PM3_SUCCESS)
 161         PrintAndLogEx(SUCCESS, "PAC/Stanley - Card: " _GREEN_("%s") ", Raw: %08X%08X%08X%08X", cardid, raw1, raw2, raw3, raw4);
 162
 163     return retval;
 164 }
 165
 166 static int CmdPacDemod(const char *Cmd) {
 167     CLIParserContext *ctx;
 168     CLIParserInit(&ctx, "lf pac demod",
 169                   "Try to find PAC/Stanley preamble, if found decode / descramble data",
 170                   "lf pac demod"
 171                  );
 172
 173     void *argtable[] = {
 174         arg_param_begin,
 175         arg_param_end
 176     };
 177     CLIExecWithReturn(ctx, Cmd, argtable, true);
 178     CLIParserFree(ctx);
 179     return demodPac(true);
 180 }
 181
 182 static int CmdPacReader(const char *Cmd) {
 183
 184     CLIParserContext *ctx;
 185     CLIParserInit(&ctx, "lf pac reader",
 186                   "read a PAC/Stanley tag",
 187                   "lf pac reader -@   -> continuous reader mode"
 188                  );
 189
 190     void *argtable[] = {
 191         arg_param_begin,
 192         arg_lit0("@", NULL, "optional - continuous reader mode"),
 193         arg_param_end
 194     };
 195     CLIExecWithReturn(ctx, Cmd, argtable, true);
 196     bool cm = arg_get_lit(ctx, 1);
 197     CLIParserFree(ctx);
 198
 199     if (cm) {
 200         PrintAndLogEx(INFO, "Press " _GREEN_("<Enter>") " to exit");
 201     }
 202
 203     do {
 204         lf_read(false, 4096 * 2 + 20);
 205         demodPac(!cm);
 206     } while (cm && !kbd_enter_pressed());
 207
 208     return PM3_SUCCESS;
 209 }
 210
 211 static int CmdPacClone(const char *Cmd) {
 212
 213     CLIParserContext *ctx;
 214     CLIParserInit(&ctx, "lf pac clone",
 215                   "clone a PAC/Stanley tag to a T55x7, Q5/T5555 or EM4305/4469 tag.",
 216                   "lf pac clone --cn CD4F5552\n"
 217                   "lf pac clone --cn CD4F5552 --q5      -> encode for Q5/T5555 tag\n"
 218                   "lf pac clone --cn CD4F5552 --em      -> encode for EM4305/4469\n"
 219                   "lf pac clone --raw FF2049906D8511C593155B56D5B2649F"
 220                  );
 221
 222     void *argtable[] = {
 223         arg_param_begin,
 224         arg_str0(NULL, "cn", "<dec>", "8 byte PAC/Stanley card ID"),
 225         arg_str0("r", "raw", "<hex>", "raw hex data. 16 bytes max"),
 226         arg_lit0(NULL, "q5", "optional - specify writing to Q5/T5555 tag"),
 227         arg_lit0(NULL, "em", "optional - specify writing to EM4305/4469 tag"),
 228         arg_param_end
 229     };
 230     CLIExecWithReturn(ctx, Cmd, argtable, false);
 231
 232     uint8_t cnstr[9];
 233     int cnlen = 9;
 234     memset(cnstr, 0x00, sizeof(cnstr));
 235     CLIGetStrWithReturn(ctx, 1, cnstr, &cnlen);
 236
 237     // skip first block,  4*4 = 16 bytes left
 238     int raw_len = 0;
 239     uint8_t raw[16] = {0};
 240     CLIGetHexWithReturn(ctx, 2, raw, &raw_len);
 241
 242     bool q5 = arg_get_lit(ctx, 3);
 243     bool em = arg_get_lit(ctx, 4);
 244     CLIParserFree(ctx);
 245
 246     if (q5 && em) {
 247         PrintAndLogEx(FAILED, "Can't specify both Q5 and EM4305 at the same time");
 248         return PM3_EINVARG;
 249     }
 250     if (cnlen && raw_len) {
 251         PrintAndLogEx(FAILED, "Can't specify both CardID and raw hex at the same time");
 252         return PM3_EINVARG;
 253     }
 254
 255     if (cnlen && cnlen < 8) {
 256         PrintAndLogEx(FAILED, "Card ID must be 8 or 9 hex digits (%d)", cnlen);
 257         return PM3_EINVARG;
 258     }
 259
 260     if (cnlen == 8 || cnlen == 9) {
 261         pac_cardid_to_raw((char *)cnstr, raw);
 262     } else {
 263         pac_raw_to_cardid(raw, cnstr);
 264     }
 265
 266     uint32_t blocks[5];
 267     for (uint8_t i = 1; i < ARRAYLEN(blocks); i++) {
 268         blocks[i] = bytes_to_num(raw + ((i - 1) * 4), sizeof(uint32_t));
 269     }
 270
 271     // Pac - compat mode, NRZ, data rate 32, 3 data blocks
 272     blocks[0] = T55x7_MODULATION_DIRECT | T55x7_BITRATE_RF_32 | 4 << T55x7_MAXBLOCK_SHIFT;
 273     char cardtype[16] = {"T55x7"};
 274     // Q5
 275     if (q5) {
 276         blocks[0] = T5555_FIXED | T5555_MODULATION_DIRECT | T5555_SET_BITRATE(32) | 4 << T5555_MAXBLOCK_SHIFT;
 277         snprintf(cardtype, sizeof(cardtype), "Q5/T5555");
 278     }
 279
 280     // EM4305
 281     if (em) {
 282         blocks[0] = EM4305_PAC_CONFIG_BLOCK;
 283         snprintf(cardtype, sizeof(cardtype), "EM4305/4469");
 284     }
 285
 286     PrintAndLogEx(INFO, "Preparing to clone PAC/Stanley tag to " _YELLOW_("%s") " with ID " _GREEN_("%s")  " raw " _GREEN_("%s")
 287                   , cardtype
 288                   , cnstr
 289                   , sprint_hex_inrow(raw, sizeof(raw))
 290                  );
 291
 292     print_blocks(blocks,  ARRAYLEN(blocks));
 293
 294     int res;
 295     if (em) {
 296         res = em4x05_clone_tag(blocks, ARRAYLEN(blocks), 0, false);
 297     } else {
 298         res = clone_t55xx_tag(blocks, ARRAYLEN(blocks));
 299     }
 300     PrintAndLogEx(SUCCESS, "Done");
 301     PrintAndLogEx(HINT, "Hint: try " _YELLOW_("`lf pac reader`") " to verify");
 302     return res;
 303 }
 304
 305 static int CmdPacSim(const char *Cmd) {
 306
 307     CLIParserContext *ctx;
 308     CLIParserInit(&ctx, "lf pac sim",
 309                   "Enables simulation of PAC/Stanley card with specified card number.\n"
 310                   "Simulation runs until the button is pressed or another USB command is issued.\n"
 311                   "The card ID is 8 byte number. Larger values are truncated.",
 312                   "lf pac sim --cn CD4F5552\n"
 313                   "lf pac sim --raw FF2049906D8511C593155B56D5B2649F"
 314                  );
 315
 316     void *argtable[] = {
 317         arg_param_begin,
 318         arg_str0(NULL, "cn", "<dec>", "8 byte PAC/Stanley card ID"),
 319         arg_str0("r", "raw", "<hex>", "raw hex data. 16 bytes max"),
 320         arg_param_end
 321     };
 322     CLIExecWithReturn(ctx, Cmd, argtable, false);
 323
 324     uint8_t cnstr[10];
 325     int cnlen = 9;
 326     memset(cnstr, 0x00, sizeof(cnstr));
 327     CLIGetStrWithReturn(ctx, 1, cnstr, &cnlen);
 328
 329     // skip first block,  4*4 = 16 bytes left
 330     int raw_len = 0;
 331     uint8_t raw[16] = {0};
 332     CLIGetHexWithReturn(ctx, 2, raw, &raw_len);
 333     CLIParserFree(ctx);
 334
 335     if (cnlen && raw_len) {
 336         PrintAndLogEx(FAILED, "Can't specify both CardID and raw hex at the same time");
 337         return PM3_EINVARG;
 338     }
 339
 340     if (cnlen && cnlen < 8) {
 341         PrintAndLogEx(FAILED, "Card ID must be 8 or 9 hex digits (%d)", cnlen);
 342         return PM3_EINVARG;
 343     }
 344
 345     if (cnlen == 8 || cnlen == 9) {
 346         pac_cardid_to_raw((char *)cnstr, raw);
 347     } else {
 348         pac_raw_to_cardid(raw, cnstr);
 349     }
 350
 351     uint8_t bs[128];
 352     for (size_t i = 0; i < 4; i++) {
 353         uint32_t tmp = bytes_to_num(raw + (i * sizeof(uint32_t)), sizeof(uint32_t));
 354         num_to_bytebits(tmp, sizeof(uint32_t) * 8, bs + (i * sizeof(uint32_t) * 8));
 355     }
 356
 357     PrintAndLogEx(SUCCESS, "Simulating PAC/Stanley - ID " _YELLOW_("%s")" raw " _YELLOW_("%s")
 358                   , cnstr
 359                   , sprint_hex_inrow(raw, sizeof(raw))
 360                  );
 361
 362     // NRZ sim.
 363     lf_nrzsim_t *payload = calloc(1, sizeof(lf_nrzsim_t) + sizeof(bs));
 364     payload->invert = 0;
 365     payload->separator = 0;
 366     payload->clock = 32;
 367     memcpy(payload->data, bs, sizeof(bs));
 368
 369     clearCommandBuffer();
 370     SendCommandNG(CMD_LF_NRZ_SIMULATE, (uint8_t *)payload,  sizeof(lf_nrzsim_t) + sizeof(bs));
 371     free(payload);
 372
 373     PacketResponseNG resp;
 374     WaitForResponse(CMD_LF_NRZ_SIMULATE, &resp);
 375
 376     PrintAndLogEx(INFO, "Done");
 377     if (resp.status != PM3_EOPABORTED)
 378         return resp.status;
 379
 380     return PM3_SUCCESS;
 381 }
 382
 383 static command_t CommandTable[] = {
 384     {"help",  CmdHelp,        AlwaysAvailable, "This help"},
 385     {"demod", CmdPacDemod,    AlwaysAvailable, "demodulate a PAC tag from the GraphBuffer"},
 386     {"reader",  CmdPacReader, IfPm3Lf,         "attempt to read and extract tag data"},
 387     {"clone", CmdPacClone,    IfPm3Lf,         "clone PAC tag to T55x7"},
 388     {"sim",   CmdPacSim,      IfPm3Lf,         "simulate PAC tag"},
 389     {NULL, NULL, NULL, NULL}
 390 };
 391
 392 static int CmdHelp(const char *Cmd) {
 393     (void)Cmd; // Cmd is not used so far
 394     CmdsHelp(CommandTable);
 395     return PM3_SUCCESS;
 396 }
 397
 398 int CmdLFPac(const char *Cmd) {
 399     clearCommandBuffer();
 400     return CmdsParse(CommandTable, Cmd);
 401 }
 402
 403 // find PAC preamble in already demoded data
 404 int detectPac(uint8_t *dest, size_t *size, bool *invert) {
 405     // make sure buffer has data
 406     if (*size < 128)
 407         return -1;
 408
 409     size_t startIdx = 0;
 410     uint8_t preamble[] = {1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0};
 411     if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx)) {
 412
 413         // preamble not found
 414         uint8_t pre_inv[] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1};
 415         if (!preambleSearch(dest, pre_inv, sizeof(pre_inv), size, &startIdx)) {
 416             return -2;
 417         } else {
 418             *invert = true;
 419         }
 420     }
 421
 422     // wrong demoded size
 423     if (*size != 128)
 424         return -3;
 425
 426     // return start position
 427     return (int)startIdx;
 428 }
 429
 430