ADD: next step ISO11784/85

diff --git a/client/cmddata.c b/client/cmddata.c
index 90a0921ca75152d59139e420a4d534d1773bdc4a..87cf4bac7dc4995e9e80d52e82c4060b0515dd1f 100644 (file)
--- a/client/cmddata.c
+++ b/client/cmddata.c
@@ -1457,21 +1457,65 @@ int CmdFSKdemodPyramid(const char *Cmd)
        return 1;
 }
 
+// ISO11784/85 demod  (aka animal tag)  BIPHASE rf/32,  with preamble of 00000000001 (128bits)
+// 8 databits 1 parity
+// CIITT 16 chksum
+// NATIONAL CODE, ICAR database
+// COUNTRY CODE (ISO3166) 
+// FLAG (animal/non-animal)
 int CmdIso11784demodBI(const char *Cmd){
-       //ASK/Biphase demod,
-       uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0};
-       size_t size = getFromGraphBuf(BitStream);
-       if (size==0) return 0;
-
-       //get binary from Biphase wave
-       int idx = ISO11784demodBI(BitStream, &size);
-       setDemodBuf(BitStream,128,idx);
+               
+       if (!ASKbiphaseDemod(Cmd, FALSE)){
+               if (g_debugMode) PrintAndLog("ASKbiphaseDemod failed 1st try");
+               return 0;
+       }
+       size_t size = DemodBufferLen;
 
-       size = removeParity(BitStream, idx+8, 4, 1, 88);
-       // if (size != 66){
-               // if (g_debugMode==1) PrintAndLog("DEBUG: Error - at parity check-tag size does not match AWID format");
-               // return 0;
-       // }
+       int ans = ISO11784demodBI(DemodBuffer, &size);
+       if (ans < 0){
+               if (g_debugMode) PrintAndLog("Error ISO11784Demod");
+               return 0;
+       }
+       
+       //size = removeParity(BitStream, idx+11, 9, 1, 104);
+       
+       //got a good demod
+       uint32_t ByteStream[13] = {0x00};
+       uint8_t xorKey=0;
+       uint8_t keyCnt=0;
+       uint8_t bitCnt=0;
+       uint8_t ByteCnt=0;
+       size_t startIdx = ans + 11; //start after preamble
+       for (size_t idx = 0; idx<size-11; idx++){
+               if ((idx+1) % 5 == 0){
+                       //spacer bit - should be 0
+                       if (DemodBuffer[startIdx+idx] != 0) {
+                               if (g_debugMode) PrintAndLog("Error spacer not 0: %d, pos: %d",DemodBuffer[startIdx+idx],startIdx+idx);
+                               return 0;
+                       }
+                       continue;
+               } 
+               if (keyCnt<8){ //lsb first
+                       xorKey = xorKey | (DemodBuffer[startIdx+idx]<<keyCnt);
+                       keyCnt++;
+                       if (keyCnt==8 && g_debugMode) PrintAndLog("xorKey Found: %02x", xorKey);
+                       continue;
+               }
+               //lsb first
+               ByteStream[ByteCnt] = ByteStream[ByteCnt] | (DemodBuffer[startIdx+idx]<<bitCnt);
+               bitCnt++;
+               if (bitCnt % 8 == 0){
+                       if (g_debugMode) PrintAndLog("byte %d: %02x",ByteCnt,ByteStream[ByteCnt]);
+                       bitCnt=0;
+                       ByteCnt++;
+               }
+       }
+       for (uint8_t i = 0; i < ByteCnt; i++){
+               ByteStream[i] ^= xorKey; //xor
+               if (g_debugMode) PrintAndLog("byte %d after xor: %02x", i, ByteStream[i]);
+       }
+       //now ByteStream contains 13 bytes of decrypted raw tag data
+       setDemodBuf(DemodBuffer+ans, 104, 0);
        return 1;
 }
 

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 2c65014409eebcb09ca22ea6a85d79f9649b179f..62f52a2f0f4d2d3f0cce596b38a9f32b52ebadf2 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -590,22 +590,17 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p
        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)
 {
        //make sure buffer has enough data
-       if (*size < 128*50) return -1;
-
-       if (justNoise(dest, *size)) return -2;
+       if (*size < 128) return -1;
 
-       // FSK demodulator
-       *size = fskdemod(dest, *size, 50, 1, 10, 8);  // fsk2a RF/50 
-       if (*size < 96) return -3;  //did we get a good demod?
-
-       uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,1};
        size_t startIdx = 0;
+       uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,1};
+
        uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
-       if (errChk == 0) return -4; //preamble not found
-       if (*size != 128) return -5;
+       if (errChk == 0) return -2; //preamble not found
        return (int)startIdx;
 }