fix missing terminating 0 in my_executable_directory (thanks @iceman1001).

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 74e3e6b64a0b0e0e35ecf8ca1f4b06904281a746..6beef130bfd8afcda1396a018f08c0a8fd41a7e4 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -33,13 +33,17 @@
 //-----------------------------------------------------------------------------
 
 #include <string.h>  // for memset, memcmp and size_t
 //-----------------------------------------------------------------------------
 
 #include <string.h>  // for memset, memcmp and size_t

+#include "lfdemod.h"

 #include <stdint.h>  // for uint_32+
 #include <stdbool.h> // for bool
 #include <stdint.h>  // for uint_32+
 #include <stdbool.h> // for bool

+#include "parity.h"  // for parity test

 
 //**********************************************************************************************
 //---------------------------------Utilities Section--------------------------------------------
 //**********************************************************************************************
 #define LOWEST_DEFAULT_CLOCK 32
 
 //**********************************************************************************************
 //---------------------------------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, ...){}
 #ifndef ON_DEVICE
 //to allow debug print calls when used not on device
 void dummy(char *fmt, ...){}
 #ifndef ON_DEVICE


@@ -53,11 +57,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 +75,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;


@@ -81,40 +84,35 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi
 // by marshmellow
 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
 // returns 1 if passed
 // by marshmellow
 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
 // returns 1 if passed

-uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) {
-       uint8_t ans = 0;
-       for (uint8_t i = 0; i < bitLen; i++){
-               ans ^= ((bits >> i) & 1);
-       }
-       if (g_debugMode) prnt("DEBUG: ans: %d, ptype: %d, bits: %08X",ans,pType,bits);
-       return (ans == pType);
+bool parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) {
+       return oddparity32(bits) ^ pType;

 }
 
 // by marshmellow
 }
 
 // by marshmellow

-// takes a array of binary values, start position, length of bits per parity (includes parity bit),
-//   Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run) 
+// takes a array of binary values, start position, length of bits per parity (includes parity bit - MAX 32),
+//   Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run)

 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) {
        uint32_t parityWd = 0;
 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) {
        uint32_t parityWd = 0;

-       size_t j = 0, bitCnt = 0;
+       size_t bitCnt = 0;

        for (int word = 0; word < (bLen); word+=pLen) {
                for (int bit=0; bit < pLen; bit++) {
        for (int word = 0; word < (bLen); word+=pLen) {
                for (int bit=0; bit < pLen; bit++) {

+                       if (word+bit >= bLen) break;

                        parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
                        parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];

-                       BitStream[j++] = (BitStream[startIdx+word+bit]);
+                       BitStream[bitCnt++] = (BitStream[startIdx+word+bit]);

                }
                if (word+pLen > bLen) break;
 
                }
                if (word+pLen > bLen) break;
 

-               j--; // overwrite parity with next data
+               bitCnt--; // overwrite parity with next data

                // if parity fails then return 0
                switch (pType) {
                // if parity fails then return 0
                switch (pType) {

-                       case 3: if (BitStream[j]==1) {return 0;} break; //should be 0 spacer bit
-                       case 2: if (BitStream[j]==0) {return 0;} break; //should be 1 spacer bit
+                       case 3: if (BitStream[bitCnt]==1) {return 0;} break; //should be 0 spacer bit
+                       case 2: if (BitStream[bitCnt]==0) {return 0;} break; //should be 1 spacer bit

                        default: if (parityTest(parityWd, pLen, pType) == 0) {return 0;} break; //test parity
                }
                        default: if (parityTest(parityWd, pLen, pType) == 0) {return 0;} break; //test parity
                }

-               bitCnt+=(pLen-1);

                parityWd = 0;
        }
        // if we got here then all the parities passed
                parityWd = 0;
        }
        // if we got here then all the parities passed

-       //return ID start index and size
+       //return size

        return bitCnt;
 }
 
        return bitCnt;
 }
 


@@ -197,18 +195,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;


@@ -280,6 +278,33 @@ bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int
        return true;
 }
 
        return true;
 }
 

+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 // was "samples[i] + fc" but why?  must have been used to weed out some wave error... removed..
+               if (samples[i] < 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
 //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) {


@@ -307,15 +332,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;


@@ -509,7 +536,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...


@@ -521,7 +548,7 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar
 
        //get high and low peak
        int peak, low;
 
        //get high and low peak
        int peak, low;

-       if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return 0;
+       if (getHiLo(dest, loopCnt, &peak, &low, 90, 90) < 1) return 0;

 
        int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low);
        size_t ii;
 
        int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low);
        size_t ii;


@@ -530,26 +557,24 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar
        uint16_t smplCnt = 0;
        int16_t peakcnt = 0;
        int16_t peaksdet[] = {0,0,0,0,0,0,0,0};
        uint16_t smplCnt = 0;
        int16_t peakcnt = 0;
        int16_t peaksdet[] = {0,0,0,0,0,0,0,0};

-       uint16_t maxPeak = 255;
-       bool firstpeak = false;
-       //test for large clipped waves
-       for (i=0; i<loopCnt; i++){
-               if (dest[i] >= peak || dest[i] <= low){
-                       if (!firstpeak) continue;
+       uint16_t minPeak = 255;
+       bool firstpeak = true;
+       //test for large clipped waves - ignore first peak
+       for (i=0; i<loopCnt; i++) {
+               if (dest[i] >= peak || dest[i] <= low) {
+                       if (firstpeak) continue;

                        smplCnt++;
                } else {
                        smplCnt++;
                } else {

-                       firstpeak=true;
-                       if (smplCnt > 6 ){
-                               if (maxPeak > smplCnt){
-                                       maxPeak = smplCnt;
-                                       //prnt("maxPk: %d",maxPeak);
-                               }
+                       firstpeak = false;
+                       if (smplCnt > 0) {
+                               if (minPeak > smplCnt && smplCnt > 7) minPeak = smplCnt;

                                peakcnt++;
                                peakcnt++;

-                               //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt);
-                               smplCnt=0;
+                               if (g_debugMode == 2) prnt("DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d",minPeak,smplCnt,peakcnt);
+                               smplCnt = 0;                            

                        }
                }
        }
                        }
                }
        }

+       if (minPeak < 8) return 0;

        bool errBitHigh = 0;
        bool bitHigh = 0;
        uint8_t ignoreCnt = 0;
        bool errBitHigh = 0;
        bool bitHigh = 0;
        uint8_t ignoreCnt = 0;


@@ -559,12 +584,12 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar
        size_t bestStart[]={0,0,0,0,0,0,0,0,0};
        peakcnt=0;
        //test each valid clock from smallest to greatest to see which lines up
        size_t bestStart[]={0,0,0,0,0,0,0,0,0};
        peakcnt=0;
        //test each valid clock from smallest to greatest to see which lines up

-       for(clkCnt=0; clkCnt < 8; ++clkCnt){
+       for(clkCnt=0; clkCnt < 8; ++clkCnt) {

                //ignore clocks smaller than smallest peak
                //ignore clocks smaller than smallest peak

-               if (clk[clkCnt] < maxPeak - (clk[clkCnt]/4)) continue;
+               if (clk[clkCnt] < minPeak - (clk[clkCnt]/4)) continue;

                //try lining up the peaks by moving starting point (try first 256)
                //try lining up the peaks by moving starting point (try first 256)

-               for (ii=20; ii < loopCnt; ++ii){
-                       if ((dest[ii] >= peak) || (dest[ii] <= low)){
+               for (ii=20; ii < loopCnt; ++ii) {
+                       if ((dest[ii] >= peak) || (dest[ii] <= low)) {

                                peakcnt = 0;
                                bitHigh = false;
                                ignoreCnt = 0;
                                peakcnt = 0;
                                bitHigh = false;
                                ignoreCnt = 0;


@@ -588,8 +613,8 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar
                                                        lastBit += clk[clkCnt];
                                                }
                                        //else if not a clock bit and no peaks
                                                        lastBit += clk[clkCnt];
                                                }
                                        //else if not a clock bit and no peaks

-                                       } else if (dest[i] < peak && dest[i] > low){
-                                               if (ignoreCnt==0){
+                                       } else if (dest[i] < peak && dest[i] > low) {
+                                               if (ignoreCnt==0) {

                                                        bitHigh=false;
                                                        if (errBitHigh==true) peakcnt--;
                                                        errBitHigh=false;
                                                        bitHigh=false;
                                                        if (errBitHigh==true) peakcnt--;
                                                        errBitHigh=false;


@@ -611,25 +636,20 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar
        }
        int iii=7;
        uint8_t best=0;
        }
        int iii=7;
        uint8_t best=0;

-       for (iii=7; iii > 0; iii--){
+       for (iii=7; iii > 0; iii--) {

                if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) {
                        if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) {
                                best = iii;
                        }
                if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) {
                        if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) {
                                best = iii;
                        }

-               } else if (peaksdet[iii] > peaksdet[best]){
+               } else if (peaksdet[iii] > peaksdet[best]) {

                        best = iii;
                }
                        best = iii;
                }

-               if (g_debugMode==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, maxPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],maxPeak, clk[best], lowestTransition);
+               if (g_debugMode==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],minPeak, clk[best], lowestTransition);

        }
        *clockStartIdx  = bestStart[best];
        return clk[best];
 }
 
        }
        *clockStartIdx  = bestStart[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


@@ -697,6 +717,7 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) {
                        best3=i;
                }
                if (g_debugMode==2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u",fcLens[i],fcCnts[i],fcLens[best1],fcLens[best2]);
                        best3=i;
                }
                if (g_debugMode==2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u",fcLens[i],fcCnts[i],fcLens[best1],fcLens[best2]);

+               if (fcLens[i]==0) break;

        }
        if (fcLens[best1]==0) return 0;
        uint8_t fcH=0, fcL=0;
        }
        if (fcLens[best1]==0) return 0;
        uint8_t fcH=0, fcL=0;


@@ -714,18 +735,24 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) {
        // TODO: take top 3 answers and compare to known Field clocks to get top 2
 
        uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
        // TODO: take top 3 answers and compare to known Field clocks to get top 2
 
        uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;

-       if (fskAdj) return fcs; 
-       return fcLens[best1];
+       if (fskAdj) return fcs;
+       return (uint16_t)fcLens[best2] << 8 | fcLens[best1];

 }
 
 //by marshmellow
 //detect psk clock by reading each phase shift
 // a phase shift is determined by measuring the sample length of each wave
 }
 
 //by marshmellow
 //detect psk clock by reading each phase shift
 // a phase shift is determined by measuring the sample length of each wave

-int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseShift) {
+int DetectPSKClock(uint8_t dest[], size_t size, int clock, size_t *firstPhaseShift, uint8_t *curPhase, uint8_t *fc) {

        uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
        uint16_t loopCnt = 4096;  //don't need to loop through entire array...
        if (size == 0) return 0;
        uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
        uint16_t loopCnt = 4096;  //don't need to loop through entire array...
        if (size == 0) return 0;

-       if (size<loopCnt) loopCnt = size-20;
+       if (size+3<loopCnt) loopCnt = size-20;
+
+       uint16_t fcs = countFC(dest, size, 0);
+       *fc = fcs & 0xFF;
+       if (g_debugMode==2) prnt("DEBUG PSK: FC: %d, FC2: %d",*fc, fcs>>8);
+       if ((fcs>>8) == 10 && *fc == 8) return -1;
+       if (*fc!=2 && *fc!=4 && *fc!=8) return -1;

 
        //if we already have a valid clock quit
        size_t i=1;
 
        //if we already have a valid clock quit
        size_t i=1;


@@ -733,37 +760,28 @@ int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseSh
                if (clk[i] == clock) return clock;
 
        size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
                if (clk[i] == clock) return clock;
 
        size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;

-       uint8_t clkCnt, fc=0, fullWaveLen=0, tol=1;
-       uint16_t peakcnt=0, errCnt=0, waveLenCnt=0;
+
+       uint8_t clkCnt, tol=1;
+       uint16_t peakcnt=0, errCnt=0, waveLenCnt=0, fullWaveLen=0;

        uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
        uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0};
        uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
        uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0};

-       fc = countFC(dest, size, 0);
-       if (fc!=2 && fc!=4 && fc!=8) return -1;
-       if (g_debugMode==2) prnt("DEBUG PSK: FC: %d",fc);

 
 

-       //find first full wave
-       for (i=160; i<loopCnt; i++){
-               if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
-                       if (waveStart == 0) {
-                               waveStart = i+1;
-                               //prnt("DEBUG: waveStart: %d",waveStart);
-                       } else {
-                               waveEnd = i+1;
-                               //prnt("DEBUG: waveEnd: %d",waveEnd);
-                               waveLenCnt = waveEnd-waveStart;
-                               if (waveLenCnt > fc){
-                                       firstFullWave = waveStart;
-                                       fullWaveLen=waveLenCnt;
-                                       break;
-                               } 
-                               waveStart=0;
-                       }
-               }
+       //find start of modulating data in trace 
+       i = findModStart(dest, size, *fc);
+
+       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
+               firstFullWave = 160;
+               fullWaveLen = 0;

        }
        }

+

        *firstPhaseShift = firstFullWave;
        if (g_debugMode ==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
        //test each valid clock from greatest to smallest to see which lines up
        *firstPhaseShift = firstFullWave;
        if (g_debugMode ==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
        //test each valid clock from greatest to smallest to see which lines up

-       for(clkCnt=7; clkCnt >= 1 ; clkCnt--){
+       for(clkCnt=7; clkCnt >= 1 ; clkCnt--) {
+               tol = *fc/2;

                lastClkBit = firstFullWave; //set end of wave as clock align
                waveStart = 0;
                errCnt=0;
                lastClkBit = firstFullWave; //set end of wave as clock align
                waveStart = 0;
                errCnt=0;


@@ -779,9 +797,9 @@ int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseSh
                                } else { //waveEnd
                                        waveEnd = i+1;
                                        waveLenCnt = waveEnd-waveStart;
                                } else { //waveEnd
                                        waveEnd = i+1;
                                        waveLenCnt = waveEnd-waveStart;

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

                                                //if this wave is a phase shift
                                                //if this wave is a phase shift

-                                               if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,i+1,fc);
+                                               if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,i+1,*fc);

                                                if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
                                                        peakcnt++;
                                                        lastClkBit+=clk[clkCnt];
                                                if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
                                                        peakcnt++;
                                                        lastClkBit+=clk[clkCnt];


@@ -790,7 +808,7 @@ int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseSh
                                                } else { //phase shift before supposed to based on clock
                                                        errCnt++;
                                                }
                                                } else { //phase shift before supposed to based on clock
                                                        errCnt++;
                                                }

-                                       } else if (i+1 > lastClkBit + clk[clkCnt] + tol + fc){
+                                       } else if (i+1 > lastClkBit + clk[clkCnt] + tol + *fc){

                                                lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
                                        }
                                        waveStart=i+1;
                                                lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
                                        }
                                        waveStart=i+1;


@@ -815,10 +833,11 @@ int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseSh
        return clk[best];
 }
 
        return clk[best];
 }
 

-int DetectPSKClock(uint8_t dest[], size_t size, int clock) {
-       int firstPhaseShift = 0;
-       return DetectPSKClock_ext(dest, size, clock, &firstPhaseShift);
-}
+//int DetectPSKClock(uint8_t dest[], size_t size, int clock) {
+//     size_t firstPhaseShift = 0;
+//     uint8_t curPhase = 0;
+//     return DetectPSKClock_ext(dest, size, clock, &firstPhaseShift, &curPhase);
+//}

 
 //by marshmellow
 //detects the bit clock for FSK given the high and low Field Clocks
 
 //by marshmellow
 //detects the bit clock for FSK given the high and low Field Clocks


@@ -939,6 +958,8 @@ 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...
 bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) {
 
 // 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, size_t *i) {

+       if (buffSize < *i+4) return false;
+

        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


@@ -961,7 +982,7 @@ bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststa
        //need to loop through all samples and identify our clock, look for the ST pattern
        int clk = 0; 
        int tol = 0;
        //need to loop through all samples and identify our clock, look for the ST pattern
        int clk = 0; 
        int tol = 0;

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

        size_t i = 0;
        //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
        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
        size_t i = 0;
        //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
        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


@@ -1029,9 +1050,9 @@ bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststa
                return false;
        }
        size_t dataloc = start;
                return false;
        }
        size_t dataloc = start;

-       if (buffer[dataloc-(clk*4)-(clk/8)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {
+       if (buffer[dataloc-(clk*4)-(clk/4)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {

                //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start 
                //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start 

-               for ( i=0; i <= (clk/8); ++i ) {
+               for ( i=0; i <= (clk/4); ++i ) {

                        if ( buffer[dataloc - (clk*4) - i] <= low ) {
                                dataloc -= i;
                                break;
                        if ( buffer[dataloc - (clk*4) - i] <= low ) {
                                dataloc -= i;
                                break;


@@ -1046,14 +1067,15 @@ bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststa
        // warning - overwriting buffer given with raw wave data with ST removed...
        while ( dataloc < bufsize-(clk/2) ) {
                //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part)
        // warning - overwriting buffer given with raw wave data with ST removed...
        while ( dataloc < bufsize-(clk/2) ) {
                //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part)

-               if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+3]<high && buffer[dataloc+3]>low) {
+               if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+clk/4]<high && buffer[dataloc+clk/4]>low) {

                        for(i=0; i < clk/2-tol; ++i) {
                                buffer[dataloc+i] = high+5;
                        }
                        for(i=0; i < clk/2-tol; ++i) {
                                buffer[dataloc+i] = high+5;
                        }

-               } //test for single sample outlier (high between two lows) in the case of very strong waves
-               if (buffer[dataloc] >= high && buffer[dataloc+2] <= low) {
-                       buffer[dataloc] = buffer[dataloc+2];
-                       buffer[dataloc+1] = buffer[dataloc+2];
+               } //test for small spike outlier (high between two lows) in the case of very strong waves
+               if (buffer[dataloc] > low && buffer[dataloc+clk/4] <= low) {
+                       for(i=0; i < clk/4; ++i) {
+                               buffer[dataloc+i] = buffer[dataloc+clk/4];
+                       }

                }
                if (firstrun) {
                        *stend = dataloc;
                }
                if (firstrun) {
                        *stend = dataloc;


@@ -1334,9 +1356,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;


@@ -1368,10 +1391,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) {


@@ -1380,16 +1399,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;


@@ -1401,7 +1419,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


@@ -1534,56 +1552,31 @@ void psk2TOpsk1(uint8_t *BitStream, size_t size) {
 //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;
-       fc = countFC(dest, *size, 1);
-       uint8_t fc2 = fc >> 8;
-       if (fc2 == 10) return -1; //fsk found - quit
-       fc = fc & 0xFF;
-       if (fc!=2 && fc!=4 && fc!=8) return -1;
-       //PrintAndLog("DEBUG: FC: %d",fc);
-       *clock = DetectPSKClock(dest, *size, *clock);
+       uint8_t fc=0;
+       size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
+       uint16_t fullWaveLen=0, waveLenCnt=0, avgWaveVal;
+       uint16_t errCnt=0, errCnt2=0;
+       
+       *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc);

        if (*clock == 0) return -1;
        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);
-
-       //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];
-       }
+       //if clock detect found firstfullwave...
+       uint16_t tol = fc/2;

        if (firstFullWave == 0) {
        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
-               firstFullWave = 160;
-               memset(dest, curPhase, firstFullWave / *clock);
+               //find start of modulating data in trace 
+               i = findModStart(dest, *size, fc);
+               //find first phase shift
+               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
+                       firstFullWave = 160;
+                       memset(dest, curPhase, firstFullWave / *clock);
+               } else {
+                       memset(dest, curPhase^1, firstFullWave / *clock);
+               }

        } else {
                memset(dest, curPhase^1, firstFullWave / *clock);
        }
        } else {
                memset(dest, curPhase^1, firstFullWave / *clock);
        }


@@ -1596,9 +1589,9 @@ int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *
        if (g_debugMode==2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit,(unsigned int) fc);
        waveStart = 0;
        dest[numBits++] = curPhase; //set first read bit
        if (g_debugMode==2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit,(unsigned int) fc);
        waveStart = 0;
        dest[numBits++] = curPhase; //set first read bit

-       for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){
+       for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++) {

                //top edge of wave = start of new wave 
                //top edge of wave = start of new wave 

-               if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
+               if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]) {

                        if (waveStart == 0) {
                                waveStart = i+1;
                                waveLenCnt = 0;
                        if (waveStart == 0) {
                                waveStart = i+1;
                                waveLenCnt = 0;


@@ -1606,27 +1599,27 @@ 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){
-                                       //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
+                               if (waveLenCnt > fc) {

                                        //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);
                                        //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
+                                       if (i+1 >= lastClkBit + *clock - tol) { //should be a clock bit

                                                curPhase ^= 1;
                                                dest[numBits++] = curPhase;
                                                lastClkBit += *clock;
                                                curPhase ^= 1;
                                                dest[numBits++] = curPhase;
                                                lastClkBit += *clock;

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

                                                //noise after a phase shift - ignore
                                        } else { //phase shift before supposed to based on clock
                                                errCnt++;
                                                dest[numBits++] = 7;
                                        }
                                                //noise after a phase shift - ignore
                                        } else { //phase shift before supposed to based on clock
                                                errCnt++;
                                                dest[numBits++] = 7;
                                        }

-                               } else if (i+1 > lastClkBit + *clock + tol + fc){
+                               } else if (i+1 > lastClkBit + *clock + tol + fc) {

                                        lastClkBit += *clock; //no phase shift but clock bit
                                        dest[numBits++] = curPhase;
                                } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often)
                                        errCnt2++;
                                        if(errCnt2 > 101) return errCnt2;
                                        lastClkBit += *clock; //no phase shift but clock bit
                                        dest[numBits++] = curPhase;
                                } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often)
                                        errCnt2++;
                                        if(errCnt2 > 101) return errCnt2;

+                                       avgWaveVal += dest[i+1];
+                                       continue;

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


@@ -1694,6 +1687,7 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_
                *hi = (bytebits_to_byte(BitStream, 24)); 
                *lo = ((uint64_t)(bytebits_to_byte(BitStream + 24, 32)) << 32) | (bytebits_to_byte(BitStream + 24 + 32, 32));
        } else {
                *hi = (bytebits_to_byte(BitStream, 24)); 
                *lo = ((uint64_t)(bytebits_to_byte(BitStream + 24, 32)) << 32) | (bytebits_to_byte(BitStream + 24 + 32, 32));
        } else {

+               if (g_debugMode) prnt("Error removing parity: %u", *size);

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


@@ -1710,6 +1704,8 @@ int FDXBdemodBI(uint8_t *dest, size_t *size) {
 
        uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
        if (errChk == 0) return -2; //preamble not found
 
        uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
        if (errChk == 0) return -2; //preamble not found

+       if (*size != 128) return -3; //wrong size for fdxb
+       //return start position

        return (int)startIdx;
 }
 
        return (int)startIdx;
 }
 


@@ -1808,7 +1804,7 @@ int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert) {
        } 
        if (*size != 64 && *size != 224) return -2;
        if (*invert==1)
        } 
        if (*size != 64 && *size != 224) return -2;
        if (*invert==1)

-               for (size_t i = startidx; i < *size; i++)
+               for (size_t i = startidx; i < *size + startidx; i++) 

                        bitStream[i] ^= 1;
 
        return (int) startidx;
                        bitStream[i] ^= 1;
 
        return (int) startidx;