EM410x bug fix

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 5b0bc29d28dfb61fd8daf75e07ceb64603c930be..92ad633e7e8e200abf8214da1c9ffd5c8b3e3ae4 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -88,37 +88,34 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_
     return 0;
   }
   // 111111111 bit pattern represent start of frame
-  uint8_t preamble[] = {1,1,1,1,1,1,1,1,1};
+  //  include 0 in front to help get start pos
+  uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1};
   uint32_t idx = 0;
   uint32_t parityBits = 0;
   uint8_t errChk = 0;
   uint8_t FmtLen = 10;
   *startIdx = 0;
-  for (uint8_t extraBitChk=0; extraBitChk<5; extraBitChk++){
-    errChk = preambleSearch(BitStream+extraBitChk+*startIdx, preamble, sizeof(preamble), size, startIdx);
-    if (errChk == 0) return 0;
-    if (*size>64) FmtLen = 22;
-    if (*size<64) return 0;
-    idx = *startIdx + 9;
-    for (i=0; i<FmtLen; i++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
-      parityBits = bytebits_to_byte(BitStream+(i*5)+idx,5);
-      //check even parity
-      if (parityTest(parityBits, 5, 0) == 0){
-        //parity failed try next bit (in the case of 1111111111) but last 9 = preamble
-        startIdx++;
-        errChk = 0;
-        break;
-      }
-      //set uint64 with ID from BitStream
-      for (uint8_t ii=0; ii<4; ii++){
-        *hi = (*hi << 1) | (*lo >> 63);
-        *lo = (*lo << 1) | (BitStream[(i*5)+ii+idx]);
-      }
+  errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, startIdx);
+  if (errChk == 0 || *size < 64) return 0;
+  if (*size > 64) FmtLen = 22;
+  *startIdx += 1; //get rid of 0 from preamble
+  idx = *startIdx + 9;
+  for (i=0; i<FmtLen; i++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
+    parityBits = bytebits_to_byte(BitStream+(i*5)+idx,5);
+    //check even parity
+    if (parityTest(parityBits, 5, 0) == 0){
+      //parity failed quit
+       return 0;
+    }
+    //set uint64 with ID from BitStream
+    for (uint8_t ii=0; ii<4; ii++){
+      *hi = (*hi << 1) | (*lo >> 63);
+      *lo = (*lo << 1) | (BitStream[(i*5)+ii+idx]);
     }
-    if (errChk != 0) return 1;
-    //skip last 5 bit parity test for simplicity.
-    // *size = 64 | 128;
   }
+  if (errChk != 0) return 1;
+  //skip last 5 bit parity test for simplicity.
+  // *size = 64 | 128;
   return 0;
 }
 
@@ -375,34 +372,39 @@ int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int
                        smplCnt++;
                } else if (BinStream[i] <= low && !waveHigh){
                        smplCnt++;
-               } else { //not high or low or a transition
-                       if (smplCnt > clk-(clk/4)) { //full clock
-                               if (smplCnt > clk + (clk/4)) { //too many samples
-                                       errCnt++;
-                                       BinStream[bitCnt++]=77;
-                               } else if (waveHigh) {
-                                       BinStream[bitCnt++] = invert;
-                                       BinStream[bitCnt++] = invert;
-                               } else if (!waveHigh) {
-                                       BinStream[bitCnt++] = invert ^ 1;
-                                       BinStream[bitCnt++] = invert ^ 1;
-                               }
-                               waveHigh ^= 1;  
-                               smplCnt = 0;
-                       } else if (smplCnt > (clk/2) - (clk/5)) {
-                               if (waveHigh) {
-                                       BinStream[bitCnt++] = invert;
-                               } else if (!waveHigh) {
-                                       BinStream[bitCnt++] = invert ^ 1;
+               } else { //transition
+                       if ((BinStream[i] >= high && !waveHigh) || (BinStream[i] <= low && waveHigh)){
+                               if (smplCnt > clk-(clk/4)-1) { //full clock
+                                       if (smplCnt > clk + (clk/4)+1) { //too many samples
+                                               errCnt++;
+                                               BinStream[bitCnt++]=77;
+                                       } else if (waveHigh) {
+                                               BinStream[bitCnt++] = invert;
+                                               BinStream[bitCnt++] = invert;
+                                       } else if (!waveHigh) {
+                                               BinStream[bitCnt++] = invert ^ 1;
+                                               BinStream[bitCnt++] = invert ^ 1;
+                                       }
+                                       waveHigh ^= 1;  
+                                       smplCnt = 0;
+                               } else if (smplCnt > (clk/2) - (clk/4)-1) {
+                                       if (waveHigh) {
+                                               BinStream[bitCnt++] = invert;
+                                       } else if (!waveHigh) {
+                                               BinStream[bitCnt++] = invert ^ 1;
+                                       }
+                                       waveHigh ^= 1;  
+                                       smplCnt = 0;
+                               } else if (!bitCnt) {
+                                       //first bit
+                                       waveHigh = (BinStream[i] >= high);
+                                       smplCnt = 1;
+                               } else {
+                                       smplCnt++;
+                                       //transition bit oops
                                }
-                               waveHigh ^= 1;  
-                               smplCnt = 0;
-                       } else if (!bitCnt) {
-                               //first bit
-                               waveHigh = (BinStream[i] >= high);
-                               smplCnt = 1;
-                       } else {
-                               //transition bit? ignore
+                       } else { //haven't hit new high or new low yet
+                               smplCnt++;
                        }
                }
        }
@@ -888,16 +890,18 @@ int PyramiddemodFSK(uint8_t *dest, size_t *size)
 
 uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, int high, int low)
 {
-       uint8_t allPeaks=1;
+       uint16_t allPeaks=1;
        uint16_t cntPeaks=0;
-       for (size_t i=30; i<255; i++){
+       size_t loopEnd = 572;
+       if (loopEnd > size) loopEnd = size;
+       for (size_t i=60; i<loopEnd; i++){
                if (dest[i]>low && dest[i]<high) 
                        allPeaks=0;
                else
                        cntPeaks++;
        }
-       if (allPeaks==0){
-               if (cntPeaks>210) return 1;
+       if (allPeaks == 0){
+               if (cntPeaks > 300) return 1;
        }
        return allPeaks;
 }
@@ -939,10 +943,12 @@ int DetectStrongAskClock(uint8_t dest[], size_t size)
                        }
                }
        }
+       uint8_t tol;
        for (idx=8; idx>0; idx--){
-               if (clk[idx] >= highCnt && clk[idx] <= highCnt+2)
+               tol = clk[idx]/8;
+               if (clk[idx] >= highCnt - tol && clk[idx] <= highCnt + tol)
                        return clk[idx];
-               if (clk[idx] >= highCnt2 && clk[idx] <= highCnt2+2)
+               if (clk[idx] >= highCnt2 - tol && clk[idx] <= highCnt2 + tol)
                        return clk[idx];
        }
        return -1;