ADD: @marshmellow42 's fixed version of the ISO11784 FDX-B

ADD: crc16_ccitt_rev  (reverse version of crc16_ccitt 0x0000)

diff --git a/client/cmddata.c b/client/cmddata.c
index 3b9c3a663d61c84d20ad93bd67fab5a5af9de2e2..0f336f1084e27b08f84f28b1232b106be48c4c83 100644 (file)
--- a/client/cmddata.c
+++ b/client/cmddata.c
@@ -23,6 +23,8 @@
 #include "lfdemod.h"
 #include "usb_cmd.h"
 #include "crc.h"
+#include "crc16.h"
+
 
 uint8_t DemodBuffer[MAX_DEMOD_BUF_LEN];
 uint8_t g_debugMode;
@@ -1457,11 +1459,11 @@ int CmdFSKdemodPyramid(const char *Cmd)
        return 1;
 }
 
-// ISO11784/85 demod  (aka animal tag)  BIPHASE, inverted, rf/32,  with preamble of 00000000001 (128bits)
+// FDX-B ISO11784/85 demod  (aka animal tag)  BIPHASE, inverted, rf/32,  with preamble of 00000000001 (128bits)
 // 8 databits + 1 parity (1)
 // CIITT 16 chksum
 // NATIONAL CODE, ICAR database
-// COUNTRY CODE (ISO3166) 
+// COUNTRY CODE (ISO3166) or http://cms.abvma.ca/uploads/ManufacturersISOsandCountryCodes.pdf
 // FLAG (animal/non-animal)
 /*
 38 IDbits   
@@ -1472,62 +1474,81 @@ int CmdFSKdemodPyramid(const char *Cmd)
 16 ccitt CRC chksum over 64bit ID CODE.
 24 appli bits.
 
--- sample: 985121004515220
-
-Now is nibble shifting, byte shifting.
+-- sample: 985121004515220  [ 37FF65B88EF94 ]
 */
-int CmdIso11784demodBI(const char *Cmd){
+int CmdFDXBdemodBI(const char *Cmd){
 
        int invert = 1;
        int clk = 32;           
        int errCnt = 0;
+       int maxErr = 0;
        uint8_t BitStream[MAX_DEMOD_BUF_LEN];   
        size_t size = getFromGraphBuf(BitStream);       
        
-       errCnt = askdemod(BitStream, &size, &clk, &invert, 0, 0, 0);
-       if ( errCnt<0 ) { 
-               if (g_debugMode) PrintAndLog("DEBUG: no data found %d, clock: 32", errCnt);
+       errCnt = askdemod(BitStream, &size, &clk, &invert, maxErr, 0, 0);
+       if ( errCnt < 0 || errCnt > maxErr ) { 
+               if (g_debugMode) PrintAndLog("DEBUG: no data or error found %d, clock: 32", errCnt);
                return 0;
        }
 
-       errCnt = BiphaseRawDecode(BitStream, &size, 0, 1);
-       if (errCnt < 0){
+       errCnt = BiphaseRawDecode(BitStream, &size, maxErr, 1);
+       if (errCnt < 0 || errCnt > maxErr ) {
                if (g_debugMode) PrintAndLog("Error BiphaseRawDecode: %d", errCnt);
                return 0;
        } 
        
-       int preambleIndex = ISO11784demodBI(BitStream, &size);
+       int preambleIndex = FDXBdemodBI(BitStream, &size);
        if (preambleIndex < 0){
-               if (g_debugMode) PrintAndLog("Error ISO11784Demod , no startmarker found :: %d",preambleIndex);
+               if (g_debugMode) PrintAndLog("Error FDXBDemod , no startmarker found :: %d",preambleIndex);
                return 0;
        }
        
-       PrintAndLog("startmarker %d;   Size %d", preambleIndex, size);
+       setDemodBuf(BitStream, 128, preambleIndex);
 
-       //got a good demod
-       uint8_t ByteStream[16] = {0x00};
-       uint8_t bitCnt = 0;
-       uint8_t ByteCnt = 0;
-       size_t startIdx = preambleIndex + 11; //start after preamble
-       for (size_t idx = 0; idx < size; idx++){
-
-               if ( bitCnt == 9 ){
-                       bitCnt = 0;
-                       continue;
-               }
-               //lsb first
-               ByteStream[ByteCnt] |= ( BitStream[startIdx+idx] << bitCnt );
-               bitCnt++;
-               if (bitCnt % 8 == 0){
-                       if (g_debugMode) PrintAndLog("byte %d: %02x", ByteCnt, ByteStream[ByteCnt]);
-                       bitCnt = 9;
-                       ByteCnt++;
-               }
+       size = removeParity(BitStream, preambleIndex + 11, 9, 2, 128-11);
+       if ( size <= 103 ) {
+               if (g_debugMode) PrintAndLog("Error removeParity:: %d", size);
+               return 0;
        }
-       PrintAndLog("DATA:  %s", sprint_hex(ByteStream, 14));
-       //now ByteStream contains 16 bytes of decrypted raw tag data
-       setDemodBuf(BitStream+preambleIndex, 128, 0);
-       printDemodBuff();
+       if (g_debugMode) {
+               char *bin = sprint_bin_break(BitStream,size,16);
+               PrintAndLog("DEBUG BinStream:\n%s",bin);
+       }
+       PrintAndLog("\nFDX-B / ISO 11784/5 Animal Tag ID Found:");
+       if (g_debugMode) PrintAndLog("startmarker %d;   Size %d", preambleIndex, size);
+
+       //got a good demod        
+       
+       //marshmellows
+       uint64_t NationalCode = ((uint64_t)(bytebits_to_byteLSBF(BitStream+32,6)) << 32) | bytebits_to_byteLSBF(BitStream,32);
+       uint32_t countryCode = bytebits_to_byteLSBF(BitStream+38,10);
+       uint8_t dataBlockBit = BitStream[48];
+       uint32_t reservedCode = bytebits_to_byteLSBF(BitStream+49,14);
+       uint8_t animalBit = BitStream[63];
+
+       uint16_t crc16 = bytebits_to_byteLSBF(BitStream+64,16);
+       uint32_t extended = bytebits_to_byteLSBF(BitStream+80,24);
+
+       uint64_t rawid = ((uint64_t)bytebits_to_byte(BitStream+32,32) << 32) | bytebits_to_byte(BitStream,32);
+       uint8_t raw[8];
+       num_to_bytes(rawid, 8, raw);
+       PrintAndLog("%s", sprint_hex(raw,8));
+       uint16_t crcCalc = crc16_ccitt_rev( raw ,8);
+       
+       PrintAndLog("Animal ID:     %u-%012llu", countryCode, NationalCode);
+       PrintAndLog("National Code: %012llu", NationalCode);
+       PrintAndLog("CountryCode:   %u", countryCode);
+       PrintAndLog("Extended Data: %s", dataBlockBit ? "True" : "False");
+       PrintAndLog("reserved Code: %u", reservedCode);
+       PrintAndLog("Animal Tag:    %s", animalBit ? "True" : "False");
+       PrintAndLog("CRC:           0x%02X", crc16);
+       PrintAndLog("CRC : %X == %X  %s", crc16, crcCalc, ( crcCalc == crc16 )?"ok":"!"  );
+       PrintAndLog("Extended:      0x%X", extended);      
+       
+/*     
+       //uint16_t crcCalc = crc16_ccitt( ByteStream, 8);
+       PrintAndLog("Application ID: %04X", applicationid);
+       */
        return 1;
        
 }
@@ -1947,7 +1968,7 @@ typedef struct {
        uint8_t * buffer;
        uint32_t numbits;
        uint32_t position;
-}BitstreamOut;
+} BitstreamOut;
 
 bool _headBit( BitstreamOut *stream)
 {
@@ -2277,6 +2298,7 @@ static command_t CommandTable[] =
        {"buffclear",       CmdBuffClear,       1, "Clear sample buffer and graph window"},
        {"dec",             CmdDec,             1, "Decimate samples"},
        {"detectclock",     CmdDetectClockRate, 1, "[modulation] Detect clock rate of wave in GraphBuffer (options: 'a','f','n','p' for ask, fsk, nrz, psk respectively)"},
+       {"fdxbdemod",       CmdFDXBdemodBI    , 1, "Demodulate a FDX-B ISO11784/85 Biphase tag from GraphBuffer"},
        {"fskawiddemod",    CmdFSKdemodAWID,    1, "Demodulate an AWID FSK tag from GraphBuffer"},
        //{"fskfcdetect",   CmdFSKfcDetect,     1, "Try to detect the Field Clock of an FSK wave"},
        {"fskhiddemod",     CmdFSKdemodHID,     1, "Demodulate a HID FSK tag from GraphBuffer"},
@@ -2288,7 +2310,6 @@ static command_t CommandTable[] =
        {"hexsamples",      CmdHexsamples,      0, "<bytes> [<offset>] -- Dump big buffer as hex bytes"},
        {"hide",            CmdHide,            1, "Hide graph window"},
        {"hpf",             CmdHpf,             1, "Remove DC offset from trace"},
-       {"iso11784demod",   CmdIso11784demodBI, 1, "Demodulate a ISO11784/85 Biphase tag from GraphBuffer"},
        {"load",            CmdLoad,            1, "<filename> -- Load trace (to graph window"},
        {"ltrim",           CmdLtrim,           1, "<samples> -- Trim samples from left of trace"},
        {"rtrim",           CmdRtrim,           1, "<location to end trace> -- Trim samples from right of trace"},

diff --git a/client/cmddata.h b/client/cmddata.h
index 4ad5fbd0c63d8d4fa0847af05b11bdffcf867719..fcc51a6bee079068127950e46a2fde9dd9edf3bf 100644 (file)
--- a/client/cmddata.h
+++ b/client/cmddata.h
@@ -27,13 +27,13 @@ int CmdBitsamples(const char *Cmd);
 int CmdBuffClear(const char *Cmd);
 int CmdDec(const char *Cmd);
 int CmdDetectClockRate(const char *Cmd);
+int CmdFDXBdemodBI(const char *Cmd);
 int CmdFSKdemodAWID(const char *Cmd);
 int CmdFSKdemodHID(const char *Cmd);
 int CmdFSKdemodIO(const char *Cmd);
 int CmdFSKdemodParadox(const char *Cmd);
 int CmdFSKdemodPyramid(const char *Cmd);
 int CmdFSKrawdemod(const char *Cmd);
-int CmdIso11784demodBI(const char *Cmd);
 int CmdPSK1rawDemod(const char *Cmd);
 int CmdPSK2rawDemod(const char *Cmd);
 int CmdPSKNexWatch(const char *Cmd);

diff --git a/client/cmdlf.c b/client/cmdlf.c
index 7c5d273ff175da047df06b0a06af48c67df4f679..aa7f63a9c0830fc6bdfcab57296fc5250529f30a 100644 (file)
--- a/client/cmdlf.c
+++ b/client/cmdlf.c
@@ -1077,6 +1077,12 @@ int CmdLFfind(const char *Cmd)
     return 1;
   }
 
+       ans=CmdFDXBdemodBI("");
+       if (ans>0) {
+               PrintAndLog("\nValid FDX-B ID Found!");
+               return 1;
+       }
+
        ans=EM4x50Read("", false);
        if (ans>0) {
                PrintAndLog("\nValid EM4x50 ID Found!");

diff --git a/client/cmdlft55xx.c b/client/cmdlft55xx.c
index cf19c0ecf0390c4fb96c13adaffff4cd81cda860..2953e7802726094cfef7a8005796765ec0daf9e4 100644 (file)
--- a/client/cmdlft55xx.c
+++ b/client/cmdlft55xx.c
@@ -1050,6 +1050,7 @@ char * GetSelectedModulationStr( uint8_t id){
        return buf;\r
 }\r
 \r
+/*\r
 uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits){\r
        \r
        int i = start;\r
@@ -1063,7 +1064,7 @@ uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits){
 \r
        return tmp;\r
 }\r
-\r
+*/\r
 static command_t CommandTable[] =\r
 {\r
   {"help",   CmdHelp,           1, "This help"},\r

diff --git a/client/util.c b/client/util.c
index 30547080b7714b6ac02f1ccd830116186c4f0c4a..66e0a6979d7ab92ae99e96d64baecacaf670af83 100644 (file)
--- a/client/util.c
+++ b/client/util.c
@@ -453,3 +453,18 @@ void xor(unsigned char * dst, unsigned char * src, size_t len) {
 int32_t le24toh (uint8_t data[3]) {
     return (data[2] << 16) | (data[1] << 8) | data[0];
 }
+
+
+uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits) {
+       
+       int i = start;
+       int j = len-1;
+
+       if (len > 32) return 0;
+
+       uint32_t tmp = 0;
+       for (; j >= 0; --j, ++i)
+               tmp     |= bits[i] << j;
+
+       return tmp;
+}

diff --git a/client/util.h b/client/util.h
index 4c7e93a7b92ca8675bbb62f7984f6f38ddf39fcf..a57daf82c9dc8d5e8221edec02f1727801c8830a 100644 (file)
--- a/client/util.h
+++ b/client/util.h
@@ -65,4 +65,4 @@ void wiegand_add_parity(char *target, char *source, char length);
 
 void xor(unsigned char * dst, unsigned char * src, size_t len);
 int32_t le24toh (uint8_t data[3]);
-
+uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits);

diff --git a/common/crc16.c b/common/crc16.c
index 973cd103c009b16697b7523b1cbf60b107500f7a..a04c40127c82a996cff60985c1650ca629772c19 100644 (file)
--- a/common/crc16.c
+++ b/common/crc16.c
@@ -43,3 +43,7 @@ uint16_t crc16(uint8_t const *message, int length, uint16_t remainder, uint16_t
 uint16_t crc16_ccitt(uint8_t const *message, int length) {
     return crc16(message, length, 0xffff, 0x1021);
 }
+
+uint16_t crc16_ccitt_rev(uint8_t const *message, int length) {
+    return crc16(message, length, 0x0000, 0x1021);
+}
\ No newline at end of file

diff --git a/common/crc16.h b/common/crc16.h
index d16d83b5afdb162c088e35e4a01aee33e2760e3d..254e7fdf759a7231f51ebc2a10af1674a7cd395e 100644 (file)
--- a/common/crc16.h
+++ b/common/crc16.h
@@ -12,4 +12,5 @@
 unsigned short update_crc16(unsigned short crc, unsigned char c);
 uint16_t crc16(uint8_t const *message, int length, uint16_t remainder, uint16_t polynomial);
 uint16_t crc16_ccitt(uint8_t const *message, int length);
+uint16_t crc16_ccitt_rev(uint8_t const *message, int length);
 #endif

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 62f52a2f0f4d2d3f0cce596b38a9f32b52ebadf2..94e08ffa7563ad3569f2473057b476bf041db6cb 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -537,6 +537,17 @@ uint32_t bytebits_to_byte(uint8_t* src, size_t numbits)
        return num;
 }
 
+//least significant bit first
+uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits)
+{
+       uint32_t num = 0;
+       for(int i = 0 ; i < numbits ; i++)
+       {
+               num = (num << 1) | *(src + (numbits-(i+1)));
+       }
+       return num;
+}
+
 int IOdemodFSK(uint8_t *dest, size_t size)
 {
        if (justNoise(dest, size)) return -1;
@@ -569,7 +580,7 @@ int IOdemodFSK(uint8_t *dest, size_t size)
 
 // by marshmellow
 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
-//   Parity Type (1 for odd 0 for even), and binary Length (length to run) 
+//   Parity Type (1 for odd; 0 for even; 2 for just drop it), and binary Length (length to run) 
 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen)
 {
        uint32_t parityWd = 0;
@@ -581,7 +592,9 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p
                }
                j--;
                // if parity fails then return 0
+               if (pType != 2) {
                if (parityTest(parityWd, pLen, pType) == 0) return -1;
+               }
                bitCnt+=(pLen-1);
                parityWd = 0;
        }
@@ -589,9 +602,10 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p
        //return ID start index and size
        return bitCnt;
 }
+
 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
 // BitStream must contain previously askrawdemod and biphasedemoded data
-int ISO11784demodBI(uint8_t *dest, size_t *size)
+int FDXBdemodBI(uint8_t *dest, size_t *size)
 {
        //make sure buffer has enough data
        if (*size < 128) return -1;

diff --git a/common/lfdemod.h b/common/lfdemod.h
index f2cee27f786e44c2febb2e631fc76e68e4e2d270..d16aab9eb4205ff1f34a1438411ad04bc1f0820e 100644 (file)
--- a/common/lfdemod.h
+++ b/common/lfdemod.h
@@ -19,6 +19,7 @@
 int      askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType);
 int      BiphaseRawDecode(uint8_t * BitStream, size_t *size, int offset, int invert);
 uint32_t bytebits_to_byte(uint8_t* src, size_t numbits);
+uint32_t bytebits_to_byteLSBF(uint8_t* src, size_t numbits);
 uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj);
 int      DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr);
 uint8_t  DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low);
@@ -40,6 +41,7 @@ void     psk1TOpsk2(uint8_t *BitStream, size_t size);
 size_t   removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen);
 
 //tag specific
+int FDXBdemodBI(uint8_t *dest, size_t *size);
 int AWIDdemodFSK(uint8_t *dest, size_t *size);
 int gProxII_Demod(uint8_t BitStream[], size_t *size);
 int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo);
@@ -47,5 +49,5 @@ int IOdemodFSK(uint8_t *dest, size_t size);
 int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert);
 int PyramiddemodFSK(uint8_t *dest, size_t *size);
 int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo);
-int ISO11784demodBI(uint8_t *dest, size_t *size);
+
 #endif