split hid demod/awid demod to respective files

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index fdcb475b7882e2dbfae1e6d2addfc9514f9bd740..8307a89088f57c5e468665c602dea7effcc0e984 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -39,6 +39,8 @@
 //**********************************************************************************************
 //---------------------------------Utilities Section--------------------------------------------
 //**********************************************************************************************
 //**********************************************************************************************
 //---------------------------------Utilities Section--------------------------------------------
 //**********************************************************************************************

+#define LOWEST_DEFAULT_CLOCK 32
+#define FSK_PSK_THRESHOLD   123

 
 //to allow debug print calls when used not on device
 void dummy(char *fmt, ...){}
 
 //to allow debug print calls when used not on device
 void dummy(char *fmt, ...){}


@@ -53,11 +55,10 @@ void dummy(char *fmt, ...){}
 #endif
 
 uint8_t justNoise(uint8_t *BitStream, size_t size) {
 #endif
 
 uint8_t justNoise(uint8_t *BitStream, size_t size) {

-       static const uint8_t THRESHOLD = 123;

        //test samples are not just noise
        uint8_t justNoise1 = 1;
        for(size_t idx=0; idx < size && justNoise1 ;idx++){
        //test samples are not just noise
        uint8_t justNoise1 = 1;
        for(size_t idx=0; idx < size && justNoise1 ;idx++){

-               justNoise1 = BitStream[idx] < THRESHOLD;
+               justNoise1 = BitStream[idx] < FSK_PSK_THRESHOLD;

        }
        return justNoise1;
 }
        }
        return justNoise1;
 }


@@ -72,7 +73,7 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi
                if (BitStream[i] > *high) *high = BitStream[i];
                if (BitStream[i] < *low) *low = BitStream[i];
        }
                if (BitStream[i] > *high) *high = BitStream[i];
                if (BitStream[i] < *low) *low = BitStream[i];
        }

-       if (*high < 123) return -1; // just noise
+       if (*high < FSK_PSK_THRESHOLD) return -1; // just noise

        *high = ((*high-128)*fuzzHi + 12800)/100;
        *low = ((*low-128)*fuzzLo + 12800)/100;
        return 1;
        *high = ((*high-128)*fuzzHi + 12800)/100;
        *low = ((*low-128)*fuzzLo + 12800)/100;
        return 1;


@@ -197,18 +198,18 @@ uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_
 }
 
 // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.
 }
 
 // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.

-size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_t expWaveSize) {
+size_t findModStart(uint8_t dest[], size_t size, uint8_t expWaveSize) {

        size_t i = 0;
        size_t waveSizeCnt = 0;
        uint8_t thresholdCnt = 0;
        size_t i = 0;
        size_t waveSizeCnt = 0;
        uint8_t thresholdCnt = 0;

-       bool isAboveThreshold = dest[i++] >= threshold_value;
+       bool isAboveThreshold = dest[i++] >= FSK_PSK_THRESHOLD;

        for (; i < size-20; i++ ) {
        for (; i < size-20; i++ ) {

-               if(dest[i] < threshold_value && isAboveThreshold) {
+               if(dest[i] < FSK_PSK_THRESHOLD && isAboveThreshold) {

                        thresholdCnt++;
                        if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;                     
                        isAboveThreshold = false;
                        waveSizeCnt = 0;
                        thresholdCnt++;
                        if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;                     
                        isAboveThreshold = false;
                        waveSizeCnt = 0;

-               } else if (dest[i] >= threshold_value && !isAboveThreshold) {
+               } else if (dest[i] >= FSK_PSK_THRESHOLD && !isAboveThreshold) {

                        thresholdCnt++;
                        if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;                     
                        isAboveThreshold = true;
                        thresholdCnt++;
                        if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;                     
                        isAboveThreshold = true;


@@ -222,6 +223,64 @@ size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_
        return i;
 }
 
        return i;
 }
 

+int getClosestClock(int testclk) {
+       uint8_t fndClk[] = {8,16,32,40,50,64,128};
+
+       for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++)
+               if (testclk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && testclk <= fndClk[clkCnt]+1)
+                       return fndClk[clkCnt];
+
+       return 0;
+}
+
+void getNextLow(uint8_t samples[], size_t size, int low, size_t *i) {
+       while ((samples[*i] > low) && (*i < size))
+               *i+=1;
+}
+
+void getNextHigh(uint8_t samples[], size_t size, int high, size_t *i) {
+       while ((samples[*i] < high) && (*i < size))
+               *i+=1;
+}
+
+// load wave counters
+bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) {
+       size_t i=0, firstLow, firstHigh;
+       size_t testsize = (size < 512) ? size : 512;
+
+       if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) {
+               if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
+               return false; //just noise
+       }
+
+       // get to first full low to prime loop and skip incomplete first pulse
+       getNextHigh(samples, size, *high, &i);
+       getNextLow(samples, size, *low, &i);
+       *skip = i;
+
+       // populate tmpbuff buffer with pulse lengths
+       while (i < size) {
+               // measure from low to low
+               firstLow = i;
+               //find first high point for this wave
+               getNextHigh(samples, size, *high, &i);
+               firstHigh = i;
+
+               getNextLow(samples, size, *low, &i);
+
+               if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK))
+                       break;
+
+               highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low
+               lowToLowWaveLen[*waveCnt] = i - firstLow;
+               *waveCnt += 1;
+               if (i-firstLow < *minClk && i < size) {
+                       *minClk = i - firstLow;
+               }
+       }
+       return true;
+}
+

 //by marshmellow
 //amplify based on ask edge detection  -  not accurate enough to use all the time
 void askAmp(uint8_t *BitStream, size_t size) {
 //by marshmellow
 //amplify based on ask edge detection  -  not accurate enough to use all the time
 void askAmp(uint8_t *BitStream, size_t size) {


@@ -249,15 +308,17 @@ uint32_t manchesterEncode2Bytes(uint16_t datain) {
 
 //by marshmellow
 //encode binary data into binary manchester 
 
 //by marshmellow
 //encode binary data into binary manchester 

-//NOTE: BitStream must have double the size available in memory to do the swap
+//NOTE: BitStream must have triple the size of "size" available in memory to do the swap

 int ManchesterEncode(uint8_t *BitStream, size_t size) {
 int ManchesterEncode(uint8_t *BitStream, size_t size) {

-       size_t modIdx=size, i=0;
-       if (size>modIdx) return -1;
+       //allow up to 4K out (means BitStream must be at least 2048+4096 to handle the swap)
+       size = (size>2048) ? 2048 : size;
+       size_t modIdx = size;
+       size_t i;

        for (size_t idx=0; idx < size; idx++){
                BitStream[idx+modIdx++] = BitStream[idx];
                BitStream[idx+modIdx++] = BitStream[idx]^1;
        }
        for (size_t idx=0; idx < size; idx++){
                BitStream[idx+modIdx++] = BitStream[idx];
                BitStream[idx+modIdx++] = BitStream[idx]^1;
        }

-       for (; i<(size*2); i++){
+       for (i=0; i<(size*2); i++){

                BitStream[i] = BitStream[i+size];
        }
        return i;
                BitStream[i] = BitStream[i+size];
        }
        return i;


@@ -289,28 +350,22 @@ uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t lo
 // by marshmellow
 // to help detect clocks on heavily clipped samples
 // based on count of low to low
 // by marshmellow
 // to help detect clocks on heavily clipped samples
 // based on count of low to low

-int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low, int *clock) {
-       uint8_t fndClk[] = {8,16,32,40,50,64,128};
+int DetectStrongAskClock(uint8_t dest[], size_t size, int high, int low, int *clock) {

        size_t startwave;
        size_t i = 100;
        size_t minClk = 255;
        int shortestWaveIdx = 0;
                // get to first full low to prime loop and skip incomplete first pulse
        size_t startwave;
        size_t i = 100;
        size_t minClk = 255;
        int shortestWaveIdx = 0;
                // get to first full low to prime loop and skip incomplete first pulse

-       while ((dest[i] < high) && (i < size))
-               ++i;
-       while ((dest[i] > low) && (i < size))
-               ++i;
+       getNextHigh(dest, size, high, &i);
+       getNextLow(dest, size, low, &i);

 
        // loop through all samples
        while (i < size) {
                // measure from low to low
 
        // loop through all samples
        while (i < size) {
                // measure from low to low

-               while ((dest[i] > low) && (i < size))
-                       ++i;

                startwave = i;
                startwave = i;

-               while ((dest[i] < high) && (i < size))
-                       ++i;
-               while ((dest[i] > low) && (i < size))
-                       ++i;
+
+               getNextHigh(dest, size, high, &i);
+               getNextLow(dest, size, low, &i);

                //get minimum measured distance
                if (i-startwave < minClk && i < size) {
                        minClk = i - startwave;
                //get minimum measured distance
                if (i-startwave < minClk && i < size) {
                        minClk = i - startwave;


@@ -318,14 +373,12 @@ int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low,
                }
        }
        // set clock
                }
        }
        // set clock

-       if (g_debugMode==2) prnt("DEBUG ASK: detectstrongASKclk smallest wave: %d",minClk);
-       for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) {
-               if (minClk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && minClk <= fndClk[clkCnt]+1) {
-                       *clock = fndClk[clkCnt];
-                       return shortestWaveIdx;
-               }
-       }
-       return 0;
+       if (g_debugMode==2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d",minClk);
+       *clock = getClosestClock(minClk);
+       if (*clock == 0) 
+               return 0;
+       
+       return shortestWaveIdx;

 }
 
 // by marshmellow
 }
 
 // by marshmellow


@@ -459,7 +512,7 @@ int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low){
 
 //by marshmellow
 //detect nrz clock by reading #peaks vs no peaks(or errors)
 
 //by marshmellow
 //detect nrz clock by reading #peaks vs no peaks(or errors)

-int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) {
+int DetectNRZClock(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) {

        size_t i=0;
        uint8_t clk[]={8,16,32,40,50,64,100,128,255};
        size_t loopCnt = 4096;  //don't need to loop through entire array...
        size_t i=0;
        uint8_t clk[]={8,16,32,40,50,64,100,128,255};
        size_t loopCnt = 4096;  //don't need to loop through entire array...


@@ -575,11 +628,6 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar
        return clk[best];
 }
 
        return clk[best];
 }
 

-int DetectNRZClock(uint8_t dest[], size_t size, int clock) {
-       size_t bestStart=0;
-       return DetectNRZClock_ext(dest, size, clock, &bestStart);
-}
-

 //by marshmellow
 //countFC is to detect the field clock lengths.
 //counts and returns the 2 most common wave lengths
 //by marshmellow
 //countFC is to detect the field clock lengths.
 //counts and returns the 2 most common wave lengths


@@ -888,14 +936,14 @@ uint8_t   detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc
 //**********************************************************************************************
 
 // look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index...
 //**********************************************************************************************
 
 // look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index...

-bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, int i) {
-       for (; i < buffSize - 4; ++i) {
-               *stStartIdx += lowToLowWaveLen[i]; //caution part of this wave may be data and part may be ST....  to be accounted for in main function for now...
-               if (lowToLowWaveLen[i] >= clk*1-tol && lowToLowWaveLen[i] <= (clk*2)+tol && highToLowWaveLen[i] < clk+tol) {           //1 to 2 clocks depending on 2 bits prior
-                       if (lowToLowWaveLen[i+1] >= clk*2-tol && lowToLowWaveLen[i+1] <= clk*2+tol && highToLowWaveLen[i+1] > clk*3/2-tol) {       //2 clocks and wave size is 1 1/2
-                               if (lowToLowWaveLen[i+2] >= (clk*3)/2-tol && lowToLowWaveLen[i+2] <= clk*2+tol && highToLowWaveLen[i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave
-                                       if (lowToLowWaveLen[i+3] >= clk*1-tol && lowToLowWaveLen[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
-                                               *stStopLoc = i + 3;
+bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) {
+       for (; *i < buffSize - 4; *i+=1) {
+               *stStartIdx += lowToLowWaveLen[*i]; //caution part of this wave may be data and part may be ST....  to be accounted for in main function for now...
+               if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) {           //1 to 2 clocks depending on 2 bits prior
+                       if (lowToLowWaveLen[*i+1] >= clk*2-tol && lowToLowWaveLen[*i+1] <= clk*2+tol && highToLowWaveLen[*i+1] > clk*3/2-tol) {       //2 clocks and wave size is 1 1/2
+                               if (lowToLowWaveLen[*i+2] >= (clk*3)/2-tol && lowToLowWaveLen[*i+2] <= clk*2+tol && highToLowWaveLen[*i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave
+                                       if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
+                                               *stStopLoc = *i + 3;

                                                return true;
                                        }
                                }
                                                return true;
                                        }
                                }


@@ -909,77 +957,36 @@ bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLo
 bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststart, size_t *stend) {
        size_t bufsize = *size;
        //need to loop through all samples and identify our clock, look for the ST pattern
 bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststart, size_t *stend) {
        size_t bufsize = *size;
        //need to loop through all samples and identify our clock, look for the ST pattern

-       uint8_t fndClk[] = {8,16,32,40,50,64,128};

        int clk = 0; 
        int tol = 0;
        int clk = 0; 
        int tol = 0;

-       int i, j, skip, start, end, low, high, minClk, waveStart;
+       int j, high, low, skip, start, end, minClk=255;
+       size_t i = 0;

        //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
        //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]

-       int tmpbuff[bufsize / 32]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured
-       int waveLen[bufsize / 32]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed...
-       size_t testsize = (bufsize < 512) ? bufsize : 512;
+       int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured
+       int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed...
+       //size_t testsize = (bufsize < 512) ? bufsize : 512;

        int phaseoff = 0;
        high = low = 128;
        memset(tmpbuff, 0, sizeof(tmpbuff));
        int phaseoff = 0;
        high = low = 128;
        memset(tmpbuff, 0, sizeof(tmpbuff));

+       memset(waveLen, 0, sizeof(waveLen));

 
 

-       if ( getHiLo(buffer, testsize, &high, &low, 80, 80) == -1 ) {
-               if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
-               return false; //just noise
-       }
-       i = 0;
-       j = 0;
-       minClk = 255;
-       // get to first full low to prime loop and skip incomplete first pulse
-       while ((buffer[i] < high) && (i < bufsize))
-               ++i;
-       while ((buffer[i] > low) && (i < bufsize))
-               ++i;
-       skip = i;
-
-       // populate tmpbuff buffer with pulse lengths
-       while (i < bufsize) {
-               // measure from low to low
-               while ((buffer[i] > low) && (i < bufsize))
-                       ++i;
-               start= i;
-               while ((buffer[i] < high) && (i < bufsize))
-                       ++i;
-               //first high point for this wave
-               waveStart = i;
-               while ((buffer[i] > low) && (i < bufsize))
-                       ++i;
-               if (j >= (bufsize/32)) {
-                       break;
-               }
-               waveLen[j] = i - waveStart; //first high to first low
-               tmpbuff[j++] = i - start;
-               if (i-start < minClk && i < bufsize) {
-                       minClk = i - start;
-               }
-       }
+       if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false;

        // set clock  - might be able to get this externally and remove this work...
        // set clock  - might be able to get this externally and remove this work...

-       if (!clk) {
-               for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) {
-                       tol = fndClk[clkCnt]/8;
-                       if (minClk >= fndClk[clkCnt]-tol && minClk <= fndClk[clkCnt]+1) { 
-                               clk=fndClk[clkCnt];
-                               break;
-                       }
-               }
-               // clock not found - ERROR
-               if (!clk) {
-                       if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
-                       return false;
-               }
-       } else tol = clk/8;
-
+       clk = getClosestClock(minClk);
+       // clock not found - ERROR
+       if (clk == 0) {
+               if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
+               return false;
+       }

        *foundclock = clk;
        *foundclock = clk;

-       i=0;
-       if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, i)) {
+
+       tol = clk/8;
+       if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) {

                // first ST not found - ERROR
                if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
                return false;
        } else {
                // first ST not found - ERROR
                if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
                return false;
        } else {

-               if (g_debugMode==2) prnt("DEBUG STT: first STT found at: %d, j=%d",start, j);
+               if (g_debugMode==2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j);

        }
        if (waveLen[i+2] > clk*1+tol)
                phaseoff = 0;
        }
        if (waveLen[i+2] > clk*1+tol)
                phaseoff = 0;


@@ -992,7 +999,8 @@ bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststa
        // now do it again to find the end
        int dummy1 = 0;
        end = skip;
        // now do it again to find the end
        int dummy1 = 0;
        end = skip;

-       if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, i+3)) {
+       i+=3;
+       if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) {

                //didn't find second ST - ERROR
                if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting");
                return false;
                //didn't find second ST - ERROR
                if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting");
                return false;


@@ -1071,6 +1079,44 @@ bool DetectST(uint8_t    buffer[], size_t *size, int *foundclock) {
        return DetectST_ext(buffer, size, foundclock, &ststart, &stend);
 }
 
        return DetectST_ext(buffer, size, foundclock, &ststart, &stend);
 }
 

+//by marshmellow
+//take 11 10 01 11 00 and make 01100 ... miller decoding 
+//check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
+//decodes miller encoded binary
+//NOTE  askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!
+int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) {
+       if (*size < 16) return -1;
+       uint16_t MaxBits = 512, errCnt = 0;
+       size_t i, bitCnt=0;
+       uint8_t alignCnt = 0, curBit = BitStream[0], alignedIdx = 0;
+       uint8_t halfClkErr = 0;
+       //find alignment, needs 4 1s or 0s to properly align
+       for (i=1; i < *size-1; i++) {
+               alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0;
+               curBit = BitStream[i];
+               if (alignCnt == 4) break;
+       }
+       // for now error if alignment not found.  later add option to run it with multiple offsets...
+       if (alignCnt != 4) {
+               if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it");
+               return -1;
+       }
+       alignedIdx = (i-1) % 2;
+       for (i=alignedIdx; i < *size-3; i+=2) {
+               halfClkErr = (uint8_t)((halfClkErr << 1 | BitStream[i]) & 0xFF);
+               if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) {
+                       errCnt++;
+                       BitStream[bitCnt++] = 7;
+                       continue;
+               }
+               BitStream[bitCnt++] = BitStream[i] ^ BitStream[i+1] ^ invert;
+
+               if (bitCnt > MaxBits) break;
+       }
+       *size = bitCnt;
+       return errCnt;
+}
+

 //by marshmellow
 //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
 //by marshmellow
 //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


@@ -1286,9 +1332,10 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr
 
 // by marshmellow - demodulate NRZ wave - requires a read with strong signal
 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
 
 // by marshmellow - demodulate NRZ wave - requires a read with strong signal
 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak

-int nrzRawDemod_ext(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) {
+int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) {

        if (justNoise(dest, *size)) return -1;
        if (justNoise(dest, *size)) return -1;

-       *clk = DetectNRZClock(dest, *size, *clk);
+       size_t clkStartIdx = 0;
+       *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx);

        if (*clk==0) return -2;
        size_t i, gLen = 4096;
        if (gLen>*size) gLen = *size-20;
        if (*clk==0) return -2;
        size_t i, gLen = 4096;
        if (gLen>*size) gLen = *size-20;


@@ -1320,10 +1367,6 @@ int nrzRawDemod_ext(uint8_t *dest, size_t *size, int *clk, int *invert, int *sta
        *size = numBits;
        return 0;
 }
        *size = numBits;
        return 0;
 }

-int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert) {
-       int startIdx = 0;
-       return nrzRawDemod_ext(dest, size, clk, invert, &startIdx);
-}

 
 //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq])
 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) {
 
 //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq])
 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) {


@@ -1332,16 +1375,15 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow
        if (fchigh==0) fchigh=10;
        if (fclow==0) fclow=8;
        //set the threshold close to 0 (graph) or 128 std to avoid static
        if (fchigh==0) fchigh=10;
        if (fclow==0) fclow=8;
        //set the threshold close to 0 (graph) or 128 std to avoid static

-       uint8_t threshold_value = 123; 

        size_t preLastSample = 0;
        size_t LastSample = 0;
        size_t currSample = 0;
        if ( size < 1024 ) return 0; // not enough samples
 
        //find start of modulating data in trace 
        size_t preLastSample = 0;
        size_t LastSample = 0;
        size_t currSample = 0;
        if ( size < 1024 ) return 0; // not enough samples
 
        //find start of modulating data in trace 

-       idx = findModStart(dest, size, threshold_value, fchigh);
+       idx = findModStart(dest, size, fchigh);

        // Need to threshold first sample
        // Need to threshold first sample

-       if(dest[idx] < threshold_value) dest[0] = 0;
+       if(dest[idx] < FSK_PSK_THRESHOLD) dest[0] = 0;

        else dest[0] = 1;
        
        last_transition = idx;
        else dest[0] = 1;
        
        last_transition = idx;


@@ -1353,7 +1395,7 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow
        //  (could also be fc/5 && fc/7 for fsk1 = 4-9)
        for(; idx < size; idx++) {
                // threshold current value
        //  (could also be fc/5 && fc/7 for fsk1 = 4-9)
        for(; idx < size; idx++) {
                // threshold current value

-               if (dest[idx] < threshold_value) dest[idx] = 0;
+               if (dest[idx] < FSK_PSK_THRESHOLD) dest[idx] = 0;

                else dest[idx] = 1;
 
                // Check for 0->1 transition
                else dest[idx] = 1;
 
                // Check for 0->1 transition


@@ -1483,54 +1525,55 @@ void psk2TOpsk1(uint8_t *BitStream, size_t size) {
        return;
 }
 
        return;
 }
 

+size_t pskFindFirstPhaseShift(uint8_t samples[], size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) {
+       uint16_t loopCnt = (size+3 < 4096) ? size : 4096;  //don't need to loop through entire array...
+
+       uint16_t avgWaveVal=0, lastAvgWaveVal=0;
+       size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave;
+       for (; i<loopCnt; i++) {
+               // find peak 
+               if (samples[i]+fc < samples[i+1] && samples[i+1] >= samples[i+2]){
+                       waveEnd = i+1;
+                       if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart);
+                       waveLenCnt = waveEnd-waveStart;
+                       if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack
+                               lastAvgWaveVal = avgWaveVal/(waveLenCnt);
+                               firstFullWave = waveStart;
+                               *fullWaveLen = waveLenCnt;
+                               //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
+                               if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1;
+                               return firstFullWave;
+                       }
+                       waveStart = i+1;
+                       avgWaveVal = 0;
+               }
+               avgWaveVal += samples[i+2];
+       }
+       return 0;
+}
+

 //by marshmellow - demodulate PSK1 wave 
 //uses wave lengths (# Samples) 
 int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *startIdx) {
 //by marshmellow - demodulate PSK1 wave 
 //uses wave lengths (# Samples) 
 int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *startIdx) {

-       if (size == 0) return -1;
-       uint16_t loopCnt = 4096;  //don't need to loop through entire array...
-       if (*size<loopCnt) loopCnt = *size;
+       if (*size < 170) return -1;

 
 

-       size_t numBits=0;

        uint8_t curPhase = *invert;
        uint8_t curPhase = *invert;

-       size_t i=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
-       uint16_t fc=0, fullWaveLen=0, tol=1;
-       uint16_t errCnt=0, waveLenCnt=0, errCnt2=0;
+       size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
+       uint16_t fc=0, fullWaveLen=0, waveLenCnt=0, avgWaveVal, tol=1;
+       uint16_t errCnt=0, errCnt2=0;
+       

        fc = countFC(dest, *size, 1);
        fc = countFC(dest, *size, 1);

-       uint8_t fc2 = fc >> 8;
-       if (fc2 == 10) return -1; //fsk found - quit
+       if ((fc >> 8) == 10) return -1; //fsk found - quit

        fc = fc & 0xFF;
        if (fc!=2 && fc!=4 && fc!=8) return -1;
        fc = fc & 0xFF;
        if (fc!=2 && fc!=4 && fc!=8) return -1;

-       //PrintAndLog("DEBUG: FC: %d",fc);

        *clock = DetectPSKClock(dest, *size, *clock);
        if (*clock == 0) return -1;
 
        //find start of modulating data in trace 
        *clock = DetectPSKClock(dest, *size, *clock);
        if (*clock == 0) return -1;
 
        //find start of modulating data in trace 

-       uint8_t threshold_value = 123; //-5
-       i = findModStart(dest, *size, threshold_value, fc);
+       i = findModStart(dest, *size, fc);

 
        //find first phase shift
 
        //find first phase shift

-       int avgWaveVal=0, lastAvgWaveVal=0;
-       waveStart = i;
-       for (; i<loopCnt; i++) {
-               // find peak 
-               if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
-                       waveEnd = i+1;
-                       if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u",waveEnd, waveStart);
-                       waveLenCnt = waveEnd-waveStart;
-                       if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+3)){ //not first peak and is a large wave but not out of whack
-                               lastAvgWaveVal = avgWaveVal/(waveLenCnt);
-                               firstFullWave = waveStart;
-                               fullWaveLen=waveLenCnt;
-                               //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
-                               if (lastAvgWaveVal > threshold_value) curPhase ^= 1;
-                               break;
-                       }
-
-                       waveStart = i+1;
-                       avgWaveVal = 0;
-               }
-               avgWaveVal += dest[i+2];
-       }
+       firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen);

        if (firstFullWave == 0) {
                // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
                // so skip a little to ensure we are past any Start Signal
        if (firstFullWave == 0) {
                // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
                // so skip a little to ensure we are past any Start Signal


@@ -1558,9 +1601,7 @@ int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *
                        } else { //waveEnd
                                waveEnd = i+1;
                                waveLenCnt = waveEnd-waveStart;
                        } else { //waveEnd
                                waveEnd = i+1;
                                waveLenCnt = waveEnd-waveStart;

-                               lastAvgWaveVal = avgWaveVal/waveLenCnt;

                                if (waveLenCnt > fc){
                                if (waveLenCnt > fc){

-                                       //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);

                                        //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
                                        //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


@@ -1850,7 +1891,6 @@ int VikingDemod_AM(uint8_t *dest, size_t *size) {
        return (int) startIdx;
 }
 
        return (int) startIdx;
 }
 

-

 // by iceman
 // find Visa2000 preamble in already demoded data
 int Visa2kDemod_AM(uint8_t *dest, size_t *size) {
 // by iceman
 // find Visa2000 preamble in already demoded data
 int Visa2kDemod_AM(uint8_t *dest, size_t *size) {