further refactor

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 750dbf1b43a17bd1d2c011abde63f752748fff0e..74e3e6b64a0b0e0e35ecf8ca1f4b06904281a746 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -39,7 +39,7 @@
 //**********************************************************************************************
 //---------------------------------Utilities Section--------------------------------------------
 //**********************************************************************************************
 //**********************************************************************************************
 //---------------------------------Utilities Section--------------------------------------------
 //**********************************************************************************************

-
+#define LOWEST_DEFAULT_CLOCK 32

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


@@ -222,6 +222,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) {


@@ -289,28 +347,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 +370,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


@@ -888,7 +938,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 +959,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");


@@ -1074,17 +1082,18 @@ bool DetectST(uint8_t   buffer[], size_t *size, int *foundclock) {
 }
 
 //by marshmellow
 }
 
 //by marshmellow

-//take 11 10 01 11 00 and make 01100 ... miller decoding
+//take 11 10 01 11 00 and make 01100 ... miller decoding 

 //check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
 //decodes miller encoded binary
 //NOTE  askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!
 //check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
 //decodes miller encoded binary
 //NOTE  askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!

-/*int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) {
+int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) {

        if (*size < 16) return -1;
        if (*size < 16) return -1;

-       uint16_t MaxBits = 512, errCnt = 0, bestErr = 1000, bestRun = 0;
-       size_t i, ii, bitCnt=0;
-       uint8_t alignCnt = 0, curBit = BitStream[0];
+       uint16_t MaxBits = 512, errCnt = 0;
+       size_t i, bitCnt=0;
+       uint8_t alignCnt = 0, curBit = BitStream[0], alignedIdx = 0;
+       uint8_t halfClkErr = 0;

        //find alignment, needs 4 1s or 0s to properly align
        //find alignment, needs 4 1s or 0s to properly align

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

                alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0;
                curBit = BitStream[i];
                if (alignCnt == 4) break;
                alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0;
                curBit = BitStream[i];
                if (alignCnt == 4) break;


@@ -1094,8 +1103,21 @@ bool DetectST(uint8_t    buffer[], size_t *size, int *foundclock) {
                if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it");
                return -1;
        }
                if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it");
                return -1;
        }

+       alignedIdx = (i-1) % 2;
+       for (i=alignedIdx; i < *size-3; i+=2) {
+               halfClkErr = (uint8_t)((halfClkErr << 1 | BitStream[i]) & 0xFF);
+               if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) {
+                       errCnt++;
+                       BitStream[bitCnt++] = 7;
+                       continue;
+               }
+               BitStream[bitCnt++] = BitStream[i] ^ BitStream[i+1] ^ invert;

 
 

-}*/
+               if (bitCnt > MaxBits) break;
+       }
+       *size = bitCnt;
+       return errCnt;
+}

 
 //by marshmellow
 //take 01 or 10 = 1 and 11 or 00 = 0
 
 //by marshmellow
 //take 01 or 10 = 1 and 11 or 00 = 0


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

-

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