fix: ukbhit() for OS X

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index fb10f6864d81b813bef66cd79b61e9d5d9ac7ac2..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,7 +936,7 @@ 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) {
+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
        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


@@ -909,72 +957,30 @@ 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));
        memset(waveLen, 0, sizeof(waveLen));
 
        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;
+
+       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");
        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");


@@ -1326,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;


@@ -1360,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) {


@@ -1372,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;


@@ -1393,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


@@ -1523,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


@@ -1598,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