client/src/cmdlfpyramid.c

   1 //-----------------------------------------------------------------------------
   2 //
   3 // by marshmellow
   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 Farpoint / Pyramid tag commands
  10 // FSK2a, rf/50, 128 bits (complete)
  11 //-----------------------------------------------------------------------------
  12 #include "cmdlfpyramid.h"
  13 #include "common.h"
  14 #include <string.h>
  15 #include <stdlib.h>
  16 #include <stdio.h>
  17 #include <ctype.h>
  18 #include "commonutil.h"   // ARRAYLEN
  19 #include "cmdparser.h"    // command_t
  20 #include "comms.h"
  21 #include "ui.h"
  22 #include "graph.h"
  23 #include "cmddata.h"
  24 #include "cmdlf.h"
  25 #include "protocols.h"  // for T55xx config register definitions
  26 #include "lfdemod.h"    // parityTest
  27 #include "crc.h"
  28 #include "cmdlft55xx.h" // verifywrite
  29 #include "cliparser.h"
  30 #include "cmdlfem4x05.h"  // EM Defines
  31
  32 static int CmdHelp(const char *Cmd);
  33
  34 //Pyramid Prox demod - FSK RF/50 with preamble of 0000000000000001  (always a 128 bit data stream)
  35 //print full Farpointe Data/Pyramid Prox ID and some bit format details if found
  36 int demodPyramid(bool verbose) {
  37     (void) verbose; // unused so far
  38     //raw fsk demod no manchester decoding no start bit finding just get binary from wave
  39     uint8_t bits[MAX_GRAPH_TRACE_LEN] = {0};
  40     size_t size = getFromGraphBuf(bits);
  41     if (size == 0) {
  42         PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid not enough samples");
  43         return PM3_ESOFT;
  44     }
  45     //get binary from fsk wave
  46     int waveIdx = 0;
  47     int idx = detectPyramid(bits, &size, &waveIdx);
  48     if (idx < 0) {
  49         if (idx == -1)
  50             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: not enough samples");
  51         else if (idx == -2)
  52             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: only noise found");
  53         else if (idx == -3)
  54             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: problem during FSK demod");
  55         else if (idx == -4)
  56             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: preamble not found");
  57         else if (idx == -5)
  58             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: size not correct: %zu", size);
  59         else
  60             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: error demoding fsk idx: %d", idx);
  61         return PM3_ESOFT;
  62     }
  63     setDemodBuff(bits, size, idx);
  64     setClockGrid(50, waveIdx + (idx * 50));
  65
  66     // Index map
  67     // 0           10          20          30            40          50          60
  68     // |           |           |           |             |           |           |
  69     // 0123456 7 8901234 5 6789012 3 4567890 1 2345678 9 0123456 7 8901234 5 6789012 3
  70     // -----------------------------------------------------------------------------
  71     // 0000000 0 0000000 1 0000000 1 0000000 1 0000000 1 0000000 1 0000000 1 0000000 1
  72     // premable  xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o
  73
  74     // 64    70            80          90          100         110           120
  75     // |     |             |           |           |           |             |
  76     // 4567890 1 2345678 9 0123456 7 8901234 5 6789012 3 4567890 1 2345678 9 0123456 7
  77     // -----------------------------------------------------------------------------
  78     // 0000000 1 0000000 1 0000000 1 0110111 0 0011000 1 0000001 0 0001100 1 1001010 0
  79     // xxxxxxx o xxxxxxx o xxxxxxx o xswffff o ffffccc o ccccccc o ccccccw o ppppppp o
  80     //                                  |---115---||---------71---------|
  81     // s = format start bit, o = odd parity of last 7 bits
  82     // f = facility code, c = card number
  83     // w = wiegand parity, x = extra space for other formats
  84     // p = CRC8maxim checksum
  85     // (26 bit format shown)
  86
  87     //get bytes for checksum calc
  88     uint8_t checksum = bytebits_to_byte(bits + idx + 120, 8);
  89     uint8_t csBuff[14] = {0x00};
  90     for (uint8_t i = 0; i < 13; i++) {
  91         csBuff[i] = bytebits_to_byte(bits + idx + 16 + (i * 8), 8);
  92     }
  93     //check checksum calc
  94     //checksum calc thanks to ICEMAN!!
  95     uint32_t checkCS =  CRC8Maxim(csBuff, 13);
  96
  97     //get raw ID before removing parities
  98     uint32_t rawLo = bytebits_to_byte(bits + idx + 96, 32);
  99     uint32_t rawHi = bytebits_to_byte(bits + idx + 64, 32);
 100     uint32_t rawHi2 = bytebits_to_byte(bits + idx + 32, 32);
 101     uint32_t rawHi3 = bytebits_to_byte(bits + idx, 32);
 102
 103     size = removeParity(bits, idx + 8, 8, 1, 120);
 104     if (size != 105) {
 105         if (size == 0)
 106             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: parity check failed - IDX: %d, hi3: %08X", idx, rawHi3);
 107         else
 108             PrintAndLogEx(DEBUG, "DEBUG: Error - Pyramid: at parity check - tag size does not match Pyramid format, SIZE: %zu, IDX: %d, hi3: %08X", size, idx, rawHi3);
 109         return PM3_ESOFT;
 110     }
 111
 112     // ok valid card found!
 113
 114     // Index map
 115     // 0         10        20        30        40        50        60        70
 116     // |         |         |         |         |         |         |         |
 117     // 01234567890123456789012345678901234567890123456789012345678901234567890
 118     // -----------------------------------------------------------------------
 119     // 00000000000000000000000000000000000000000000000000000000000000000000000
 120     // xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
 121
 122     // 71         80         90          100
 123     // |          |          |           |
 124     // 1 2 34567890 1234567890123456 7 8901234
 125     // ---------------------------------------
 126     // 1 1 01110011 0000000001000110 0 1001010
 127     // s w ffffffff cccccccccccccccc w ppppppp
 128     //     |--115-| |------71------|
 129     // s = format start bit, o = odd parity of last 7 bits
 130     // f = facility code, c = card number
 131     // w = wiegand parity, x = extra space for other formats
 132     // p = CRC8-Maxim checksum
 133     // (26 bit format shown)
 134
 135     //find start bit to get fmtLen
 136     int j;
 137     for (j = 0; j < size; ++j) {
 138         if (bits[j]) break;
 139     }
 140
 141     uint8_t fmtLen = size - j - 8;
 142
 143     if (fmtLen == 26) {
 144         uint32_t fc = bytebits_to_byte(bits + 73, 8);
 145         uint32_t cardnum = bytebits_to_byte(bits + 81, 16);
 146         uint32_t code1 = bytebits_to_byte(bits + 72, fmtLen);
 147         PrintAndLogEx(SUCCESS, "Pyramid - len: " _GREEN_("%d") ", FC: " _GREEN_("%d") " Card: " _GREEN_("%d") " - Wiegand: " _GREEN_("%x")", Raw: %08x%08x%08x%08x", fmtLen, fc, cardnum, code1, rawHi3, rawHi2, rawHi, rawLo);
 148     } else if (fmtLen == 45) {
 149         fmtLen = 42; //end = 10 bits not 7 like 26 bit fmt
 150         uint32_t fc = bytebits_to_byte(bits + 53, 10);
 151         uint32_t cardnum = bytebits_to_byte(bits + 63, 32);
 152         PrintAndLogEx(SUCCESS, "Pyramid - len: " _GREEN_("%d") ", FC: " _GREEN_("%d") " Card: " _GREEN_("%d") ", Raw: %08x%08x%08x%08x", fmtLen, fc, cardnum, rawHi3, rawHi2, rawHi, rawLo);
 153         /*
 154             } else if (fmtLen > 32) {
 155                 uint32_t cardnum = bytebits_to_byte(bits + 81, 16);
 156                 //uint32_t code1 = bytebits_to_byte(bits+(size-fmtLen),fmtLen-32);
 157                 //code2 = bytebits_to_byte(bits+(size-32),32);
 158                 PrintAndLogEx(SUCCESS, "Pyramid ID Found - BitLength: %d -unknown BitLength- (%d), Raw: %08x%08x%08x%08x", fmtLen, cardnum, rawHi3, rawHi2, rawHi, rawLo);
 159                 */
 160     } else {
 161         uint32_t cardnum = bytebits_to_byte(bits + 81, 16);
 162         //uint32_t code1 = bytebits_to_byte(bits+(size-fmtLen),fmtLen);
 163         PrintAndLogEx(SUCCESS, "Pyramid - len: " _GREEN_("%d") " -unknown- Card: " _GREEN_("%d") ", Raw: %08x%08x%08x%08x", fmtLen, cardnum, rawHi3, rawHi2, rawHi, rawLo);
 164     }
 165
 166     PrintAndLogEx(DEBUG, "DEBUG: Pyramid: checksum : 0x%02X - 0x%02X - %s"
 167                   , checksum
 168                   , checkCS
 169                   , (checksum == checkCS) ? _GREEN_("ok") : _RED_("fail")
 170                  );
 171
 172     PrintAndLogEx(DEBUG, "DEBUG: Pyramid: idx: %d, Len: %d, Printing Demod Buffer:", idx, 128);
 173     if (g_debugMode) {
 174         printDemodBuff(0, false, false, false);
 175     }
 176
 177     return PM3_SUCCESS;
 178 }
 179
 180 static int CmdPyramidDemod(const char *Cmd) {
 181     CLIParserContext *ctx;
 182     CLIParserInit(&ctx, "lf pyramid demod",
 183                   "Try to find Farpoint/Pyramid preamble, if found decode / descramble data",
 184                   "lf pyramid demod"
 185                  );
 186
 187     void *argtable[] = {
 188         arg_param_begin,
 189         arg_param_end
 190     };
 191     CLIExecWithReturn(ctx, Cmd, argtable, true);
 192     CLIParserFree(ctx);
 193     return demodPyramid(true);
 194 }
 195
 196 static int CmdPyramidReader(const char *Cmd) {
 197     CLIParserContext *ctx;
 198     CLIParserInit(&ctx, "lf pyramid reader",
 199                   "read a Farpointe/Pyramid tag",
 200                   "lf pyramid reader -@   -> continuous reader mode"
 201                  );
 202
 203     void *argtable[] = {
 204         arg_param_begin,
 205         arg_lit0("@", NULL, "optional - continuous reader mode"),
 206         arg_param_end
 207     };
 208     CLIExecWithReturn(ctx, Cmd, argtable, true);
 209     bool cm = arg_get_lit(ctx, 1);
 210     CLIParserFree(ctx);
 211
 212     if (cm) {
 213         PrintAndLogEx(INFO, "Press " _GREEN_("<Enter>") " to exit");
 214     }
 215
 216     do {
 217         lf_read(false, 15000);
 218         demodPyramid(true);
 219     } while (cm && !kbd_enter_pressed());
 220
 221     return PM3_SUCCESS;
 222 }
 223
 224 static int CmdPyramidClone(const char *Cmd) {
 225
 226     CLIParserContext *ctx;
 227     CLIParserInit(&ctx, "lf pyramid clone",
 228                   "clone a Farpointe/Pyramid tag to a T55x7, Q5/T5555 or EM4305/4469 tag.\n"
 229                   "The facility-code is 8-bit and the card number is 16-bit.  Larger values are truncated.\n"
 230                   "Currently only works on 26bit",
 231                   "lf pyramid clone --fc 123 --cn 11223\n"
 232                   "lf pyramid clone --fc 123 --cn 11223 --q5  -> encode for Q5/T5555 tag\n"
 233                   "lf pyramid clone --fc 123 --cn 11223 --em  -> encode for EM4305/4469"
 234                  );
 235
 236     void *argtable[] = {
 237         arg_param_begin,
 238         arg_u64_1(NULL, "fc", "<dec>", "8-bit value facility code"),
 239         arg_u64_1(NULL, "cn", "<dec>", "16-bit value card number"),
 240         arg_lit0(NULL, "q5", "optional - specify writing to Q5/T5555 tag"),
 241         arg_lit0(NULL, "em", "optional - specify writing to EM4305/4469 tag"),
 242         arg_param_end
 243     };
 244     CLIExecWithReturn(ctx, Cmd, argtable, false);
 245
 246     uint32_t fc = arg_get_u32_def(ctx, 1, 0);
 247     uint32_t cn = arg_get_u32_def(ctx, 2, 0);
 248     bool q5 = arg_get_lit(ctx, 3);
 249     bool em = arg_get_lit(ctx, 4);
 250     CLIParserFree(ctx);
 251
 252     if (q5 && em) {
 253         PrintAndLogEx(FAILED, "Can't specify both Q5 and EM4305 at the same time");
 254         return PM3_EINVARG;
 255     }
 256
 257     uint32_t blocks[5];
 258     uint8_t *bs = calloc(128, sizeof(uint8_t));
 259     if (bs == NULL) {
 260         return PM3_EMALLOC;
 261     }
 262
 263     uint32_t facilitycode = (fc & 0x000000FF);
 264     uint32_t cardnumber = (cn & 0x0000FFFF);
 265
 266     if (getPyramidBits(facilitycode, cardnumber, bs) != PM3_SUCCESS) {
 267         PrintAndLogEx(ERR, "Error with tag bitstream generation.");
 268         return PM3_ESOFT;
 269     }
 270
 271     //Pyramid - compat mode, FSK2a, data rate 50, 4 data blocks
 272     blocks[0] = T55x7_MODULATION_FSK2a | T55x7_BITRATE_RF_50 | 4 << T55x7_MAXBLOCK_SHIFT;
 273     char cardtype[16] = {"T55x7"};
 274
 275     // Q5
 276     if (q5) {
 277         blocks[0] = T5555_FIXED | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | T5555_SET_BITRATE(50) | 4 << T5555_MAXBLOCK_SHIFT;
 278         snprintf(cardtype, sizeof(cardtype), "Q5/T5555");
 279     }
 280     // EM4305
 281     if (em) {
 282         blocks[0] = EM4305_PYRAMID_CONFIG_BLOCK;
 283         snprintf(cardtype, sizeof(cardtype), "EM4305/4469");
 284     }
 285
 286     blocks[1] = bytebits_to_byte(bs, 32);
 287     blocks[2] = bytebits_to_byte(bs + 32, 32);
 288     blocks[3] = bytebits_to_byte(bs + 64, 32);
 289     blocks[4] = bytebits_to_byte(bs + 96, 32);
 290
 291     free(bs);
 292
 293     PrintAndLogEx(INFO, "Preparing to clone Farpointe/Pyramid to " _YELLOW_("%s") "  with Facility Code: %u, Card Number: %u", cardtype, facilitycode, cardnumber);
 294     print_blocks(blocks,  ARRAYLEN(blocks));
 295
 296     int res;
 297     if (em) {
 298         res = em4x05_clone_tag(blocks, ARRAYLEN(blocks), 0, false);
 299     } else {
 300         res = clone_t55xx_tag(blocks, ARRAYLEN(blocks));
 301     }
 302     PrintAndLogEx(SUCCESS, "Done");
 303     PrintAndLogEx(HINT, "Hint: try " _YELLOW_("`lf pyramid reader`") " to verify");
 304     return res;
 305 }
 306
 307 static int CmdPyramidSim(const char *Cmd) {
 308     CLIParserContext *ctx;
 309     CLIParserInit(&ctx, "lf pyramid sim",
 310                   "Enables simulation of Farpointe/Pyramid card with specified card number.\n"
 311                   "Simulation runs until the button is pressed or another USB command is issued.\n"
 312                   "The facility-code is 8-bit and the card number is 16-bit.  Larger values are truncated.\n"
 313                   "Currently work only on 26bit",
 314                   "lf pyramid sim --fc 123 --cn 1337"
 315                  );
 316
 317     void *argtable[] = {
 318         arg_param_begin,
 319         arg_u64_1(NULL, "fc", "<dec>", "8-bit value facility code"),
 320         arg_u64_1(NULL, "cn", "<dec>", "16-bit value card number"),
 321         arg_param_end
 322     };
 323     CLIExecWithReturn(ctx, Cmd, argtable, false);
 324     uint32_t fc = arg_get_u32_def(ctx, 1, 0);
 325     uint32_t cn = arg_get_u32_def(ctx, 2, 0);
 326     CLIParserFree(ctx);
 327
 328     uint32_t facilitycode = (fc & 0x000000FF);
 329     uint32_t cardnumber = (cn & 0x0000FFFF);
 330
 331
 332     uint8_t bs[128];
 333     memset(bs, 0x00, sizeof(bs));
 334     if (getPyramidBits(facilitycode, cardnumber, bs) != PM3_SUCCESS) {
 335         PrintAndLogEx(ERR, "Error with tag bitstream generation.");
 336         return PM3_ESOFT;
 337     }
 338
 339     PrintAndLogEx(SUCCESS, "Simulating Farpointe/Pyramid - Facility Code: " _YELLOW_("%u") ", CardNumber: " _YELLOW_("%u"), facilitycode, cardnumber);
 340
 341     // Pyramid uses:  fcHigh: 10, fcLow: 8, clk: 50, invert: 0
 342     lf_fsksim_t *payload = calloc(1, sizeof(lf_fsksim_t) + sizeof(bs));
 343     payload->fchigh = 10;
 344     payload->fclow =  8;
 345     payload->separator = 0;
 346     payload->clock = 50;
 347     memcpy(payload->data, bs, sizeof(bs));
 348
 349     clearCommandBuffer();
 350     SendCommandNG(CMD_LF_FSK_SIMULATE, (uint8_t *)payload,  sizeof(lf_fsksim_t) + sizeof(bs));
 351     free(payload);
 352
 353     PacketResponseNG resp;
 354     WaitForResponse(CMD_LF_FSK_SIMULATE, &resp);
 355
 356     PrintAndLogEx(INFO, "Done");
 357     if (resp.status != PM3_EOPABORTED)
 358         return resp.status;
 359     return PM3_SUCCESS;
 360 }
 361
 362 static command_t CommandTable[] = {
 363     {"help",    CmdHelp,          AlwaysAvailable, "this help"},
 364     {"demod",   CmdPyramidDemod,  AlwaysAvailable, "demodulate a Pyramid FSK tag from the GraphBuffer"},
 365     {"reader",  CmdPyramidReader, IfPm3Lf,         "attempt to read and extract tag data"},
 366     {"clone",   CmdPyramidClone,  IfPm3Lf,         "clone pyramid tag to T55x7 or Q5/T5555"},
 367     {"sim",     CmdPyramidSim,    IfPm3Lf,         "simulate pyramid tag"},
 368     {NULL, NULL, NULL, NULL}
 369 };
 370
 371 int CmdLFPyramid(const char *Cmd) {
 372     clearCommandBuffer();
 373     return CmdsParse(CommandTable, Cmd);
 374 }
 375
 376 int CmdHelp(const char *Cmd) {
 377     (void)Cmd; // Cmd is not used so far
 378     CmdsHelp(CommandTable);
 379     return PM3_SUCCESS;
 380 }
 381
 382 // Works for 26bits.
 383 int getPyramidBits(uint32_t fc, uint32_t cn, uint8_t *pyramidBits) {
 384
 385     uint8_t pre[128];
 386     memset(pre, 0x00, sizeof(pre));
 387
 388     // format start bit
 389     pre[79] = 1;
 390
 391     // Get 26 wiegand from FacilityCode, CardNumber
 392     uint8_t wiegand[24];
 393     memset(wiegand, 0x00, sizeof(wiegand));
 394     num_to_bytebits(fc, 8, wiegand);
 395     num_to_bytebits(cn, 16, wiegand + 8);
 396
 397     // add wiegand parity bits (dest, source, len)
 398     wiegand_add_parity(pre + 80, wiegand, 24);
 399
 400     // add paritybits (bitsource, dest, sourcelen, paritylen, parityType (odd, even,)
 401     addParity(pre + 8, pyramidBits + 8, 102, 8, 1);
 402
 403     // add checksum
 404     uint8_t csBuff[13];
 405     for (uint8_t i = 0; i < 13; i++)
 406         csBuff[i] = bytebits_to_byte(pyramidBits + 16 + (i * 8), 8);
 407
 408     uint32_t crc = CRC8Maxim(csBuff, 13);
 409     num_to_bytebits(crc, 8, pyramidBits + 120);
 410     return PM3_SUCCESS;
 411 }
 412
 413 // FSK Demod then try to locate a Farpointe Data (pyramid) ID
 414 int detectPyramid(uint8_t *dest, size_t *size, int *waveStartIdx) {
 415     //make sure buffer has data
 416     if (*size < 128 * 50) return -1;
 417
 418     //test samples are not just noise
 419     if (getSignalProperties()->isnoise) return -2;
 420
 421     // FSK demodulator RF/50 FSK 10,8
 422     *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx);  // pyramid fsk2
 423
 424     //did we get a good demod?
 425     if (*size < 128) return -3;
 426
 427     size_t startIdx = 0;
 428     uint8_t preamble[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1};
 429     if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
 430         return -4; //preamble not found
 431
 432     // wrong size?  (between to preambles)
 433     if (*size != 128) return -5;
 434
 435     return (int)startIdx;
 436 }
 437