fix return value for error.

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 74e3e6b64a0b0e0e35ecf8ca1f4b06904281a746..c56301c07f07ab75c0da0499fbf59d504e204859 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -35,11 +35,14 @@
 #include <string.h>  // for memset, memcmp and size_t
 #include <stdint.h>  // for uint_32+
 #include <stdbool.h> // for bool
 #include <string.h>  // for memset, memcmp and size_t
 #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 +56,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 +74,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 +83,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 +194,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;


@@ -307,15 +304,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 +508,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...


@@ -625,11 +624,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


@@ -1334,9 +1328,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 +1363,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 +1371,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 +1391,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


@@ -1531,54 +1521,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


@@ -1606,9 +1597,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


@@ -1694,6 +1683,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 +1700,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;
 }