Merge pull request #215 from AdamLaurie/master

[proxmark3-svn] / common / lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index a6e979a28494f559d0659403d70140715db59077..8324c440f4c19f918159215ff6dc94b397596f80 100644 (file)
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -81,10 +81,9 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p
                j--; // overwrite parity with next data
                // 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
-                       default: //test parity
-                               if (parityTest(parityWd, pLen, pType) == 0) return 0; break;
+                       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
+                       default: if (parityTest(parityWd, pLen, pType) == 0) {return 0;} break; //test parity
                }
                bitCnt+=(pLen-1);
                parityWd = 0;
@@ -271,7 +270,7 @@ void askAmp(uint8_t *BitStream, size_t size)
        }
        return;
 }
-
+ 
 //by marshmellow
 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
 int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType)
@@ -281,7 +280,7 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr
        if (*clk==0 || start < 0) return -3;
        if (*invert != 1) *invert = 0;
        if (amp==1) askAmp(BinStream, *size);
-       if (g_debugMode==2) prnt("DEBUG ASK: clk %d, beststart %d", *clk, start);
+       if (g_debugMode==2) prnt("DEBUG ASK: clk %d, beststart %d, amp %d", *clk, start, amp);
 
        uint8_t initLoopMax = 255;
        if (initLoopMax > *size) initLoopMax = *size;
@@ -1556,8 +1555,8 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
        int tol = 0;
        int i, j, skip, start, end, low, high, minClk, waveStart;
        bool complete = false;
-       int tmpbuff[bufsize / 64];
-       int waveLen[bufsize / 64];
+       int tmpbuff[bufsize / 32]; //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured
+       int waveLen[bufsize / 32]; //  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;
@@ -1589,7 +1588,7 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
                waveStart = i;
                while ((buffer[i] > low) && (i < bufsize))
                        ++i;
-               if (j >= (bufsize/64)) {
+               if (j >= (bufsize/32)) {
                        break;
                }
                waveLen[j] = i - waveStart; //first high to first low
@@ -1635,6 +1634,8 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
        if (start < 0) {
                if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
                return false;
+       } else {
+               if (g_debugMode==2) prnt("DEBUG STT: first STT found at: %d, j=%d",start, j);
        }
        if (waveLen[i+2] > clk*1+tol)
                phaseoff = 0;
@@ -1648,7 +1649,7 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
        end = skip;
        for (i += 3; i < j - 4; ++i) {
                end += tmpbuff[i];
-               if (tmpbuff[i] >= clk*1-tol && tmpbuff[i] <= (clk*2)+tol) {           //1 to 2 clocks depending on 2 bits prior
+               if (tmpbuff[i] >= clk*1-tol && tmpbuff[i] <= (clk*2)+tol && waveLen[i] < clk+tol) {           //1 to 2 clocks depending on 2 bits prior
                        if (tmpbuff[i+1] >= clk*2-tol && tmpbuff[i+1] <= clk*2+tol && waveLen[i+1] > clk*3/2-tol) {       //2 clocks and wave size is 1 1/2
                                if (tmpbuff[i+2] >= (clk*3)/2-tol && tmpbuff[i+2] <= clk*2+tol && waveLen[i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave
                                        if (tmpbuff[i+3] >= clk*1-tol && tmpbuff[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
@@ -1670,12 +1671,15 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
        start = skip;
        size_t datalen = end - start;
        // check validity of datalen (should be even clock increments)  - use a tolerance of up to 1/8th a clock
-       if (datalen % clk > clk/8) {
+       if ( clk - (datalen % clk) <= clk/8) {
+               // padd the amount off - could be problematic...  but shouldn't happen often
+               datalen += clk - (datalen % clk);
+       } else if ( (datalen % clk) <= clk/8 ) {
+               // padd the amount off - could be problematic...  but shouldn't happen often
+               datalen -= datalen % clk;
+       } else {
                if (g_debugMode==2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk);
                return false;
-       } else {
-               // padd the amount off - could be problematic...  but shouldn't happen often
-               datalen += datalen % clk;
        }
        // if datalen is less than one t55xx block - ERROR
        if (datalen/clk < 8*4) {
@@ -1683,8 +1687,20 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
                return false;
        }
        size_t dataloc = start;
+       if (buffer[dataloc-(clk*4)-(clk/8)] <= 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 
+               for ( i=0; i <= (clk/8); ++i ) {
+                       if ( buffer[dataloc - (clk*4) - i] <= low ) {
+                               dataloc -= i;
+                               break;
+                       }
+               }
+       }
+       
        size_t newloc = 0;
        i=0;
+       if (g_debugMode==2) prnt("DEBUG STT: Starting STT trim - start: %d, datalen: %d ",dataloc, datalen);            
+
        // 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)
@@ -1698,11 +1714,12 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
                                if (i+newloc < dataloc)
                                        buffer[i+newloc] = buffer[dataloc];
 
-                               dataloc++;                              
+                               dataloc++;
                        }
                }
                newloc += i;
                //skip next ST  -  we just assume it will be there from now on...
+               if (g_debugMode==2) prnt("DEBUG STT: skipping STT at %d to %d", dataloc, dataloc+(clk*4));
                dataloc += clk*4;
        }
        *size = newloc;