X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/7009555e448259178dbd7443c30cd96047647b2e..2b90e3e670393990a243e1791dccd84acb9526eb:/common/lfdemod.c?ds=sidebyside diff --git a/common/lfdemod.c b/common/lfdemod.c index fae61206..7d40d22e 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -11,8 +11,6 @@ #include #include #include "lfdemod.h" - - uint8_t justNoise(uint8_t *BitStream, size_t size) { static const uint8_t THRESHOLD = 123; @@ -31,13 +29,13 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi *high=0; *low=255; // get high and low thresholds - for (int i=0; i < size; i++){ + for (size_t i=0; i < size; i++){ if (BitStream[i] > *high) *high = BitStream[i]; 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; } @@ -83,10 +81,8 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ // otherwise could be a void with no arguments //set defaults uint32_t i = 0; - if (BitStream[1]>1){ //allow only 1s and 0s - // PrintAndLog("no data found"); - return 0; - } + if (BitStream[1]>1) return 0; //allow only 1s and 0s + // 111111111 bit pattern represent start of frame // include 0 in front to help get start pos uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1}; @@ -102,11 +98,8 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ idx = *startIdx + 9; for (i=0; i> 63); @@ -120,185 +113,189 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ } //by marshmellow -//takes 3 arguments - clock, invert, maxErr as integers -//attempts to demodulate ask while decoding manchester -//prints binary found and saves in graphbuffer for further commands -int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr) +//demodulates strong heavily clipped samples +int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low) { - int i; - //int clk2=*clk; - int start = DetectASKClock(BinStream, *size, clk, 20); //clock default - if (*clk==0) return -3; - if (start < 0) return -3; - // if autodetected too low then adjust //MAY NEED ADJUSTMENT - //if (clk2==0 && *clk<8) *clk =64; - //if (clk2==0 && *clk<32) *clk=32; - if (*invert != 0 && *invert != 1) *invert=0; - uint32_t initLoopMax = 200; - if (initLoopMax > *size) initLoopMax=*size; - // Detect high and lows - // 25% fuzz in case highs and lows aren't clipped [marshmellow] - int high, low, ans; - ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75); - if (ans<1) return -2; //just noise - - // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low); - int lastBit = 0; //set first clock check - uint32_t bitnum = 0; //output counter - int 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 - int iii = 0; - uint32_t gLen = *size; - if (gLen > 3000) gLen=3000; - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr) gLen=*clk*2; - uint8_t errCnt =0; - uint16_t MaxBits = 500; - uint32_t bestStart = *size; - int bestErrCnt = maxErr+1; - // PrintAndLog("DEBUG - lastbit - %d",lastBit); - // loop to find first wave that works - for (iii=0; iii < gLen; ++iii){ - if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){ - lastBit=iii-*clk; - errCnt=0; - // loop through to see if this start location works - for (i = iii; i < *size; ++i) { - if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){ - lastBit+=*clk; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - } else { - //mid value found or no bar supposed to be here - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - - //debug - //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); - + size_t bitCnt=0, smplCnt=0, errCnt=0; + uint8_t waveHigh = 0; + 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++; - lastBit+=*clk;//skip over until hit too many errors - if (errCnt>(maxErr)) break; //allow 1 error for every 1000 samples else start over + BinStream[bitCnt++]=7; + } 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 } - if ((i-iii) >(MaxBits * *clk)) break; //got plenty of 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= high) && ((i-lastBit) > (*clk-tol))){ - lastBit += *clk; - BinStream[bitnum] = *invert; - bitnum++; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - BinStream[bitnum] = 1-*invert; - bitnum++; - } else { - //mid value found or no bar supposed to be here - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - - //debug - //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); - if (bitnum > 0){ - BinStream[bitnum]=77; - bitnum++; - } + *size = bitCnt; + return errCnt; +} - lastBit+=*clk;//skip over error - } - } - if (bitnum >=MaxBits) break; - } - *size=bitnum; - } else{ - *invert=bestStart; - *clk=iii; - return -1; +//by marshmellow +void askAmp(uint8_t *BitStream, size_t size) +{ + for(size_t i = 1; i=30) //large jump up + BitStream[i]=127; + else if(BitStream[i]-BitStream[i-1]<=-20) //large jump down + BitStream[i]=-127; } - return bestErrCnt; + return; } //by marshmellow -//encode binary data into binary manchester -int ManchesterEncode(uint8_t *BitStream, size_t size) +//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) { - size_t modIdx=20000, i=0; - if (size>modIdx) return -1; - for (size_t idx=0; idx < size; idx++){ - BitStream[idx+modIdx++] = BitStream[idx]; - BitStream[idx+modIdx++] = BitStream[idx]^1; + if (*size==0) return -1; + int start = DetectASKClock(BinStream, *size, clk, maxErr); //clock default + if (*clk==0 || start < 0) return -3; + if (*invert != 1) *invert = 0; + if (amp==1) askAmp(BinStream, *size); + + uint8_t initLoopMax = 255; + if (initLoopMax > *size) initLoopMax = *size; + // Detect high and lows + //25% clip in case highs and lows aren't clipped [marshmellow] + int high, low; + if (getHiLo(BinStream, initLoopMax, &high, &low, 75, 75) < 1) + return -2; //just noise + + size_t errCnt = 0; + // if clean clipped waves detected run alternate demod + if (DetectCleanAskWave(BinStream, *size, high, low)) { + errCnt = cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); + if (askType) //askman + return manrawdecode(BinStream, size, 0); + else //askraw + return errCnt; } - for (; i<(size*2); i++){ - BitStream[i] = BitStream[i+20000]; + + int lastBit; //set first clock check - can go negative + size_t i, bitnum = 0; //output counter + uint8_t midBit = 0; + 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 MaxBits = 1024; + lastBit = start - *clk; + + for (i = start; i < *size; ++i) { + if (i-lastBit >= *clk-tol){ + if (BinStream[i] >= high) { + BinStream[bitnum++] = *invert; + } else if (BinStream[i] <= low) { + BinStream[bitnum++] = *invert ^ 1; + } else if (i-lastBit >= *clk+tol) { + if (bitnum > 0) { + BinStream[bitnum++]=7; + errCnt++; + } + } else { //in tolerance - looking for peak + continue; + } + midBit = 0; + lastBit += *clk; + } else if (i-lastBit >= (*clk/2-tol) && !midBit && !askType){ + if (BinStream[i] >= high) { + BinStream[bitnum++] = *invert; + } else if (BinStream[i] <= low) { + BinStream[bitnum++] = *invert ^ 1; + } else if (i-lastBit >= *clk/2+tol) { + BinStream[bitnum] = BinStream[bitnum-1]; + bitnum++; + } else { //in tolerance - looking for peak + continue; + } + midBit = 1; + } + if (bitnum >= MaxBits) break; } - return i; + *size = bitnum; + return errCnt; } //by marshmellow //take 10 and 01 and manchester decode //run through 2 times and take least errCnt -int manrawdecode(uint8_t * BitStream, size_t *size) +int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert) { uint16_t bitnum=0, MaxBits = 512, errCnt = 0; size_t i, ii; uint16_t bestErr = 1000, bestRun = 0; - if (size == 0) return -1; + if (*size < 16) return -1; + //find correct start position [alignment] for (ii=0;ii<2;++ii){ - i=0; - for (i=i+ii;i<*size-2;i+=2){ - if(BitStream[i]==1 && (BitStream[i+1]==0)){ - } else if((BitStream[i]==0)&& BitStream[i+1]==1){ - } else { + for (i=ii; i<*size-3; i+=2) + if (BitStream[i]==BitStream[i+1]) errCnt++; - } - if(bitnum>MaxBits) break; - } + if (bestErr>errCnt){ bestErr=errCnt; bestRun=ii; } errCnt=0; } - errCnt=bestErr; - if (errCnt<20){ - ii=bestRun; - i=0; - for (i=i+ii; 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; - //errCnt++; - } - if(bitnum>MaxBits) break; + //decode + for (i=bestRun; i < *size-3; i+=2){ + if(BitStream[i] == 1 && (BitStream[i+1] == 0)){ + BitStream[bitnum++]=invert; + } else if((BitStream[i] == 0) && BitStream[i+1] == 1){ + BitStream[bitnum++]=invert^1; + } else { + BitStream[bitnum++]=7; } - *size=bitnum; + if(bitnum>MaxBits) break; } - return errCnt; + *size=bitnum; + return bestErr; +} + +//by marshmellow +//encode binary data into binary manchester +int ManchesterEncode(uint8_t *BitStream, size_t size) +{ + size_t modIdx=20000, i=0; + if (size>modIdx) return -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++){ + BitStream[i] = BitStream[i+20000]; + } + return i; } //by marshmellow @@ -307,9 +304,9 @@ int manrawdecode(uint8_t * BitStream, size_t *size) //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) { - uint16_t bitnum=0; - uint32_t errCnt =0; - size_t i=offset; + uint16_t bitnum = 0; + uint16_t errCnt = 0; + size_t i = offset; uint16_t MaxBits=512; //if not enough samples - error if (*size < 51) return -1; @@ -323,7 +320,7 @@ int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) for (i=offset; i<*size-3; i+=2){ //check for phase error if (BitStream[i+1]==BitStream[i+2]) { - BitStream[bitnum++]=77; + BitStream[bitnum++]=7; errCnt++; } if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){ @@ -331,7 +328,7 @@ int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){ BitStream[bitnum++]=invert; } else { - BitStream[bitnum++]=77; + BitStream[bitnum++]=7; errCnt++; } if(bitnum>MaxBits) break; @@ -340,238 +337,7 @@ int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) return errCnt; } -//by marshmellow -void askAmp(uint8_t *BitStream, size_t size) -{ - int shift = 127; - int shiftedVal=0; - for(int i = 1; i=30) //large jump up - shift=127; - else if(BitStream[i]-BitStream[i-1]<=-20) //large jump down - shift=-127; - - shiftedVal=BitStream[i]+shift; - - if (shiftedVal>255) - shiftedVal=255; - else if (shiftedVal<0) - shiftedVal=0; - BitStream[i-1] = shiftedVal; - } - return; -} - -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) -{ - uint32_t i; - if (*size==0) return -1; - int start = DetectASKClock(BinStream, *size, clk, 20); //clock default - if (*clk==0) return -1; - if (start<0) return -1; - if (*invert != 0 && *invert != 1) *invert =0; - if (amp==1) askAmp(BinStream, *size); - - uint32_t initLoopMax = 200; - if (initLoopMax > *size) initLoopMax=*size; - // Detect high and lows - //25% clip in case highs and lows aren't clipped [marshmellow] - uint8_t clip = 75; - int high, low, ans; - ans = getHiLo(BinStream, initLoopMax, &high, &low, clip, clip); - if (ans<1) return -1; //just noise - - if (DetectCleanAskWave(BinStream, *size, high, low)) { - //PrintAndLog("Clean"); - return cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); - } - - //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low); - int lastBit = 0; //set first clock check - uint32_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=0; //clock tolerance may not be needed anymore currently set to - // + or - 1 but could be increased for poor waves or removed entirely - uint32_t iii = 0; - uint32_t gLen = *size; - if (gLen > 500) gLen=500; - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr) gLen = *clk * 2; - uint8_t errCnt =0; - uint32_t bestStart = *size; - uint32_t bestErrCnt = maxErr; //(*size/1000); - uint8_t midBit=0; - uint16_t MaxBits=1000; - - //PrintAndLog("DEBUG - lastbit - %d",lastBit); - //loop to find first wave that works - for (iii=start; iii < gLen; ++iii){ - if ((BinStream[iii]>=high) || (BinStream[iii]<=low)){ - lastBit=iii-*clk; - errCnt=0; - //loop through to see if this start location works - for (i = iii; i < *size; ++i) { - if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){ - lastBit+=*clk; - midBit=0; - } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - midBit=0; - } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){ - //no mid bar found - midBit=1; - } else { - //mid value found or no bar supposed to be here - - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - //debug - //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){ - //errCnt=0; - 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= high) && ((i-lastBit) > (*clk-tol))){ - lastBit += *clk; - BinStream[bitnum] = *invert; - bitnum++; - midBit=0; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - BinStream[bitnum] = 1 - *invert; - bitnum++; - midBit=0; - } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - BinStream[bitnum] = 1 - *invert; - bitnum++; - } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - BinStream[bitnum] = *invert; - bitnum++; - } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){ - //no mid bar found - midBit=1; - if (bitnum!=0) BinStream[bitnum] = BinStream[bitnum-1]; - bitnum++; - - } else { - //mid value found or no bar supposed to be here - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - - //debug - //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); - if (bitnum > 0){ - BinStream[bitnum]=77; - bitnum++; - } - lastBit+=*clk;//skip over error - } - } - if (bitnum >= MaxBits) break; - } - *size=bitnum; - } else{ - *invert=bestStart; - *clk=iii; - return -1; - } - return bestErrCnt; -} - +// by marshmellow // demod gProxIIDemod // error returns as -x // success returns start position in BitStream @@ -596,8 +362,8 @@ int gProxII_Demod(uint8_t BitStream[], size_t *size) //translate wave to 11111100000 (1 for each short wave 0 for each long wave) size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow) { - uint32_t last_transition = 0; - uint32_t idx = 1; + size_t last_transition = 0; + size_t idx = 1; //uint32_t maxVal=0; if (fchigh==0) fchigh=10; if (fclow==0) fclow=8; @@ -626,82 +392,62 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow if ((idx-last_transition)<(fclow-2)){ //0-5 = garbage noise //do nothing with extra garbage } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves - dest[numBits]=1; + dest[numBits++]=1; } else if ((idx-last_transition) > (fchigh+1) && !numBits) { //12 + and first bit = garbage //do nothing with beginning garbage } else { //9+ = 10 waves - dest[numBits]=0; + dest[numBits++]=0; } last_transition = idx; - numBits++; } } return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0 } -uint32_t myround2(float f) -{ - if (f >= 2000) return 2000;//something bad happened - return (uint32_t) (f + (float)0.5); -} - //translate 11111100000 to 10 -size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxConsequtiveBits, +size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow) { uint8_t lastval=dest[0]; - uint32_t idx=0; + size_t idx=0; size_t numBits=0; uint32_t n=1; - float lowWaves = (((float)(rfLen))/((float)fclow)); - float highWaves = (((float)(rfLen))/((float)fchigh)); for( idx=1; idx < size; idx++) { - - if (dest[idx]==lastval) { - n++; - continue; - } n++; + if (dest[idx]==lastval) continue; + //if lastval was 1, we have a 1->0 crossing if (dest[idx-1]==1) { - if (!numBits && n < (uint8_t)lowWaves) { + if (!numBits && n < rfLen/fclow) { n=0; lastval = dest[idx]; continue; } - n=myround2(((float)n)/lowWaves); + n = (n * fclow + rfLen/2) / rfLen; } else {// 0->1 crossing //test first bitsample too small - if (!numBits && n < (uint8_t)highWaves) { + if (!numBits && n < rfLen/fchigh) { n=0; lastval = dest[idx]; continue; } - n = myround2(((float)n)/highWaves); //-1 for fudge factor + n = (n * fchigh + rfLen/2) / rfLen; } if (n == 0) n = 1; - if(n < maxConsequtiveBits) //Consecutive - { - if(invert==0){ //invert bits - memset(dest+numBits, dest[idx-1] , n); - }else{ - memset(dest+numBits, dest[idx-1]^1 , n); - } - numBits += n; - } + memset(dest+numBits, dest[idx-1]^invert , n); + numBits += n; n=0; lastval=dest[idx]; }//end for - // if valid extra bits at the end were all the same frequency - add them in - if (n > lowWaves && n > highWaves) { + if (n > rfLen/fchigh) { if (dest[idx-2]==1) { - n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow)); + n = (n * fclow + rfLen/2) / rfLen; } else { - n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh)); //-1 for fudge factor + n = (n * fchigh + rfLen/2) / rfLen; } - memset(dest, dest[idx-1]^invert , n); + memset(dest+numBits, dest[idx-1]^invert , n); numBits += n; } return numBits; @@ -712,7 +458,7 @@ int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t { // FSK demodulator size = fsk_wave_demod(dest, size, fchigh, fclow); - size = aggregate_bits(dest, size, rfLen, 192, invert, fchigh, fclow); + size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow); return size; } @@ -724,7 +470,7 @@ int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32 size_t numStart=0, size2=*size, startIdx=0; // FSK demodulator *size = fskdemod(dest, size2,50,1,10,8); //fsk2a - if (*size < 96) return -2; + if (*size < 96*2) return -2; // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1 uint8_t preamble[] = {0,0,0,1,1,1,0,1}; // find bitstring in array @@ -887,12 +633,13 @@ int PyramiddemodFSK(uint8_t *dest, size_t *size) return (int)startIdx; } - -uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, int high, int low) +// by marshmellow +// to detect a wave that has heavily clipped (clean) samples +uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low) { uint16_t allPeaks=1; uint16_t cntPeaks=0; - size_t loopEnd = 572; + size_t loopEnd = 512+60; if (loopEnd > size) loopEnd = size; for (size_t i=60; ilow && dest[i]128) { - if (!high){ - high=1; - if (cnt > highCnt){ - if (highCnt != 0) highCnt2 = highCnt; - highCnt = cnt; - } else if (cnt > highCnt2) { - highCnt2 = cnt; - } - cnt=1; - } else { - cnt++; - } - } else if (dest[idx] <= 128){ - if (high) { - high=0; - if (cnt > highCnt) { - if (highCnt != 0) highCnt2 = highCnt; - highCnt = cnt; - } else if (cnt > highCnt2) { - highCnt2 = cnt; - } - cnt=1; - } else { - cnt++; - } - } + uint8_t fndClk[] = {8,16,32,40,50,64,128}; + size_t startwave; + size_t i = 0; + size_t minClk = 255; + // 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; + + // loop through all samples + while (i < size) { + // measure from low to low + while ((dest[i] > low) && (i < size)) + ++i; + startwave= i; + while ((dest[i] < high) && (i < size)) + ++i; + while ((dest[i] > low) && (i < size)) + ++i; + //get minimum measured distance + if (i-startwave < minClk && i < size) + minClk = i - startwave; } - uint8_t tol; - for (idx=8; idx>0; idx--){ - tol = clk[idx]/8; - if (clk[idx] >= highCnt - tol && clk[idx] <= highCnt + tol) - return clk[idx]; - if (clk[idx] >= highCnt2 - tol && clk[idx] <= highCnt2 + tol) - return clk[idx]; + // set clock + for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) { + if (minClk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && minClk <= fndClk[clkCnt]+1) + return fndClk[clkCnt]; } - return -1; + return 0; } // by marshmellow @@ -960,86 +696,103 @@ 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) { - int i=0; - int clk[]={8,16,32,40,50,64,100,128,256}; - int loopCnt = 256; //don't need to loop through entire array... - if (size == 0) return -1; - if (size0; i--){ - if (clk[i] == ans) { - *clock=ans; - return 0; + if (!clockFnd){ + if (DetectCleanAskWave(dest, size, peak, low)==1){ + int ans = DetectStrongAskClock(dest, size, peak, low); + for (i=clkEnd-1; i>0; i--){ + if (clk[i] == ans) { + *clock = ans; + //clockFnd = i; + return 0; // for strong waves i don't use the 'best start position' yet... + //break; //clock found but continue to find best startpos [not yet] + } } } } - int ii; - int clkCnt; - int tol = 0; - int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000}; - int bestStart[]={0,0,0,0,0,0,0,0,0}; - int errCnt=0; + + uint8_t ii; + uint8_t clkCnt, tol = 0; + uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000}; + uint8_t bestStart[]={0,0,0,0,0,0,0,0,0}; + size_t errCnt = 0; + size_t arrLoc, loopEnd; + + if (clockFnd>0) { + clkCnt = clockFnd; + clkEnd = clockFnd+1; + } + else clkCnt=1; + //test each valid clock from smallest to greatest to see which lines up - for(clkCnt=0; clkCnt < 8; clkCnt++){ - if (clk[clkCnt] == 32){ + for(; clkCnt < clkEnd; clkCnt++){ + if (clk[clkCnt] <= 32){ tol=1; }else{ tol=0; } - if (!maxErr) loopCnt=clk[clkCnt]*2; + //if no errors allowed - keep start within the first clock + if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt]<128) loopCnt=clk[clkCnt]*2; bestErr[clkCnt]=1000; - //try lining up the peaks by moving starting point (try first 256) + //try lining up the peaks by moving starting point (try first few clocks) for (ii=0; ii < loopCnt; ii++){ - if ((dest[ii] >= peak) || (dest[ii] <= low)){ - errCnt=0; - // now that we have the first one lined up test rest of wave array - for (i=0; i<((int)((size-ii-tol)/clk[clkCnt])-1); ++i){ - if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){ - }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){ - }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){ - }else{ //error no peak detected - errCnt++; - } - } - //if we found no errors then we can stop here - // this is correct one - return this clock - //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i); - if(errCnt==0 && clkCnt<6) { - *clock = clk[clkCnt]; - return ii; - } - //if we found errors see if it is lowest so far and save it as best run - if(errCnt low) continue; + + errCnt=0; + // now that we have the first one lined up test rest of wave array + loopEnd = ((size-ii-tol) / clk[clkCnt]) - 1; + for (i=0; i < loopEnd; ++i){ + arrLoc = ii + (i * clk[clkCnt]); + if (dest[arrLoc] >= peak || dest[arrLoc] <= low){ + }else if (dest[arrLoc-tol] >= peak || dest[arrLoc-tol] <= low){ + }else if (dest[arrLoc+tol] >= peak || dest[arrLoc+tol] <= low){ + }else{ //error no peak detected + errCnt++; } } + //if we found no errors then we can stop here and a low clock (common clocks) + // this is correct one - return this clock + //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i); + if(errCnt==0 && clkCnt<7) { + if (!clockFnd) *clock = clk[clkCnt]; + return ii; + } + //if we found errors see if it is lowest so far and save it as best run + if(errCntmaxErr) return -1; - *clock=clk[best]; + //if (bestErr[best] > maxErr) return -1; + if (!clockFnd) *clock = clk[best]; return bestStart[best]; } @@ -1063,7 +816,8 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) uint16_t peakcnt=0, errCnt=0, waveLenCnt=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}; - countFC(dest, size, &fc); + fc = countFC(dest, size, 0); + if (fc!=2 && fc!=4 && fc!=8) return -1; //PrintAndLog("DEBUG: FC: %d",fc); //find first full wave @@ -1144,9 +898,9 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) //detect nrz clock by reading #peaks vs no peaks(or errors) int DetectNRZClock(uint8_t dest[], size_t size, int clock) { - int i=0; - int clk[]={8,16,32,40,50,64,100,128,256}; - int 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... if (size == 0) return 0; if (size= peak || dest[i] <= low){ @@ -1199,7 +953,7 @@ int DetectNRZClock(uint8_t dest[], size_t size, int clock) } } int iii=7; - int best=0; + uint8_t best=0; for (iii=7; iii > 0; iii--){ if (peaksdet[iii] > peaksdet[best]){ best = iii; @@ -1217,7 +971,7 @@ void psk1TOpsk2(uint8_t *BitStream, size_t size) size_t i=1; uint8_t lastBit=BitStream[0]; for (; i*size) gLen = *size; int high, low; if (getHiLo(dest, gLen, &high, &low, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low int lastBit = 0; //set first clock check - uint32_t bitnum = 0; //output counter + size_t iii = 0, bitnum = 0; //bitnum counter + uint16_t errCnt = 0, MaxBits = 1000; + size_t bestErrCnt = maxErr+1; + size_t bestPeakCnt = 0, bestPeakStart = 0; + uint8_t bestFirstPeakHigh=0, firstPeakHigh=0, curBit=0, bitHigh=0, errBitHigh=0; uint8_t tol = 1; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave - uint32_t iii = 0; - uint16_t errCnt =0; - uint16_t MaxBits = 1000; - uint32_t bestErrCnt = maxErr+1; - uint32_t bestPeakCnt = 0; - uint32_t bestPeakStart=0; - uint8_t bestFirstPeakHigh=0; - uint8_t firstPeakHigh=0; - uint8_t curBit=0; - uint8_t bitHigh=0; - uint8_t errBitHigh=0; uint16_t peakCnt=0; uint8_t ignoreWindow=4; - uint8_t ignoreCnt=ignoreWindow; //in case of noice near peak + uint8_t ignoreCnt=ignoreWindow; //in case of noise near peak //loop to find first wave that works - align to clock for (iii=0; iii < gLen; ++iii){ if ((dest[iii]>=high) || (dest[iii]<=low)){ @@ -1346,135 +1092,93 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr) lastBit=iii-*clk; peakCnt=0; errCnt=0; - bitnum=0; //loop through to see if this start location works for (i = iii; i < *size; ++i) { - //if we found a high bar and we are at a clock bit - if ((dest[i]>=high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - bitnum++; - peakCnt++; - errBitHigh=0; - ignoreCnt=ignoreWindow; - //else if low bar found and we are at a clock point - }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - bitnum++; - peakCnt++; - errBitHigh=0; - ignoreCnt=ignoreWindow; + // if we are at a clock bit + if ((i >= lastBit + *clk - tol) && (i <= lastBit + *clk + tol)) { + //test high/low + if (dest[i] >= high || dest[i] <= low) { + bitHigh = 1; + peakCnt++; + errBitHigh = 0; + ignoreCnt = ignoreWindow; + lastBit += *clk; + } else if (i == lastBit + *clk + tol) { + lastBit += *clk; + } //else if no bars found - }else if(dest[i] < high && dest[i] > low) { + } else if (dest[i] < high && dest[i] > low){ if (ignoreCnt==0){ bitHigh=0; - if (errBitHigh==1){ - errCnt++; - } + if (errBitHigh==1) errCnt++; errBitHigh=0; } else { ignoreCnt--; } - //if we are past a clock point - if (i >= lastBit+*clk+tol){ //clock val - lastBit+=*clk; - bitnum++; - } - //else if bar found but we are not at a clock bit and we did not just have a clock bit - }else if ((dest[i]>=high || dest[i]<=low) && (ilastBit+*clk+tol) && (bitHigh==0)){ + } else if ((dest[i]>=high || dest[i]<=low) && (bitHigh==0)) { //error bar found no clock... errBitHigh=1; } - if (bitnum>=MaxBits) break; + if (((i-iii) / *clk)>=MaxBits) break; } //we got more than 64 good bits and not all errors - if (bitnum > (64) && (errCnt <= (maxErr))) { + if (((i-iii) / *clk) > 64 && (errCnt <= (maxErr))) { //possible good read - if (errCnt == 0){ - //bestStart = iii; + if (!errCnt || peakCnt > bestPeakCnt){ bestFirstPeakHigh=firstPeakHigh; bestErrCnt = errCnt; bestPeakCnt = peakCnt; bestPeakStart = iii; - break; //great read - finish + if (!errCnt) break; //great read - finish } - if (errCnt < bestErrCnt){ //set this as new best run - bestErrCnt = errCnt; - //bestStart = iii; - } - if (peakCnt > bestPeakCnt){ - bestFirstPeakHigh=firstPeakHigh; - bestPeakCnt=peakCnt; - bestPeakStart=iii; - } } } } //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart); - if (bestErrCnt <= maxErr){ - //best run is good enough set to best run and set overwrite BinStream - iii=bestPeakStart; - lastBit=bestPeakStart-*clk; - bitnum=0; - memset(dest, bestFirstPeakHigh^1, bestPeakStart / *clk); - bitnum += (bestPeakStart / *clk); - for (i = iii; i < *size; ++i) { - //if we found a high bar and we are at a clock bit - if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - curBit=1-*invert; - dest[bitnum]=curBit; - bitnum++; - errBitHigh=0; - ignoreCnt=ignoreWindow; - //else if low bar found and we are at a clock point - }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - curBit=*invert; - dest[bitnum]=curBit; - bitnum++; - errBitHigh=0; - ignoreCnt=ignoreWindow; - //else if no bars found - }else if(dest[i]low) { - if (ignoreCnt==0){ - bitHigh=0; - //if peak is done was it an error peak? - if (errBitHigh==1){ - dest[bitnum]=77; - bitnum++; - errCnt++; - } - errBitHigh=0; - } else { - ignoreCnt--; - } - //if we are past a clock point - if (i>=lastBit+*clk+tol){ //clock val - lastBit+=*clk; - dest[bitnum]=curBit; - bitnum++; + if (bestErrCnt > maxErr) return bestErrCnt; + + //best run is good enough set to best run and set overwrite BinStream + lastBit = bestPeakStart - *clk; + memset(dest, bestFirstPeakHigh^1, bestPeakStart / *clk); + bitnum += (bestPeakStart / *clk); + for (i = bestPeakStart; i < *size; ++i) { + // if expecting a clock bit + if ((i >= lastBit + *clk - tol) && (i <= lastBit + *clk + tol)) { + // test high/low + if (dest[i] >= high || dest[i] <= low) { + peakCnt++; + bitHigh = 1; + errBitHigh = 0; + ignoreCnt = ignoreWindow; + curBit = *invert; + if (dest[i] >= high) curBit ^= 1; + dest[bitnum++] = curBit; + lastBit += *clk; + //else no bars found in clock area + } else if (i == lastBit + *clk + tol) { + dest[bitnum++] = curBit; + lastBit += *clk; + } + //else if no bars found + } else if (dest[i] < high && dest[i] > low){ + if (ignoreCnt == 0){ + bitHigh = 0; + if (errBitHigh == 1){ + dest[bitnum++] = 7; + errCnt++; } - //else if bar found but we are not at a clock bit and we did not just have a clock bit - }else if ((dest[i]>=high || dest[i]<=low) && ((ilastBit+*clk+tol)) && (bitHigh==0)){ - //error bar found no clock... - errBitHigh=1; + errBitHigh=0; + } else { + ignoreCnt--; } - if (bitnum >= MaxBits) break; + } else if ((dest[i] >= high || dest[i] <= low) && (bitHigh == 0)) { + //error bar found no clock... + errBitHigh=1; } - *size=bitnum; - } else{ - *size=bitnum; - return bestErrCnt; + if (bitnum >= MaxBits) break; } - - if (bitnum>16){ - *size=bitnum; - } else return -5; - return errCnt; + *size = bitnum; + return bestErrCnt; } //by marshmellow @@ -1485,8 +1189,8 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; uint8_t rfLensFnd = 0; - uint8_t lastFCcnt=0; - uint32_t fcCounter = 0; + uint8_t lastFCcnt = 0; + uint16_t fcCounter = 0; uint16_t rfCounter = 0; uint8_t firstBitFnd = 0; size_t i; @@ -1504,45 +1208,41 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc break; for (; i < size-1; i++){ - if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1]){ - // new peak - fcCounter++; - rfCounter++; - // if we got less than the small fc + tolerance then set it to the small fc - if (fcCounter < fcLow+fcTol) - fcCounter = fcLow; - else //set it to the large fc - fcCounter = fcHigh; - - //look for bit clock (rf/xx) - if ((fcCounterlastFCcnt)){ - //not the same size as the last wave - start of new bit sequence - - if (firstBitFnd>1){ //skip first wave change - probably not a complete bit - for (int ii=0; ii<15; ii++){ - if (rfLens[ii]==rfCounter){ - rfCnts[ii]++; - rfCounter=0; - break; - } + fcCounter++; + rfCounter++; + + if (BitStream[i] <= BitStream[i-1] || BitStream[i] < BitStream[i+1]) + continue; + // else new peak + // if we got less than the small fc + tolerance then set it to the small fc + if (fcCounter < fcLow+fcTol) + fcCounter = fcLow; + else //set it to the large fc + fcCounter = fcHigh; + + //look for bit clock (rf/xx) + if ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)){ + //not the same size as the last wave - start of new bit sequence + if (firstBitFnd > 1){ //skip first wave change - probably not a complete bit + for (int ii=0; ii<15; ii++){ + if (rfLens[ii] == rfCounter){ + rfCnts[ii]++; + rfCounter = 0; + break; } - if (rfCounter>0 && rfLensFnd<15){ - //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); - rfCnts[rfLensFnd]++; - rfLens[rfLensFnd++]=rfCounter; - } - } else { - firstBitFnd++; } - rfCounter=0; - lastFCcnt=fcCounter; + if (rfCounter > 0 && rfLensFnd < 15){ + //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); + rfCnts[rfLensFnd]++; + rfLens[rfLensFnd++] = rfCounter; + } + } else { + firstBitFnd++; } - fcCounter=0; - } else { - // count sample - fcCounter++; - rfCounter++; + rfCounter=0; + lastFCcnt=fcCounter; } + fcCounter=0; } uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15; @@ -1588,13 +1288,13 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc //countFC is to detect the field clock lengths. //counts and returns the 2 most common wave lengths //mainly used for FSK field clock detection -uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC) +uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) { uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0}; uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0}; uint8_t fcLensFnd = 0; uint8_t lastFCcnt=0; - uint32_t fcCounter = 0; + uint8_t fcCounter = 0; size_t i; if (size == 0) return 0; @@ -1607,13 +1307,14 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC) if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){ // new up transition fcCounter++; - - //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) - if (lastFCcnt==5 && fcCounter==9) fcCounter--; - //if odd and not rc/5 add one (for when we get a fc 9 instead of 10) - if ((fcCounter==9 && fcCounter & 1) || fcCounter==4) fcCounter++; - + if (fskAdj){ + //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) + if (lastFCcnt==5 && fcCounter==9) fcCounter--; + //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5) + if ((fcCounter==9) || fcCounter==4) fcCounter++; // save last field clock count (fc/xx) + lastFCcnt = fcCounter; + } // find which fcLens to save it to: for (int ii=0; ii<10; ii++){ if (fcLens[ii]==fcCounter){ @@ -1661,89 +1362,31 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC) fcL=fcLens[best1]; } - *mostFC=fcLens[best1]; // TODO: take top 3 answers and compare to known Field clocks to get top 2 uint16_t fcs = (((uint16_t)fcH)<<8) | fcL; // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]); - - return fcs; -} - -//by marshmellow -//countPSK_FC is to detect the psk carrier clock length. -//counts and returns the 1 most common wave length -uint8_t countPSK_FC(uint8_t *BitStream, size_t size) -{ - uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0}; - uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0}; - uint8_t fcLensFnd = 0; - uint32_t fcCounter = 0; - size_t i; - if (size == 0) return 0; - - // prime i to first up transition - for (i = 1; i < size-1; i++) - if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]) - break; - - for (; i < size-1; i++){ - if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){ - // new up transition - fcCounter++; - - // save last field clock count (fc/xx) - // find which fcLens to save it to: - for (int ii=0; ii<10; ii++){ - if (fcLens[ii]==fcCounter){ - fcCnts[ii]++; - fcCounter=0; - break; - } - } - if (fcCounter>0 && fcLensFnd<10){ - //add new fc length - fcCnts[fcLensFnd]++; - fcLens[fcLensFnd++]=fcCounter; - } - fcCounter=0; - } else { - // count sample - fcCounter++; - } - } - - uint8_t best1=9; - uint16_t maxCnt1=0; - // go through fclens and find which ones are bigest - for (i=0; i<10; i++){ - //PrintAndLog("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]); - // get the best FC value - if (fcCnts[i]>maxCnt1) { - maxCnt1=fcCnts[i]; - best1=i; - } - } - return fcLens[best1]; + if (fskAdj) return fcs; + return fcLens[best1]; } //by marshmellow - demodulate PSK1 wave //uses wave lengths (# Samples) int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) { - uint16_t loopCnt = 4096; //don't need to loop through entire array... if (size == 0) return -1; + uint16_t loopCnt = 4096; //don't need to loop through entire array... if (*size graph 0 then it is an up wave or a 1 - if (lastAvgWaveVal > 123) curPhase^=1; //fudge graph 0 a little 123 vs 128 + if (lastAvgWaveVal > 123) curPhase ^= 1; //fudge graph 0 a little 123 vs 128 break; } waveStart = i+1; avgWaveVal = 0; } - avgWaveVal+=dest[i+2]; + avgWaveVal += dest[i+2]; } //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); lastClkBit = firstFullWave; //set start of wave as clock align //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit); waveStart = 0; - errCnt=0; size_t numBits=0; //set skipped bits - memset(dest,curPhase^1,firstFullWave / *clock); + memset(dest, curPhase^1, firstFullWave / *clock); numBits += (firstFullWave / *clock); 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 if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){ if (waveStart == 0) { waveStart = i+1; - waveLenCnt=0; + waveLenCnt = 0; avgWaveVal = dest[i+1]; } else { //waveEnd waveEnd = i+1; @@ -1787,27 +1429,27 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) lastAvgWaveVal = avgWaveVal/waveLenCnt; if (waveLenCnt > fc){ //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal); - //if this wave is a phase shift + //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 - curPhase^=1; + curPhase ^= 1; dest[numBits++] = curPhase; lastClkBit += *clock; - } else if (i lastClkBit + *clock + tol + fc){ lastClkBit += *clock; //no phase shift but clock bit dest[numBits++] = curPhase; } - avgWaveVal=0; - waveStart=i+1; + avgWaveVal = 0; + waveStart = i+1; } } - avgWaveVal+=dest[i+1]; + avgWaveVal += dest[i+1]; } *size = numBits; return errCnt;