From 49f07a39d59219ecdccaaea8c04ed9ef11014e7a Mon Sep 17 00:00:00 2001
From: iceman1001 <iceman@iuse.se>
Date: Thu, 18 Jan 2024 16:20:35 +0100
Subject: [PATCH] lf idteck demod takes a raw hex string and tries to decode it

---
 CHANGELOG.md             |  1 +
 client/src/cmdlf.c       |  2 +-
 client/src/cmdlfidteck.c | 82 +++++++++++++++++++++++++++++++++-------
 3 files changed, 71 insertions(+), 14 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index a93ec51fb..5eac8d039 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -3,6 +3,7 @@ All notable changes to this project will be documented in this file.
 This project uses the changelog in accordance with [keepchangelog](http://keepachangelog.com/). Please use this to write notable changes, which is not the same as git commit log...
 
 ## [unreleased][unreleased]
+ - Changed `lf idteck demod --raw` - now supports raw hex string to decode (@iceman1001)
  - Changed `lf em 410x demod --bin` - now supports a raw binary string to demodulate. (@iceman1001)
  - Changed `lf em 4x05 info` - use `-v` verbose flag to see protection bits (@iceman1001)
  - Changed `lf em 4x05 info` - output now shows detailed bits (@iceman1001)
diff --git a/client/src/cmdlf.c b/client/src/cmdlf.c
index ab8c3a123..62ce28061 100644
--- a/client/src/cmdlf.c
+++ b/client/src/cmdlf.c
@@ -1859,7 +1859,7 @@ int CmdLFfind(const char *Cmd) {
     }
 
     // psk
-    if (demodIdteck(true) == PM3_SUCCESS) {
+    if (demodIdteck(NULL, true) == PM3_SUCCESS) {
         PrintAndLogEx(SUCCESS, "\nValid " _GREEN_("Idteck ID") " found!");
         if (search_cont) {
             found++;
diff --git a/client/src/cmdlfidteck.c b/client/src/cmdlfidteck.c
index e39354ef8..ba95d937f 100644
--- a/client/src/cmdlfidteck.c
+++ b/client/src/cmdlfidteck.c
@@ -32,18 +32,19 @@
 #include "cmdlfem4x05.h" // EM defines
 #include "protocols.h"   // T55x7 defines
 #include "cmdlft55xx.h"  // verifywrite
+#include "generator.h"
+#include "wiegand_formats.h"
 
 static int CmdHelp(const char *Cmd);
 
-int demodIdteck(bool verbose) {
-    (void) verbose; // unused so far
+static int demod_idteck_signal(void) {
     if (PSKDemod(0, 0, 100, false) != PM3_SUCCESS) {
         PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck PSKDemod failed");
         return PM3_ESOFT;
     }
     size_t size = g_DemodBufferLen;
 
-    //get binary from PSK1 wave
+    // get binary from PSK1 wave
     int idx = detectIdteck(g_DemodBuffer, &size);
     if (idx < 0) {
 
@@ -81,17 +82,67 @@ int demodIdteck(bool verbose) {
         }
     }
     setDemodBuff(g_DemodBuffer, 64, idx);
+    return PM3_SUCCESS; 
+}
 
-    //got a good demod
+int demodIdteck(uint8_t *raw, bool verbose) {
+    (void) verbose; // unused so far
+
+    uint32_t raw1 = 0;
+    uint32_t raw2 = 0;
+
+    if (raw == NULL) {
+        // get binary from PSK1 wave
+        int ret = demod_idteck_signal();
+        if (ret != PM3_SUCCESS) {
+            return ret;
+        }
+        raw1 = bytebits_to_byte(g_DemodBuffer, 32);
+        raw2 = bytebits_to_byte(g_DemodBuffer + 32, 32);
+    } else {
+        raw1 = bytes_to_num(raw, 4);
+        raw2 = bytes_to_num(raw + 4, 4);
+    }
+
+    // got a good demod
     uint32_t id = 0;
-    uint32_t raw1 = bytebits_to_byte(g_DemodBuffer, 32);
-    uint32_t raw2 = bytebits_to_byte(g_DemodBuffer + 32, 32);
 
-    //parity check (TBD)
-    //checksum check (TBD)
+    if (raw1 != 0x4944544B) {
+        PrintAndLogEx(FAILED, "No genuine IDTECK found");
+    }
+   
+    // parity check (TBD)
 
-    //output
-    PrintAndLogEx(SUCCESS, "IDTECK Tag Found: Card ID %u ,  Raw: %08X%08X", id, raw1, raw2);
+    uint32_t tmp = raw2;
+    lf_idteck_decrypt((uint16_t*)&tmp);
+
+    // checksum check (TBD)
+    // 351FBE4B  ->  90370752   Card Code  [523707]  CUSUM = 52+37+07=90
+    // So, first byte is Csum
+    uint8_t chksum = ((tmp >> 24) & 0xFF);
+    uint8_t calc = ((tmp >> 16) & 0xFF) +
+        ((tmp >> 8) & 0xFF) +
+        (tmp & 0xFF);
+    
+    id = ((tmp >> 16) & 0xFF) | ((tmp >> 8) & 0xFF) << 8 | (tmp & 0xFF) << 16;
+
+    // output
+    PrintAndLogEx(SUCCESS, "IDTECK Tag Found: Card ID " _GREEN_("%u") " ( 0x%06X ) Raw: %08X%08X  chksum 0x%02X ( %s )", 
+        id,
+        id,
+        raw1,
+        raw2,
+        chksum,
+        (chksum == calc) ? _GREEN_("ok") : _RED_("fail")
+    );
+
+    wiegand_message_t packed = {
+        .Bot = id,
+        .Mid = 0,
+        .Top = 0,
+        .Length = 26
+    };
+    HIDUnpack(0, &packed);
     return PM3_SUCCESS;
 }
 
@@ -99,16 +150,21 @@ static int CmdIdteckDemod(const char *Cmd) {
     CLIParserContext *ctx;
     CLIParserInit(&ctx, "lf idteck demod",
                   "Try to find Idteck preamble, if found decode / descramble data",
-                  "lf idteck demod"
+                  "lf idteck demod\n"
+                  "lf idteck demod --raw 4944544B351FBE4B"
                  );
 
     void *argtable[] = {
         arg_param_begin,
+        arg_str0("r", "raw", "<hex>", "raw bytes"),
         arg_param_end
     };
     CLIExecWithReturn(ctx, Cmd, argtable, true);
+    int raw_len = 0;
+    uint8_t raw[8] = {0};
+    CLIGetHexWithReturn(ctx, 1, raw, &raw_len);
     CLIParserFree(ctx);
-    return demodIdteck(true);
+    return demodIdteck(raw, true);
 }
 
 static int CmdIdteckClone(const char *Cmd) {
@@ -258,7 +314,7 @@ static int CmdIdteckReader(const char *Cmd) {
 
     do {
         lf_read(false, 5000);
-        demodIdteck(!cm);
+        demodIdteck(NULL, !cm);
     } while (cm && !kbd_enter_pressed());
 
     return PM3_SUCCESS;