X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/23f0a7d862fb41d21dd90403cb657e91705bf886..1c8fbeb93e82f6aafb885b57b9afbfadcf85c171:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index df30aba4..0f8dd452 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -81,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}; @@ -114,12 +112,12 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ return 0; } -// demodulates strong heavily clipped samples +//by marshmellow +//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++; @@ -130,7 +128,7 @@ int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int if (smplCnt > clk-(clk/4)-1) { //full clock if (smplCnt > clk + (clk/4)+1) { //too many samples errCnt++; - BinStream[bitCnt++]=77; + BinStream[bitCnt++]=7; } else if (waveHigh) { BinStream[bitCnt++] = invert; BinStream[bitCnt++] = invert; @@ -166,131 +164,100 @@ int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int } //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) +void askAmp(uint8_t *BitStream, size_t size) { - size_t i; - int start = DetectASKClock(BinStream, *size, clk, 20); //clock default + 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; +} + +//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) +{ + 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 (*invert != 1) *invert = 0; + if (amp==1) askAmp(BinStream, *size); + uint8_t initLoopMax = 255; if (initLoopMax > *size) initLoopMax = *size; // Detect high and lows - // 25% fuzz in case highs and lows aren't clipped [marshmellow] + //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 + 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)) { - cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); - return manrawdecode(BinStream, size); + errCnt = cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); + if (askType) //askman + return manrawdecode(BinStream, size, 0); + else //askraw + return errCnt; } - - // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low); - int lastBit = 0; //set first clock check - uint16_t bitnum = 0; //output counter + 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 iii = 0; - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr && initLoopMax > *clk*3) initLoopMax = *clk * 3; - 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-tol-*clk; ++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) > (32)) && (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 - } + 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; } - 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) { - if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){ - //high found and we are expecting a bar - lastBit += *clk; - BinStream[bitnum++] = *invert; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar + midBit = 0; lastBit += *clk; - BinStream[bitnum++] = *invert ^ 1; - } else if ((i-lastBit)>(*clk+tol)){ - //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); - if (bitnum > 0) { - BinStream[bitnum++] = 77; - errCnt++; - } - lastBit += *clk;//skip over error + } 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; } *size = bitnum; - return bestErrCnt; -} - -//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; + 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){ - for (i=ii; i<*size-2; i+=2) + for (i=ii; i<*size-3; i+=2) if (BitStream[i]==BitStream[i+1]) errCnt++; @@ -300,13 +267,14 @@ int manrawdecode(uint8_t * BitStream, size_t *size) } errCnt=0; } - for (i=bestRun; i < *size-2; i+=2){ + //decode + for (i=bestRun; i < *size-3; i+=2){ if(BitStream[i] == 1 && (BitStream[i+1] == 0)){ - BitStream[bitnum++]=0; + BitStream[bitnum++]=invert; } else if((BitStream[i] == 0) && BitStream[i+1] == 1){ - BitStream[bitnum++]=1; + BitStream[bitnum++]=invert^1; } else { - BitStream[bitnum++]=77; + BitStream[bitnum++]=7; } if(bitnum>MaxBits) break; } @@ -314,6 +282,22 @@ int manrawdecode(uint8_t * BitStream, size_t *size) 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 //take 01 or 10 = 1 and 11 or 00 = 0 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010 @@ -336,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)){ @@ -344,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; @@ -353,143 +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(size_t 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; -} - -//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 - 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; - // 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 -1; //just noise - - // if clean clipped waves detected run alternate demod - 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 - uint8_t midBit = 0; - size_t bestStart = start, bestErrCnt = 0; //(*size/1000); - size_t MaxBits = 1024; - - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr && initLoopMax > *clk*3) initLoopMax = *clk * 3; - //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 - *clk; ++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 32 good bits and not all errors - if ((((i-iii)/ *clk) > 32) && (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) { - if (i - lastBit > *clk){ - if (BinStream[i] >= high) { - BinStream[bitnum++] = *invert; - } else if (BinStream[i] <= low) { - BinStream[bitnum++] = *invert ^ 1; - } else { - if (bitnum > 0) { - BinStream[bitnum++]=77; - errCnt++; - } - } - midBit = 0; - lastBit += *clk; - } else if (i-lastBit > (*clk/2) && midBit == 0){ - if (BinStream[i] >= high) { - BinStream[bitnum++] = *invert; - } else if (BinStream[i] <= low) { - BinStream[bitnum++] = *invert ^ 1; - } else { - - BinStream[bitnum] = BinStream[bitnum-1]; - bitnum++; - } - midBit = 1; - } - if (bitnum >= MaxBits) break; - } - *size = bitnum; - return errCnt; -} - +// by marshmellow // demod gProxIIDemod // error returns as -x // success returns start position in BitStream @@ -521,7 +369,9 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow 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; // sync to first lo-hi transition, and threshold // Need to threshold first sample @@ -541,13 +391,22 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow // Check for 0->1 transition if (dest[idx-1] < dest[idx]) { // 0 -> 1 transition - if ((idx-last_transition)<(fclow-2)){ //0-5 = garbage noise + preLastSample = LastSample; + LastSample = currSample; + currSample = idx-last_transition; + if (currSample < (fclow-2)){ //0-5 = garbage noise //do nothing with extra garbage - } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves + } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves + if (LastSample > (fchigh-2) && preLastSample < (fchigh-1)){ + dest[numBits-1]=1; //correct last 9 wave surrounded by 8 waves + } dest[numBits++]=1; - } else if ((idx-last_transition) > (fchigh+1) && !numBits) { //12 + and first bit = garbage + + } else if (currSample > (fchigh+1) && !numBits) { //12 + and first bit = garbage //do nothing with beginning garbage - } else { //9+ = 10 waves + } else if (currSample == (fclow+1) && LastSample == (fclow-1)) { // had a 7 then a 9 should be two 8's + dest[numBits++]=1; + } else { //9+ = 10 sample waves dest[numBits++]=0; } last_transition = idx; @@ -689,6 +548,17 @@ uint32_t bytebits_to_byte(uint8_t* src, size_t numbits) return num; } +//least significant bit first +uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits) +{ + uint32_t num = 0; + for(int i = 0 ; i < numbits ; i++) + { + num = (num << 1) | *(src + (numbits-(i+1))); + } + return num; +} + int IOdemodFSK(uint8_t *dest, size_t size) { if (justNoise(dest, size)) return -1; @@ -721,7 +591,7 @@ int IOdemodFSK(uint8_t *dest, size_t size) // 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), and binary Length (length to run) +// Parity Type (1 for odd; 0 for even; 2 Always 1'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; @@ -731,9 +601,13 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p parityWd = (parityWd << 1) | BitStream[startIdx+word+bit]; BitStream[j++] = (BitStream[startIdx+word+bit]); } - j--; + j--; // overwrite parity with next data // if parity fails then return 0 - if (parityTest(parityWd, pLen, pType) == 0) return -1; + if (pType == 2) { // then marker bit which should be a 1 + if (!BitStream[j]) return 0; + } else { + if (parityTest(parityWd, pLen, pType) == 0) return 0; + } bitCnt+=(pLen-1); parityWd = 0; } @@ -742,6 +616,21 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p return bitCnt; } +// Ask/Biphase Demod then try to locate an ISO 11784/85 ID +// BitStream must contain previously askrawdemod and biphasedemoded data +int FDXBdemodBI(uint8_t *dest, size_t *size) +{ + //make sure buffer has enough data + if (*size < 128) return -1; + + size_t startIdx = 0; + uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,1}; + + uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); + if (errChk == 0) return -2; //preamble not found + return (int)startIdx; +} + // by marshmellow // FSK Demod then try to locate an AWID ID int AWIDdemodFSK(uint8_t *dest, size_t *size) @@ -785,12 +674,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 @@ -861,47 +737,62 @@ 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 clkEnd = 9; uint8_t loopCnt = 255; //don't need to loop through entire array... - if (size==0) return -1; - if (size <= loopCnt) loopCnt = size-1; //not enough samples - //if we already have a valid clock quit - - for (;i<8;++i) - if (clk[i] == *clock) return 0; + if (size <= loopCnt) return -1; //not enough samples + + //if we already have a valid clock + uint8_t clockFnd=0; + for (;i0; 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] + } } } } + 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) 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) - for (ii=0; ii < loopCnt-tol-clk[clkCnt]; ii++){ + //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) continue; errCnt=0; @@ -916,11 +807,11 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) errCnt++; } } - //if we found no errors then we can stop here + //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<6) { - *clock = clk[clkCnt]; + 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 @@ -930,9 +821,9 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) } } } - uint8_t iii=0; + uint8_t iii; uint8_t best=0; - for (iii=0; iii<8; ++iii){ + for (iii=1; iii maxErr) return -1; - *clock = clk[best]; + //if (bestErr[best] > maxErr) return -1; + if (!clockFnd) *clock = clk[best]; return bestStart[best]; } @@ -1121,7 +1012,7 @@ void psk1TOpsk2(uint8_t *BitStream, size_t size) size_t i=1; uint8_t lastBit=BitStream[0]; for (; i lastClkBit + *clock + tol + fc){ lastClkBit += *clock; //no phase shift but clock bit @@ -1604,3 +1495,30 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) *size = numBits; return errCnt; } +// on successful return 1 otherwise return 0 +int VikingDecode(uint8_t *BitStream, + size_t size, + size_t *startIdx, + uint8_t *id_bits, + size_t id_bits_size) +{ + //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future + // otherwise could be a void with no arguments + //set defaults + uint32_t i = 0; + uint32_t lastcheckindex = size - (id_bits_size * 2); + int found = 0; + while (i < lastcheckindex) + { + if (memcmp(BitStream + i,id_bits,id_bits_size) == 0) + { + *startIdx = i; + found = 1; + break; + } + i++; + } + return found; +} + +