EM410x bug fix

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 144cb32735c7269e3b6eea34ee25679309b0097d..92ad633e7e8e200abf8214da1c9ffd5c8b3e3ae4 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -55,7 +55,7 @@ uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType)
 }
 
 //by marshmellow
 }
 
 //by marshmellow

-//search for given preamble in given BitStream and return startIndex and length
+//search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length

 uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx)
 {
   uint8_t foundCnt=0;
 uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx)
 {
   uint8_t foundCnt=0;


@@ -75,48 +75,47 @@ uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_
   return 0;
 }
 
   return 0;
 }
 

-

 //by marshmellow
 //takes 1s and 0s and searches for EM410x format - output EM ID
 //by marshmellow
 //takes 1s and 0s and searches for EM410x format - output EM ID

-uint64_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx)
+uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo)

 {
   //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
   //  otherwise could be a void with no arguments
   //set defaults
 {
   //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
   //  otherwise could be a void with no arguments
   //set defaults

-  uint64_t lo=0;

   uint32_t i = 0;
   if (BitStream[1]>1){  //allow only 1s and 0s
     // PrintAndLog("no data found");
     return 0;
   }
   // 111111111 bit pattern represent start of frame
   uint32_t i = 0;
   if (BitStream[1]>1){  //allow only 1s and 0s
     // PrintAndLog("no data found");
     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;
   uint32_t idx = 0;
   uint32_t parityBits = 0;
   uint8_t errChk = 0;

+  uint8_t FmtLen = 10;

   *startIdx = 0;
   *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;
-    idx = *startIdx + 9;
-    for (i=0; i<10;i++){ //loop through 10 sets of 5 bits (50-10p = 40 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++){
-        lo = (lo << 1LL) | (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 lo;
-    //skip last 5 bit parity test for simplicity.
-    // *size = 64;

   }
   }

+  if (errChk != 0) return 1;
+  //skip last 5 bit parity test for simplicity.
+  // *size = 64 | 128;

   return 0;
 }
 
   return 0;
 }
 


@@ -151,6 +150,8 @@ int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max
        int iii = 0;
        uint32_t gLen = *size;
        if (gLen > 3000) gLen=3000;
        int iii = 0;
        uint32_t gLen = *size;
        if (gLen > 3000) gLen=3000;

+       //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance
+       if (!maxErr) gLen=*clk*2; 

        uint8_t errCnt =0;
        uint16_t MaxBits = 500;
        uint32_t bestStart = *size;
        uint8_t errCnt =0;
        uint16_t MaxBits = 500;
        uint32_t bestStart = *size;


@@ -260,16 +261,12 @@ int ManchesterEncode(uint8_t *BitStream, size_t size)
 //run through 2 times and take least errCnt
 int manrawdecode(uint8_t * BitStream, size_t *size)
 {
 //run through 2 times and take least errCnt
 int manrawdecode(uint8_t * BitStream, size_t *size)
 {

-       uint16_t bitnum=0;
-       uint16_t MaxBits = 500;
-       uint16_t errCnt = 0;
-       size_t i=1;
-       uint16_t bestErr = 1000;
-       uint16_t bestRun = 0;
-       size_t ii=1;
+       uint16_t bitnum=0, MaxBits = 512, errCnt = 0;
+       size_t i, ii;
+       uint16_t bestErr = 1000, bestRun = 0;

        if (size == 0) return -1;
        if (size == 0) return -1;

-       for (ii=1;ii<3;++ii){
-               i=1;
+       for (ii=0;ii<2;++ii){
+               i=0;

                for (i=i+ii;i<*size-2;i+=2){
                        if(BitStream[i]==1 && (BitStream[i+1]==0)){
                        } else if((BitStream[i]==0)&& BitStream[i+1]==1){
                for (i=i+ii;i<*size-2;i+=2){
                        if(BitStream[i]==1 && (BitStream[i+1]==0)){
                        } else if((BitStream[i]==0)&& BitStream[i+1]==1){


@@ -287,7 +284,7 @@ int manrawdecode(uint8_t * BitStream, size_t *size)
        errCnt=bestErr;
        if (errCnt<20){
                ii=bestRun;
        errCnt=bestErr;
        if (errCnt<20){
                ii=bestRun;

-               i=1;
+               i=0;

                for (i=i+ii; i < *size-2; i+=2){
                        if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
                                BitStream[bitnum++]=0;
                for (i=i+ii; i < *size-2; i+=2){
                        if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
                                BitStream[bitnum++]=0;


@@ -305,16 +302,30 @@ int manrawdecode(uint8_t * BitStream, size_t *size)
 }
 
 //by marshmellow
 }
 
 //by marshmellow

-//take 01 or 10 = 0 and 11 or 00 = 1
+//take 01 or 10 = 1 and 11 or 00 = 0
+//check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
+//decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding

 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert)
 {
        uint16_t bitnum=0;
        uint32_t errCnt =0;
 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert)
 {
        uint16_t bitnum=0;
        uint32_t errCnt =0;

-       uint32_t i;
-       uint16_t MaxBits=500;
-       i=offset;
-       if (size == 0) return -1;
-       for (;i<*size-2; i+=2){
+       size_t i=offset;
+       uint16_t MaxBits=512;
+       //if not enough samples - error
+       if (*size < 51) return -1;
+       //check for phase change faults - skip one sample if faulty
+       uint8_t offsetA = 1, offsetB = 1;
+       for (; i<48; i+=2){
+               if (BitStream[i+1]==BitStream[i+2]) offsetA=0; 
+               if (BitStream[i+2]==BitStream[i+3]) offsetB=0;                                  
+       }
+       if (!offsetA && offsetB) offset++;
+       for (i=offset; i<*size-3; i+=2){
+               //check for phase error
+               if (BitStream[i+1]==BitStream[i+2]) {
+                       BitStream[bitnum++]=77;
+                       errCnt++;
+               }

                if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
                        BitStream[bitnum++]=1^invert;
                } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
                if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
                        BitStream[bitnum++]=1^invert;
                } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){


@@ -351,6 +362,56 @@ void askAmp(uint8_t *BitStream, size_t size)
        return;
 }
 
        return;
 }
 

+int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low)
+{
+       size_t bitCnt=0, smplCnt=0, errCnt=0;
+       uint8_t waveHigh = 0;
+       //PrintAndLog("clk: %d", clk);
+       for (size_t i=0; i < *size; i++){
+               if (BinStream[i] >= high && waveHigh){
+                       smplCnt++;
+               } else if (BinStream[i] <= low && !waveHigh){
+                       smplCnt++;
+               } 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
+                               }
+                       } else { //haven't hit new high or new low yet
+                               smplCnt++;
+                       }
+               }
+       }
+       *size = bitCnt;
+       return errCnt;
+}
+

 //by marshmellow
 //takes 3 arguments - clock, invert and maxErr as integers
 //attempts to demodulate ask only
 //by marshmellow
 //takes 3 arguments - clock, invert and maxErr as integers
 //attempts to demodulate ask only


@@ -362,15 +423,22 @@ int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max
        if (*clk==0) return -1;
        if (start<0) return -1;
        if (*invert != 0 && *invert != 1) *invert =0;
        if (*clk==0) return -1;
        if (start<0) return -1;
        if (*invert != 0 && *invert != 1) *invert =0;

+       if (amp==1) askAmp(BinStream, *size);
+

        uint32_t initLoopMax = 200;
        if (initLoopMax > *size) initLoopMax=*size;
        // Detect high and lows
        uint32_t initLoopMax = 200;
        if (initLoopMax > *size) initLoopMax=*size;
        // Detect high and lows

-       //25% fuzz in case highs and lows aren't clipped [marshmellow]
+       //25% clip in case highs and lows aren't clipped [marshmellow]
+       uint8_t clip = 75;

        int high, low, ans;
        int high, low, ans;

-       if (amp==1) askAmp(BinStream, *size);
-       ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75);
+       ans = getHiLo(BinStream, initLoopMax, &high, &low, clip, clip);

        if (ans<1) return -1; //just noise
 
        if (ans<1) return -1; //just noise
 

+       if (DetectCleanAskWave(BinStream, *size, high, low)) {
+               //PrintAndLog("Clean");
+               return cleanAskRawDemod(BinStream, size, *clk, *invert, high, low);
+       }
+

        //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
        int lastBit = 0;  //set first clock check
        uint32_t bitnum = 0;     //output counter
        //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
        int lastBit = 0;  //set first clock check
        uint32_t bitnum = 0;     //output counter


@@ -381,11 +449,14 @@ int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max
        uint32_t iii = 0;
        uint32_t gLen = *size;
        if (gLen > 500) gLen=500;
        uint32_t iii = 0;
        uint32_t gLen = *size;
        if (gLen > 500) gLen=500;

+       //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance
+       if (!maxErr) gLen = *clk * 2; 

        uint8_t errCnt =0;
        uint32_t bestStart = *size;
        uint32_t bestErrCnt = maxErr; //(*size/1000);
        uint8_t midBit=0;
        uint16_t MaxBits=1000;
        uint8_t errCnt =0;
        uint32_t bestStart = *size;
        uint32_t bestErrCnt = maxErr; //(*size/1000);
        uint8_t midBit=0;
        uint16_t MaxBits=1000;

+

        //PrintAndLog("DEBUG - lastbit - %d",lastBit);
        //loop to find first wave that works
        for (iii=start; iii < gLen; ++iii){
        //PrintAndLog("DEBUG - lastbit - %d",lastBit);
        //loop to find first wave that works
        for (iii=start; iii < gLen; ++iii){


@@ -556,7 +627,9 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow
                                //do nothing with extra garbage
                        } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves
                                dest[numBits]=1;
                                //do nothing with extra garbage
                        } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves
                                dest[numBits]=1;

-                       } else {                                                        //9+ = 10 waves
+                       } else if ((idx-last_transition) > (fchigh+1) && !numBits) { //12 + and first bit = garbage 
+                               //do nothing with beginning garbage
+                       } else {                                         //9+ = 10 waves

                                dest[numBits]=0;
                        }
                        last_transition = idx;
                                dest[numBits]=0;
                        }
                        last_transition = idx;


@@ -580,18 +653,31 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxCons
        uint32_t idx=0;
        size_t numBits=0;
        uint32_t n=1;
        uint32_t idx=0;
        size_t numBits=0;
        uint32_t n=1;

-
+       float lowWaves = (((float)(rfLen))/((float)fclow));
+       float highWaves = (((float)(rfLen))/((float)fchigh));

        for( idx=1; idx < size; idx++) {
 
                if (dest[idx]==lastval) {
                        n++;
                        continue;
                }
        for( idx=1; idx < size; idx++) {
 
                if (dest[idx]==lastval) {
                        n++;
                        continue;
                }

+               n++;

                //if lastval was 1, we have a 1->0 crossing
                //if lastval was 1, we have a 1->0 crossing

-               if ( dest[idx-1]==1 ) {
-                       n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow));
-               } else {// 0->1 crossing
-                       n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh));  //-1 for fudge factor
+               if (dest[idx-1]==1) {
+                       if (!numBits && n < (uint8_t)lowWaves) {
+                               n=0;
+                               lastval = dest[idx];
+                               continue;
+                       }
+                       n=myround2(((float)n)/lowWaves);
+               } else {// 0->1 crossing 
+                       //test first bitsample too small
+                       if (!numBits && n < (uint8_t)highWaves) {
+                               n=0;
+                               lastval = dest[idx];
+                               continue;
+                       }
+                       n = myround2(((float)n)/highWaves);  //-1 for fudge factor

                }
                if (n == 0) n = 1;
 
                }
                if (n == 0) n = 1;
 


@@ -607,6 +693,17 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxCons
                n=0;
                lastval=dest[idx];
        }//end for
                n=0;
                lastval=dest[idx];
        }//end for

+
+       // if valid extra bits at the end were all the same frequency - add them in
+       if (n > lowWaves && n > highWaves) {
+               if (dest[idx-2]==1) {
+                       n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow));
+               } else {
+                       n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh));  //-1 for fudge factor                       
+               }
+               memset(dest, dest[idx-1]^invert , n);
+               numBits += n;
+       }

        return numBits;
 }
 //by marshmellow  (from holiman's base)
        return numBits;
 }
 //by marshmellow  (from holiman's base)


@@ -793,20 +890,70 @@ int PyramiddemodFSK(uint8_t *dest, size_t *size)
 
 uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, int high, int low)
 {
 
 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;
        uint16_t cntPeaks=0;

-       for (size_t i=20; 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 (dest[i]>low && dest[i]<high) 
                        allPeaks=0;
                else
                        cntPeaks++;
        }

-       if (allPeaks==0){
-               if (cntPeaks>190) return 1;
+       if (allPeaks == 0){
+               if (cntPeaks > 300) return 1;

        }
        return allPeaks;
 }
 
        }
        return allPeaks;
 }
 

+int DetectStrongAskClock(uint8_t dest[], size_t size)
+{
+       int clk[]={0,8,16,32,40,50,64,100,128,256};
+  size_t idx = 40;
+       uint8_t high=0;
+       size_t cnt = 0;
+       size_t highCnt = 0;
+       size_t highCnt2 = 0;
+       for (;idx < size; idx++){
+               if (dest[idx]>128) {
+                       if (!high){
+                               high=1;
+                               if (cnt > highCnt){
+                                       if (highCnt != 0) highCnt2 = highCnt;
+                                       highCnt = cnt;
+                               } else if (cnt > highCnt2) {
+                                       highCnt2 = cnt;
+                               }
+                               cnt=1;
+                       } else {
+                               cnt++;
+                       }
+               } else if (dest[idx] <= 128){
+                       if (high) {
+                               high=0;
+                               if (cnt > highCnt) {
+                                       if (highCnt != 0) highCnt2 = highCnt;
+                                       highCnt = cnt;
+                               } else if (cnt > highCnt2) {
+                                       highCnt2 = cnt;
+                               }
+                               cnt=1;
+                       } else {
+                               cnt++;
+                       }
+               }
+       }
+       uint8_t tol;
+       for (idx=8; idx>0; idx--){
+               tol = clk[idx]/8;
+               if (clk[idx] >= highCnt - tol && clk[idx] <= highCnt + tol)
+                       return clk[idx];
+               if (clk[idx] >= highCnt2 - tol && clk[idx] <= highCnt2 + tol)
+                       return clk[idx];
+       }
+       return -1;
+}
+

 // by marshmellow
 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
 // maybe somehow adjust peak trimming value based on samples to fix?
 // by marshmellow
 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
 // maybe somehow adjust peak trimming value based on samples to fix?


@@ -829,24 +976,14 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr)
   
   //test for large clean peaks
   if (DetectCleanAskWave(dest, size, peak, low)==1){
   
   //test for large clean peaks
   if (DetectCleanAskWave(dest, size, peak, low)==1){

-         uint16_t fcTest=0;
-         uint8_t mostFC=0;
-         fcTest=countFC(dest, size, &mostFC);
-         uint8_t fc1 = fcTest >> 8;
-         uint8_t fc2 = fcTest & 0xFF;
-
-         for (i=0; i<8; i++){
-               if (clk[i] == fc1) {
-                       *clock=fc1;
-                       return 0;
-               }
-               if (clk[i] == fc2) {
-                       *clock=fc2;
+       int ans = DetectStrongAskClock(dest, size);
+         for (i=7; i>0; i--){
+               if (clk[i] == ans) {
+                       *clock=ans;

                        return 0;
                }
          }
   }
                        return 0;
                }
          }
   }

-  

   int ii;
   int clkCnt;
   int tol = 0;
   int ii;
   int clkCnt;
   int tol = 0;


@@ -860,6 +997,7 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr)
     }else{
       tol=0;
     }
     }else{
       tol=0;
     }

+       if (!maxErr) loopCnt=clk[clkCnt]*2;

     bestErr[clkCnt]=1000;
     //try lining up the peaks by moving starting point (try first 256)
     for (ii=0; ii < loopCnt; ii++){
     bestErr[clkCnt]=1000;
     //try lining up the peaks by moving starting point (try first 256)
     for (ii=0; ii < loopCnt; ii++){


@@ -1079,7 +1217,9 @@ void psk1TOpsk2(uint8_t *BitStream, size_t size)
        size_t i=1;
        uint8_t lastBit=BitStream[0];
        for (; i<size; i++){
        size_t i=1;
        uint8_t lastBit=BitStream[0];
        for (; i<size; i++){

-               if (lastBit!=BitStream[i]){
+               if (BitStream[i]==77){
+                       //ignore errors
+               } else if (lastBit!=BitStream[i]){

                        lastBit=BitStream[i];
                        BitStream[i]=1;
                } else {
                        lastBit=BitStream[i];
                        BitStream[i]=1;
                } else {


@@ -1094,11 +1234,9 @@ void psk1TOpsk2(uint8_t *BitStream, size_t size)
 // from only transition waves are 1s to phase shifts change bit
 void psk2TOpsk1(uint8_t *BitStream, size_t size)
 {
 // from only transition waves are 1s to phase shifts change bit
 void psk2TOpsk1(uint8_t *BitStream, size_t size)
 {

-       size_t i;
-       uint8_t phase=BitStream[0];
-       //uint8_t lastBit=BitStream[0];
-       for (i=1; i<size; i++){
-               if (phase!=BitStream[i]){
+       uint8_t phase=0;
+       for (size_t i=0; i<size; i++){
+               if (BitStream[i]==1){

                        phase ^=1;
                }
                BitStream[i]=phase;
                        phase ^=1;
                }
                BitStream[i]=phase;


@@ -1179,11 +1317,10 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
   *clk = DetectNRZClock(dest, *size, *clk);
   if (*clk==0) return -2;
   uint32_t i;
   *clk = DetectNRZClock(dest, *size, *clk);
   if (*clk==0) return -2;
   uint32_t i;

-  int high, low, ans;
-  ans = getHiLo(dest, 1260, &high, &low, 75, 75); //25% fuzz on high 25% fuzz on low
-  if (ans<1) return -2; //just noise
-  uint32_t gLen = 256;
+  uint32_t gLen = 4096;

   if (gLen>*size) gLen = *size;
   if (gLen>*size) gLen = *size;

+  int high, low;
+  if (getHiLo(dest, gLen, &high, &low, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low

   int lastBit = 0;  //set first clock check
   uint32_t bitnum = 0;     //output counter
   uint8_t tol = 1;  //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
   int lastBit = 0;  //set first clock check
   uint32_t bitnum = 0;     //output counter
   uint8_t tol = 1;  //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave


@@ -1193,6 +1330,8 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
   uint32_t bestErrCnt = maxErr+1;
   uint32_t bestPeakCnt = 0;
   uint32_t bestPeakStart=0;
   uint32_t bestErrCnt = maxErr+1;
   uint32_t bestPeakCnt = 0;
   uint32_t bestPeakStart=0;

+  uint8_t bestFirstPeakHigh=0;
+  uint8_t firstPeakHigh=0;

   uint8_t curBit=0;
   uint8_t bitHigh=0;
   uint8_t errBitHigh=0;
   uint8_t curBit=0;
   uint8_t bitHigh=0;
   uint8_t errBitHigh=0;


@@ -1202,6 +1341,8 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
   //loop to find first wave that works - align to clock
   for (iii=0; iii < gLen; ++iii){
     if ((dest[iii]>=high) || (dest[iii]<=low)){
   //loop to find first wave that works - align to clock
   for (iii=0; iii < gLen; ++iii){
     if ((dest[iii]>=high) || (dest[iii]<=low)){

+      if (dest[iii]>=high) firstPeakHigh=1;
+      else firstPeakHigh=0;

       lastBit=iii-*clk;
       peakCnt=0;
       errCnt=0;
       lastBit=iii-*clk;
       peakCnt=0;
       errCnt=0;


@@ -1252,6 +1393,7 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
         //possible good read
         if (errCnt == 0){
           //bestStart = iii;
         //possible good read
         if (errCnt == 0){
           //bestStart = iii;

+          bestFirstPeakHigh=firstPeakHigh;

           bestErrCnt = errCnt;
           bestPeakCnt = peakCnt;
           bestPeakStart = iii;
           bestErrCnt = errCnt;
           bestPeakCnt = peakCnt;
           bestPeakStart = iii;


@@ -1262,6 +1404,7 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
           //bestStart = iii;
         }
         if (peakCnt > bestPeakCnt){
           //bestStart = iii;
         }
         if (peakCnt > bestPeakCnt){

+          bestFirstPeakHigh=firstPeakHigh;

           bestPeakCnt=peakCnt;
           bestPeakStart=iii;
         } 
           bestPeakCnt=peakCnt;
           bestPeakStart=iii;
         } 


@@ -1274,6 +1417,8 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
     iii=bestPeakStart;
     lastBit=bestPeakStart-*clk;
     bitnum=0;
     iii=bestPeakStart;
     lastBit=bestPeakStart-*clk;
     bitnum=0;

+    memset(dest, bestFirstPeakHigh^1, bestPeakStart / *clk);
+    bitnum += (bestPeakStart / *clk);

     for (i = iii; i < *size; ++i) {
       //if we found a high bar and we are at a clock bit
       if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
     for (i = iii; i < *size; ++i) {
       //if we found a high bar and we are at a clock bit
       if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){


@@ -1323,12 +1468,12 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
     *size=bitnum;
   } else{
     *size=bitnum;
     *size=bitnum;
   } else{
     *size=bitnum;

-    return -1;
+    return bestErrCnt;

   }
 
   if (bitnum>16){
     *size=bitnum;
   }
 
   if (bitnum>16){
     *size=bitnum;

-  } else return -1;
+  } else return -5;

   return errCnt;
 }
 
   return errCnt;
 }
 


@@ -1591,7 +1736,7 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
   if (*size<loopCnt) loopCnt = *size;
 
   uint8_t curPhase = *invert;
   if (*size<loopCnt) loopCnt = *size;
 
   uint8_t curPhase = *invert;

-  size_t i, waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
+  size_t i, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;

   uint8_t fc=0, fullWaveLen=0, tol=1;
   uint16_t errCnt=0, waveLenCnt=0;
   fc = countPSK_FC(dest, *size);
   uint8_t fc=0, fullWaveLen=0, tol=1;
   uint16_t errCnt=0, waveLenCnt=0;
   fc = countPSK_FC(dest, *size);


@@ -1600,38 +1745,35 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
   *clock = DetectPSKClock(dest, *size, *clock);
   if (*clock==0) return -1;
   int avgWaveVal=0, lastAvgWaveVal=0;
   *clock = DetectPSKClock(dest, *size, *clock);
   if (*clock==0) return -1;
   int avgWaveVal=0, lastAvgWaveVal=0;

-  //find first full wave
+  //find first phase shift

   for (i=0; i<loopCnt; i++){
     if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
   for (i=0; i<loopCnt; i++){
     if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){

-      if (waveStart == 0) {
-        waveStart = i+1;
-        avgWaveVal=dest[i+1];
-        //PrintAndLog("DEBUG: waveStart: %d",waveStart);
-      } else {
-        waveEnd = i+1;
-        //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
-        waveLenCnt = waveEnd-waveStart;
-        lastAvgWaveVal = avgWaveVal/waveLenCnt;
-        if (waveLenCnt > fc){
-          firstFullWave = waveStart;
-          fullWaveLen=waveLenCnt;
-          //if average wave value is > graph 0 then it is an up wave or a 1
-          if (lastAvgWaveVal > 128) curPhase^=1;
-          break;
-        } 
-        waveStart=0;
-        avgWaveVal=0;
-      }
+      waveEnd = i+1;
+      //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
+      waveLenCnt = waveEnd-waveStart;
+      if (waveLenCnt > fc && waveStart > fc){ //not first peak and is a large wave 
+        lastAvgWaveVal = avgWaveVal/(waveLenCnt);
+        firstFullWave = waveStart;
+        fullWaveLen=waveLenCnt;
+        //if average wave value is > graph 0 then it is an up wave or a 1
+        if (lastAvgWaveVal > 123) curPhase^=1;  //fudge graph 0 a little 123 vs 128
+        break;
+      } 
+      waveStart = i+1;
+      avgWaveVal = 0;

     }
     }

-    avgWaveVal+=dest[i+1];
+    avgWaveVal+=dest[i+2];

   }
   //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);  
   lastClkBit = firstFullWave; //set start of wave as clock align
   }
   //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);  
   lastClkBit = firstFullWave; //set start of wave as clock align

+  //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);

   waveStart = 0;
   errCnt=0;
   size_t numBits=0;
   waveStart = 0;
   errCnt=0;
   size_t numBits=0;

-  //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
-
+  //set skipped bits
+  memset(dest,curPhase^1,firstFullWave / *clock);
+  numBits += (firstFullWave / *clock);
+  dest[numBits++] = curPhase; //set first read bit

   for (i = firstFullWave+fullWaveLen-1; i < *size-3; i++){
     //top edge of wave = start of new wave 
     if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
   for (i = firstFullWave+fullWaveLen-1; i < *size-3; i++){
     //top edge of wave = start of new wave 
     if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){


@@ -1643,26 +1785,23 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
         waveEnd = i+1;
         waveLenCnt = waveEnd-waveStart;
         lastAvgWaveVal = avgWaveVal/waveLenCnt;
         waveEnd = i+1;
         waveLenCnt = waveEnd-waveStart;
         lastAvgWaveVal = avgWaveVal/waveLenCnt;

-        if (waveLenCnt > fc){ 
+        if (waveLenCnt > fc){  

           //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
           //if this wave is a phase shift
           //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
           if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
             curPhase^=1;
           //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
           //if this wave is a phase shift
           //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
           if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
             curPhase^=1;

-            dest[numBits] = curPhase;
-            numBits++;
+            dest[numBits++] = curPhase;

             lastClkBit += *clock;
             lastClkBit += *clock;

-          } else if (i<lastClkBit+10){
+          } else if (i<lastClkBit+10+fc){

             //noise after a phase shift - ignore
           } else { //phase shift before supposed to based on clock
             errCnt++;
             //noise after a phase shift - ignore
           } else { //phase shift before supposed to based on clock
             errCnt++;

-            dest[numBits] = 77;
-            numBits++;
+            dest[numBits++] = 77;

           }
         } else if (i+1 > lastClkBit + *clock + tol + fc){
           lastClkBit += *clock; //no phase shift but clock bit
           }
         } else if (i+1 > lastClkBit + *clock + tol + fc){
           lastClkBit += *clock; //no phase shift but clock bit

-          dest[numBits] = curPhase;
-          numBits++;
+          dest[numBits++] = curPhase;

         }
         avgWaveVal=0;
         waveStart=i+1;
         }
         avgWaveVal=0;
         waveStart=i+1;