client/src/cmdlfkeri.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 KERI tag commands
   8 // PSK1, RF/128, RF/2, 64 bits long
   9 //-----------------------------------------------------------------------------
  10 #include "cmdlfkeri.h"
  11 #include <string.h>
  12 #include <inttypes.h>
  13 #include <ctype.h>
  14 #include <stdlib.h>
  15 #include "commonutil.h"   // ARRAYLEN
  16 #include "cmdparser.h"    // command_t
  17 #include "cliparser.h"
  18 #include "comms.h"
  19 #include "ui.h"
  20 #include "cmddata.h"
  21 #include "cmdlf.h"
  22 #include "protocols.h"    // for T55xx config register definitions
  23 #include "lfdemod.h"      // preamble test
  24 #include "cmdlft55xx.h"   // verifywrite
  25 #include "cmdlfem4x05.h"  //
  26
  27 static int CmdHelp(const char *Cmd);
  28 typedef enum  {Scramble = 0, Descramble = 1} KeriMSScramble_t;
  29
  30 static int CmdKeriMSScramble(KeriMSScramble_t Action, uint32_t *FC, uint32_t *ID, uint32_t *CardID) {
  31     // 255 = Not used/Unknown other values are the bit offset in the ID/FC values
  32     uint8_t CardToID [] = { 255, 255, 255, 255, 13, 12, 20,  5, 16,  6, 21, 17,  8, 255,  0,  7,
  33                             10, 15, 255, 11,  4,  1, 255, 18, 255, 19,  2, 14,  3,  9, 255, 255
  34                           };
  35
  36     uint8_t CardToFC [] = { 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,  0, 255, 255,
  37                             255, 255,  2, 255, 255, 255,  3, 255,  4, 255, 255, 255, 255, 255,  1, 255
  38                           };
  39
  40     uint8_t card_idx; // 0 - 31
  41
  42     if (Action == Descramble) {
  43         *FC = 0;
  44         *ID = 0;
  45         for (card_idx = 0; card_idx < 32; card_idx++) {
  46             // Get Bit State
  47             bool BitState = (*CardID >> card_idx) & 1;
  48             // Card ID
  49             if (CardToID[card_idx] < 32) {
  50                 *ID = *ID | (BitState << CardToID[card_idx]);
  51             }
  52             // Card FC
  53             if (CardToFC[card_idx] < 32) {
  54                 *FC = *FC | (BitState << CardToFC[card_idx]);
  55             }
  56         }
  57     }
  58
  59     if (Action == Scramble) {
  60         *CardID = 0; // set to 0
  61
  62         for (card_idx = 0; card_idx < 32; card_idx++) {
  63             // Card ID
  64             if (CardToID[card_idx] < 32) {
  65                 if ((*ID & (1U << CardToID[card_idx])) > 0)
  66                     *CardID |= (1U << card_idx);
  67             }
  68             // Card FC
  69             if (CardToFC[card_idx] < 32) {
  70                 if ((*FC & (1U << CardToFC[card_idx])) > 0)
  71                     *CardID |= (1U << card_idx);
  72             }
  73         }
  74
  75         // Fixed bits and parity/check bits
  76         /*
  77             Add Parity and Fixed bits
  78             Bit  3 - Note Used/Fixed 1 - TBC
  79             Bit 31 - 1 Fixed Not in check/parity
  80             Bit  0,1 - 2 Bit Parity
  81         */
  82         *CardID |= (1 <<  3);
  83
  84         // Check/Parity Bits
  85         int parity = 1;
  86         for (card_idx = 4; card_idx <= 31; card_idx += 2) {
  87             parity ^= ((*CardID >> card_idx) & 11);
  88         }
  89         *CardID = *CardID | parity;
  90
  91         // Bit 31 was fixed but not in check/parity bits
  92         *CardID |= 1UL << 31;
  93
  94         PrintAndLogEx(SUCCESS, "Scrambled MS - FC: " _GREEN_("%d") " Card: " _GREEN_("%d") ", Raw: E0000000%08X", *FC, *ID, *CardID);
  95     }
  96     return PM3_SUCCESS;
  97 }
  98
  99 int demodKeri(bool verbose) {
 100     (void) verbose; // unused so far
 101
 102     if (PSKDemod(0, 0, 100, false) != PM3_SUCCESS) {
 103         PrintAndLogEx(DEBUG, "DEBUG: Error - KERI: PSK1 Demod failed");
 104         return PM3_ESOFT;
 105     }
 106
 107     bool invert = false;
 108     size_t size = DemodBufferLen;
 109     int idx = detectKeri(DemodBuffer, &size, &invert);
 110     if (idx < 0) {
 111         if (idx == -1)
 112             PrintAndLogEx(DEBUG, "DEBUG: Error - KERI: too few bits found");
 113         else if (idx == -2)
 114             PrintAndLogEx(DEBUG, "DEBUG: Error - KERI: preamble not found");
 115         else if (idx == -3)
 116             PrintAndLogEx(DEBUG, "DEBUG: Error - KERI: Size not correct: 64 != %zu", size);
 117         else
 118             PrintAndLogEx(DEBUG, "DEBUG: Error - KERI: ans: %d", idx);
 119
 120         return PM3_ESOFT;
 121     }
 122     setDemodBuff(DemodBuffer, size, idx);
 123     setClockGrid(g_DemodClock, g_DemodStartIdx + (idx * g_DemodClock));
 124
 125     /*
 126         000000000000000000000000000001XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX111
 127         ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^1###############################^^^
 128         Preamble block 29 bits of ZEROS
 129         32 bit Internal ID  (First bit always 1)
 130         3 bit of 1s in the end
 131
 132         How this is decoded to Facility ID, Card number is unknown
 133         Facility ID =  0-31  (indicates 5 bits)
 134         Card number = up to 10 digits
 135
 136         Might be a hash of FC & CN to generate Internal ID
 137     */
 138
 139
 140     /*
 141         Descramble Data.
 142     */
 143     uint32_t fc = 0;
 144     uint32_t cardid = 0;
 145     //got a good demod
 146     uint32_t raw1 = bytebits_to_byte(DemodBuffer, 32);
 147     uint32_t raw2 = bytebits_to_byte(DemodBuffer + 32, 32);
 148
 149     if (invert) {
 150         PrintAndLogEx(INFO, "Had to Invert - probably KERI");
 151         for (size_t i = 0; i < size; i++)
 152             DemodBuffer[i] ^= 1;
 153
 154         raw1 = bytebits_to_byte(DemodBuffer, 32);
 155         raw2 = bytebits_to_byte(DemodBuffer + 32, 32);
 156
 157         CmdPrintDemodBuff("x");
 158     }
 159
 160     //get internal id
 161     // uint32_t ID = bytebits_to_byte(DemodBuffer + 29, 32);
 162     // Due to the 3 sync bits being at the start of the capture
 163     // We can take the last 32bits as the internal ID.
 164     uint32_t ID = raw2;
 165     ID &= 0x7FFFFFFF;
 166
 167     PrintAndLogEx(SUCCESS, "KERI - Internal ID: " _GREEN_("%u") ", Raw: %08X%08X", ID, raw1, raw2);
 168
 169     // Just need to the low 32 bits without the 111 trailer
 170     CmdKeriMSScramble(Descramble, &fc, &cardid, &raw2);
 171
 172     PrintAndLogEx(SUCCESS, "Descrambled MS - FC: " _GREEN_("%d") " Card: " _GREEN_("%d"), fc, cardid);
 173     return PM3_SUCCESS;
 174 }
 175
 176 static int CmdKeriDemod(const char *Cmd) {
 177     CLIParserContext *ctx;
 178     CLIParserInit(&ctx, "lf keri demod",
 179                   "Try to find KERI preamble, if found decode / descramble data",
 180                   "lf keri demod"
 181                  );
 182
 183     void *argtable[] = {
 184         arg_param_begin,
 185         arg_param_end
 186     };
 187     CLIExecWithReturn(ctx, Cmd, argtable, true);
 188     CLIParserFree(ctx);
 189     return demodKeri(true);
 190 }
 191
 192 static int CmdKeriReader(const char *Cmd) {
 193     CLIParserContext *ctx;
 194     CLIParserInit(&ctx, "lf keri reader",
 195                   "read a keri tag",
 196                   "lf keri reader -@   -> continuous reader mode"
 197                  );
 198
 199     void *argtable[] = {
 200         arg_param_begin,
 201         arg_lit0("@", NULL, "optional - continuous reader mode"),
 202         arg_param_end
 203     };
 204     CLIExecWithReturn(ctx, Cmd, argtable, true);
 205     bool cm = arg_get_lit(ctx, 1);
 206     CLIParserFree(ctx);
 207
 208     if (cm) {
 209         PrintAndLogEx(INFO, "Press " _GREEN_("<Enter>") " to exit");
 210     }
 211
 212     do {
 213         lf_read(false, 10000);
 214         demodKeri(!cm);
 215     } while (cm && !kbd_enter_pressed());
 216
 217     return PM3_SUCCESS;
 218 }
 219
 220 static int CmdKeriClone(const char *Cmd) {
 221     CLIParserContext *ctx;
 222     CLIParserInit(&ctx, "lf keri clone",
 223                   "clone a KERI tag to a T55x7, Q5/T5555 or EM4305/4469 tag",
 224                   "lf keri clone -t i --cn 12345         -> Internal ID\n"
 225                   "lf keri clone -t m --fc 6 --cn 12345  -> MS ID\n");
 226
 227     void *argtable[] = {
 228         arg_param_begin,
 229         arg_str0("t",  "type", "<m|i>", "Type m - MS, i - Internal ID"),
 230         arg_int0(NULL, "fc",   "<dec>", "Facility Code"),
 231         arg_int1(NULL, "cn",   "<dec>", "KERI card ID"),
 232         arg_lit0(NULL, "q5", "specify writing to Q5/T5555 tag"),
 233         arg_lit0(NULL, "em", "specify writing to EM4305/4469 tag"),
 234         arg_param_end
 235     };
 236     CLIExecWithReturn(ctx, Cmd, argtable, false);
 237
 238     uint8_t keritype[2] = {'i'}; // default to internalid
 239     int typeLen = sizeof(keritype);
 240     CLIGetStrWithReturn(ctx, 1, keritype, &typeLen);
 241
 242     uint32_t fc = arg_get_int_def(ctx, 2, 0);
 243     uint32_t cid = arg_get_int_def(ctx, 3, 0);
 244     bool q5 = arg_get_lit(ctx, 4);
 245     bool em = arg_get_lit(ctx, 5);
 246     CLIParserFree(ctx);
 247
 248     if (q5 && em) {
 249         PrintAndLogEx(FAILED, "Can't specify both Q5 and EM4305 at the same time");
 250         return PM3_EINVARG;
 251     }
 252
 253     // Setup card data/build internal id
 254     uint32_t internalid = 0;
 255     switch (keritype[0]) {
 256         case 'i' : // Internal ID
 257             // MSB is ONE
 258             internalid = cid | 0x80000000;
 259             break;
 260         case 'm' : // MS
 261             CmdKeriMSScramble(Scramble, &fc, &cid, &internalid);
 262             break;
 263         default  :
 264             PrintAndLogEx(ERR, "Invalid type");
 265             return PM3_EINVARG;
 266     }
 267
 268     uint32_t blocks[3];
 269     blocks[0] = T55x7_TESTMODE_DISABLED | T55x7_X_MODE | T55x7_MODULATION_PSK1 | T55x7_PSKCF_RF_2 | 2 << T55x7_MAXBLOCK_SHIFT;
 270     // dynamic bitrate used
 271     blocks[0] |= 0xF << 18;
 272
 273     char cardtype[16] = {"T55x7"};
 274
 275     if (q5) {
 276         blocks[0] = T5555_FIXED | T5555_MODULATION_PSK1 | T5555_SET_BITRATE(32) | T5555_PSK_RF_2 | 2 << T5555_MAXBLOCK_SHIFT;
 277         snprintf(cardtype, sizeof(cardtype), "Q5/T5555");
 278     }
 279
 280     if (em) {
 281         blocks[0] = EM4305_KERI_CONFIG_BLOCK;
 282         snprintf(cardtype, sizeof(cardtype), "EM4305/4469");
 283     }
 284
 285
 286     // Prepare and write to card
 287     // 3 LSB is ONE
 288     uint64_t data = ((uint64_t)internalid << 3) + 7;
 289     PrintAndLogEx(INFO, "Preparing to clone KERI to " _YELLOW_("%s") " with Internal Id " _YELLOW_("%" PRIx32), cardtype, internalid);
 290
 291     blocks[1] = data >> 32;
 292     blocks[2] = data & 0xFFFFFFFF;
 293
 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
 303     PrintAndLogEx(SUCCESS, "Done");
 304     PrintAndLogEx(HINT, "Hint: try " _YELLOW_("`lf keri read`") " to verify");
 305     return res;
 306 }
 307
 308 static int CmdKeriSim(const char *Cmd) {
 309
 310     CLIParserContext *ctx;
 311     CLIParserInit(&ctx, "lf keri sim",
 312                   "Enables simulation of KERI card with internal ID.\n"
 313                   "You supply a KERI card id and it will converted to a KERI internal ID.",
 314                   "lf keri sim --cn 112233"
 315                  );
 316
 317     void *argtable[] = {
 318         arg_param_begin,
 319         arg_u64_1(NULL, "id", "<dec>", "KERI card ID"),
 320         arg_param_end
 321     };
 322     CLIExecWithReturn(ctx, Cmd, argtable, false);
 323     uint64_t internalid = arg_get_u64_def(ctx, 1, 0);
 324     CLIParserFree(ctx);
 325
 326     internalid |= 0x80000000;
 327     internalid <<= 3;
 328     internalid += 7;
 329
 330     uint8_t bs[64] = {0x00};
 331     // loop to bits
 332     uint8_t j = 0;
 333     for (int8_t i = 63; i >= 0; --i) {
 334         bs[j++] = ((internalid >> i) & 1);
 335     }
 336
 337     PrintAndLogEx(SUCCESS, "Simulating KERI - Internal Id " _YELLOW_("%" PRIu64), internalid);
 338
 339     lf_psksim_t *payload = calloc(1, sizeof(lf_psksim_t) + sizeof(bs));
 340     payload->carrier =  2;
 341     payload->invert = 0;
 342     payload->clock = 32;
 343     memcpy(payload->data, bs, sizeof(bs));
 344
 345     clearCommandBuffer();
 346     SendCommandNG(CMD_LF_PSK_SIMULATE, (uint8_t *)payload,  sizeof(lf_psksim_t) + sizeof(bs));
 347     free(payload);
 348
 349     PacketResponseNG resp;
 350     WaitForResponse(CMD_LF_PSK_SIMULATE, &resp);
 351
 352     PrintAndLogEx(INFO, "Done");
 353     if (resp.status != PM3_EOPABORTED)
 354         return resp.status;
 355     return PM3_SUCCESS;
 356 }
 357
 358 static command_t CommandTable[] = {
 359     {"help",   CmdHelp,       AlwaysAvailable, "This help"},
 360     {"demod",  CmdKeriDemod,  AlwaysAvailable, "demodulate an KERI tag from the GraphBuffer"},
 361     {"reader", CmdKeriReader, IfPm3Lf,         "attempt to read and extract tag data"},
 362     {"clone",  CmdKeriClone,  IfPm3Lf,         "clone KERI tag to T55x7 or Q5/T5555"},
 363     {"sim",    CmdKeriSim,    IfPm3Lf,         "simulate KERI tag"},
 364     {NULL, NULL, NULL, NULL}
 365 };
 366
 367 static int CmdHelp(const char *Cmd) {
 368     (void)Cmd; // Cmd is not used so far
 369     CmdsHelp(CommandTable);
 370     return PM3_SUCCESS;
 371 }
 372
 373 int CmdLFKeri(const char *Cmd) {
 374     clearCommandBuffer();
 375     return CmdsParse(CommandTable, Cmd);
 376 }
 377
 378 // find KERI preamble in already demoded data
 379 int detectKeri(uint8_t *dest, size_t *size, bool *invert) {
 380
 381     uint8_t preamble[] = {1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
 382
 383     // sanity check.
 384     if (*size < sizeof(preamble)) return -1;
 385
 386     size_t startIdx = 0;
 387     size_t found_size = *size;
 388
 389     if (!preambleSearch(dest, preamble, sizeof(preamble), &found_size, &startIdx)) {
 390
 391         found_size = *size;
 392         // if didn't find preamble try again inverting
 393         uint8_t preamble_i[] = {0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0};
 394         if (!preambleSearch(DemodBuffer, preamble_i, sizeof(preamble_i), &found_size, &startIdx))
 395             return -2;
 396
 397         *invert ^= 1;
 398     }
 399
 400     if (found_size < 64) return -3; //wrong demoded size
 401
 402     *size = found_size;
 403
 404     return (int)startIdx;
 405 }
 406