X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/070e3c4305666adaee5efd9a1d46079695d8ce49..643fb10b2df5248f830f0a9e3d8ae43b24e1c2b2:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index d3c2a01c..c00222b3 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -34,8 +34,8 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi if (BitStream[i] < *low) *low = BitStream[i]; } if (*high < 123) return -1; // just noise - *high = (int)(((*high-128)*(((float)fuzzHi)/100))+128); - *low = (int)(((*low-128)*(((float)fuzzLo)/100))+128); + *high = ((*high-128)*fuzzHi + 12800)/100; + *low = ((*low-128)*fuzzLo + 12800)/100; return 1; } @@ -114,6 +114,57 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ return 0; } +// demodulates strong heavily clipped samples +int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low) +{ + size_t bitCnt=0, smplCnt=0, errCnt=0; + uint8_t waveHigh = 0; + //PrintAndLog("clk: %d", clk); + for (size_t i=0; i < *size; i++){ + if (BinStream[i] >= high && waveHigh){ + smplCnt++; + } else if (BinStream[i] <= low && !waveHigh){ + smplCnt++; + } else { //transition + if ((BinStream[i] >= high && !waveHigh) || (BinStream[i] <= low && waveHigh)){ + if (smplCnt > clk-(clk/4)-1) { //full clock + if (smplCnt > clk + (clk/4)+1) { //too many samples + errCnt++; + BinStream[bitCnt++]=77; + } else if (waveHigh) { + BinStream[bitCnt++] = invert; + BinStream[bitCnt++] = invert; + } else if (!waveHigh) { + BinStream[bitCnt++] = invert ^ 1; + BinStream[bitCnt++] = invert ^ 1; + } + waveHigh ^= 1; + smplCnt = 0; + } else if (smplCnt > (clk/2) - (clk/4)-1) { + if (waveHigh) { + BinStream[bitCnt++] = invert; + } else if (!waveHigh) { + BinStream[bitCnt++] = invert ^ 1; + } + waveHigh ^= 1; + smplCnt = 0; + } else if (!bitCnt) { + //first bit + waveHigh = (BinStream[i] >= high); + smplCnt = 1; + } else { + smplCnt++; + //transition bit oops + } + } else { //haven't hit new high or new low yet + smplCnt++; + } + } + } + *size = bitCnt; + return errCnt; +} + //by marshmellow //takes 3 arguments - clock, invert, maxErr as integers //attempts to demodulate ask while decoding manchester @@ -121,7 +172,7 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr) { size_t i; - int start = DetectASKClock(BinStream, *size, clk, 20); //clock default + int start = DetectASKClock(BinStream, *size, clk, maxErr); //clock default if (*clk==0 || start < 0) return -3; if (*invert != 1) *invert=0; uint8_t initLoopMax = 255; @@ -131,58 +182,20 @@ int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max int high, low; if (getHiLo(BinStream, initLoopMax, &high, &low, 75, 75) < 1) return -2; //just noise + // if clean clipped waves detected run alternate demod + if (DetectCleanAskWave(BinStream, *size, high, low)) { + cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); + return manrawdecode(BinStream, size); + } + // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low); - int lastBit = 0; //set first clock check + int lastBit; //set first clock check uint16_t bitnum = 0; //output counter uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave if (*clk <= 32) tol=1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely - size_t iii = 0; - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr) initLoopMax = *clk * 2; uint16_t errCnt = 0, MaxBits = 512; - uint16_t bestStart = start; - uint16_t bestErrCnt = 0; - // PrintAndLog("DEBUG - lastbit - %d",lastBit); - // if best start position not already found by detect clock then - if (start <= 0 || start > initLoopMax){ - bestErrCnt = maxErr+1; - // loop to find first wave that works - for (iii=0; iii < initLoopMax; ++iii){ - // if no peak skip - if (BinStream[iii] < high && BinStream[iii] > low) continue; - - lastBit = iii - *clk; - // loop through to see if this start location works - for (i = iii; i < *size; ++i) { - if ((i-lastBit) > (*clk-tol) && (BinStream[i] >= high || BinStream[i] <= low)) { - lastBit += *clk; - } else if ((i-lastBit) > (*clk+tol)) { - errCnt++; - lastBit += *clk; - } - if ((i-iii) > (MaxBits * *clk) || errCnt > maxErr) break; //got plenty of bits or too many errors - } - //we got more than 64 good bits and not all errors - if ((((i-iii)/ *clk) > (64)) && (errCnt<=maxErr)) { - //possible good read - if (!errCnt || errCnt < bestErrCnt){ - bestStart = iii; //set this as new best run - bestErrCnt = errCnt; - if (!errCnt) break; //great read - finish - } - } - errCnt = 0; - } - } - if (bestErrCnt > maxErr){ - *invert = bestStart; - *clk = iii; - return -1; - } - //best run is good enough set to best run and set overwrite BinStream - lastBit = bestStart - *clk; - errCnt = 0; - for (i = bestStart; i < *size; ++i) { + lastBit = start - *clk; + for (i = start; i < *size; ++i) { if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){ //high found and we are expecting a bar lastBit += *clk; @@ -203,7 +216,7 @@ int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max if (bitnum >= MaxBits) break; } *size = bitnum; - return bestErrCnt; + return errCnt; } //by marshmellow @@ -242,19 +255,17 @@ int manrawdecode(uint8_t * BitStream, size_t *size) } errCnt=0; } - if (bestErr<20){ - for (i=bestRun; i < *size-2; i+=2){ - if(BitStream[i] == 1 && (BitStream[i+1] == 0)){ - BitStream[bitnum++]=0; - } else if((BitStream[i] == 0) && BitStream[i+1] == 1){ - BitStream[bitnum++]=1; - } else { - BitStream[bitnum++]=77; - } - if(bitnum>MaxBits) break; + for (i=bestRun; i < *size-2; i+=2){ + if(BitStream[i] == 1 && (BitStream[i+1] == 0)){ + BitStream[bitnum++]=0; + } else if((BitStream[i] == 0) && BitStream[i+1] == 1){ + BitStream[bitnum++]=1; + } else { + BitStream[bitnum++]=77; } - *size=bitnum; + if(bitnum>MaxBits) break; } + *size=bitnum; return bestErr; } @@ -319,70 +330,19 @@ void askAmp(uint8_t *BitStream, size_t size) return; } -// demodulates strong heavily clipped samples -int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low) -{ - size_t bitCnt=0, smplCnt=0, errCnt=0; - uint8_t waveHigh = 0; - //PrintAndLog("clk: %d", clk); - for (size_t i=0; i < *size; i++){ - if (BinStream[i] >= high && waveHigh){ - smplCnt++; - } else if (BinStream[i] <= low && !waveHigh){ - smplCnt++; - } else { //transition - if ((BinStream[i] >= high && !waveHigh) || (BinStream[i] <= low && waveHigh)){ - if (smplCnt > clk-(clk/4)-1) { //full clock - if (smplCnt > clk + (clk/4)+1) { //too many samples - errCnt++; - BinStream[bitCnt++]=77; - } else if (waveHigh) { - BinStream[bitCnt++] = invert; - BinStream[bitCnt++] = invert; - } else if (!waveHigh) { - BinStream[bitCnt++] = invert ^ 1; - BinStream[bitCnt++] = invert ^ 1; - } - waveHigh ^= 1; - smplCnt = 0; - } else if (smplCnt > (clk/2) - (clk/4)-1) { - if (waveHigh) { - BinStream[bitCnt++] = invert; - } else if (!waveHigh) { - BinStream[bitCnt++] = invert ^ 1; - } - waveHigh ^= 1; - smplCnt = 0; - } else if (!bitCnt) { - //first bit - waveHigh = (BinStream[i] >= high); - smplCnt = 1; - } else { - smplCnt++; - //transition bit oops - } - } else { //haven't hit new high or new low yet - smplCnt++; - } - } - } - *size = bitCnt; - return errCnt; -} - //by marshmellow //takes 3 arguments - clock, invert and maxErr as integers //attempts to demodulate ask only int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp) { if (*size==0) return -1; - int start = DetectASKClock(BinStream, *size, clk, 20); //clock default + int start = DetectASKClock(BinStream, *size, clk, maxErr); //clock default if (*clk==0 || start < 0) return -1; if (*invert != 1) *invert = 0; if (amp==1) askAmp(BinStream, *size); uint8_t initLoopMax = 255; - if (initLoopMax > *size) initLoopMax=*size; + if (initLoopMax > *size) initLoopMax = *size; // Detect high and lows //25% clip in case highs and lows aren't clipped [marshmellow] int high, low; @@ -393,67 +353,13 @@ int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max if (DetectCleanAskWave(BinStream, *size, high, low)) return cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); - int lastBit = 0; //set first clock check - can go negative - size_t i, iii = 0; - size_t errCnt = 0, bitnum = 0; //output counter + int lastBit; //set first clock check - can go negative + size_t i, errCnt = 0, bitnum = 0; //output counter uint8_t midBit = 0; - size_t bestStart = start, bestErrCnt = 0; //(*size/1000); size_t MaxBits = 1024; + lastBit = start - *clk; - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr) initLoopMax = *clk * 2; - //if best start not already found by detectclock - if (start <= 0 || start > initLoopMax){ - bestErrCnt = maxErr+1; - //PrintAndLog("DEBUG - lastbit - %d",lastBit); - //loop to find first wave that works - for (iii=0; iii < initLoopMax; ++iii){ - if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){ - lastBit = iii - *clk; - //loop through to see if this start location works - for (i = iii; i < *size; ++i) { - if (i-lastBit > *clk && (BinStream[i] >= high || BinStream[i] <= low)){ - lastBit += *clk; - midBit = 0; - } else if (i-lastBit > (*clk/2) && midBit == 0) { - midBit = 1; - } else if ((i-lastBit) > *clk) { - //should have hit a high or low based on clock!! - //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit); - errCnt++; - lastBit += *clk;//skip over until hit too many errors - if (errCnt > maxErr) - break; - } - if ((i-iii)>(MaxBits * *clk)) break; //got enough bits - } - //we got more than 64 good bits and not all errors - if ((((i-iii)/ *clk) > 64) && (errCnt<=maxErr)) { - //possible good read - if (errCnt==0){ - bestStart=iii; - bestErrCnt=errCnt; - break; //great read - finish - } - if (errCnt maxErr){ - *invert = bestStart; - *clk = iii; - return -1; - } - //best run is good enough - set to best run and overwrite BinStream - lastBit = bestStart - *clk - 1; - errCnt = 0; - - for (i = bestStart; i < *size; ++i) { + for (i = start; i < *size; ++i) { if (i - lastBit > *clk){ if (BinStream[i] >= high) { BinStream[bitnum++] = *invert; @@ -559,28 +465,26 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, size_t idx=0; size_t numBits=0; uint32_t n=1; - uint16_t lowWaves = ((rfLen*100/fclow)); // (((float)(rfLen))/((float)fclow)); - uint16_t highWaves = ((rfLen*100/fchigh)); // (((float)(rfLen))/((float)fchigh)); for( idx=1; idx < size; idx++) { n++; if (dest[idx]==lastval) continue; //if lastval was 1, we have a 1->0 crossing if (dest[idx-1]==1) { - if (!numBits && n < lowWaves/100) { + if (!numBits && n < rfLen/fclow) { n=0; lastval = dest[idx]; continue; } - n = (size_t)((((n*1000)/lowWaves)+5)/10); + n = (n * fclow + rfLen/2) / rfLen; } else {// 0->1 crossing //test first bitsample too small - if (!numBits && n < highWaves/100) { + if (!numBits && n < rfLen/fchigh) { n=0; lastval = dest[idx]; continue; } - n = (((n*1000)/highWaves)+5)/10; + n = (n * fchigh + rfLen/2) / rfLen; } if (n == 0) n = 1; @@ -590,11 +494,11 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, lastval=dest[idx]; }//end for // if valid extra bits at the end were all the same frequency - add them in - if (n > highWaves/100) { + if (n > rfLen/fchigh) { if (dest[idx-2]==1) { - n=(((n*1000)/lowWaves)+5)/10; + n = (n * fclow + rfLen/2) / rfLen; } else { - n=(((n*1000)/highWaves)+5)/10; + n = (n * fchigh + rfLen/2) / rfLen; } memset(dest+numBits, dest[idx-1]^invert , n); numBits += n; @@ -858,14 +762,17 @@ int DetectStrongAskClock(uint8_t dest[], size_t size) // return start index of best starting position for that clock and return clock (by reference) int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) { - size_t i=0; - uint8_t clk[]={8,16,32,40,50,64,100,128,255}; + size_t i=1; + uint8_t clk[]={255,8,16,32,40,50,64,100,128,255}; uint8_t loopCnt = 255; //don't need to loop through entire array... - if (size <= loopCnt) return -1; //not enough samples - //if we already have a valid clock quit - - for (;i<8;++i) - if (clk[i] == *clock) return 0; + if (size==0) return -1; + if (size <= loopCnt) loopCnt = size-1; //not enough samples + + //if we already have a valid clock + uint8_t clockFnd=0; + for (;i<9;++i) + if (clk[i] == *clock) clockFnd=i; + //clock found but continue to find best startpos //get high and low peak int peak, low; @@ -874,10 +781,11 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) //test for large clean peaks if (DetectCleanAskWave(dest, size, peak, low)==1){ int ans = DetectStrongAskClock(dest, size); - for (i=7; i>0; i--){ + for (i=8; i>1; i--){ if (clk[i] == ans) { *clock = ans; - return 0; + clockFnd = i; + break; //clock found but continue to find best startpos } } } @@ -888,16 +796,20 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) size_t errCnt = 0; size_t arrLoc, loopEnd; //test each valid clock from smallest to greatest to see which lines up - for(clkCnt=0; clkCnt < 8; clkCnt++){ + uint8_t clkEnd=9; + if (clockFnd>0) clkEnd=clockFnd+1; + else clockFnd=1; + + for(clkCnt=clockFnd; clkCnt < clkEnd; clkCnt++){ if (clk[clkCnt] == 32){ tol=1; }else{ tol=0; } - if (!maxErr) loopCnt=clk[clkCnt]*2; + if (!maxErr && loopCnt>clk[clkCnt]*3) loopCnt=clk[clkCnt]*3; bestErr[clkCnt]=1000; - //try lining up the peaks by moving starting point (try first 256) - for (ii=0; ii < loopCnt; ii++){ + //try lining up the peaks by moving starting point (try first few clocks) + for (ii=0; ii < loopCnt-tol-clk[clkCnt]; ii++){ if (dest[ii] < peak && dest[ii] > low) continue; errCnt=0;