X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/2767fc02919545bd65082b4682b2331def9a5ad5..415274a7c3253b71b582c2f563bb54080c2790be:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index 58221546..5d19c897 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -112,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++; @@ -164,52 +164,81 @@ 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; + 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; //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 - uint16_t errCnt = 0, MaxBits = 512; + 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 ((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 + 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; - 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++] = 7; - 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; } @@ -217,34 +246,18 @@ int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max return errCnt; } -//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 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++; @@ -255,11 +268,11 @@ int manrawdecode(uint8_t * BitStream, size_t *size) errCnt=0; } //decode - for (i=bestRun; i < *size-2; i+=2){ + 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++]=7; } @@ -269,6 +282,32 @@ int manrawdecode(uint8_t * BitStream, size_t *size) return bestErr; } +uint32_t manchesterEncode2Bytes(uint16_t datain) { + uint32_t output = 0; + uint8_t curBit = 0; + for (uint8_t i=0; i<16; i++) { + curBit = (datain >> (15-i) & 1); + output |= (1<<(((15-i)*2)+curBit)); + } + return output; +} + +//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 @@ -308,89 +347,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, 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; - // 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; //set first clock check - can go negative - size_t i, errCnt = 0, bitnum = 0; //output counter - uint8_t midBit = 0; - size_t MaxBits = 1024; - lastBit = start - *clk; - - for (i = start; 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++]=7; - 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 @@ -422,7 +379,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 @@ -442,13 +401,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; @@ -579,7 +547,7 @@ int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, ui return (int)startIdx; } -uint32_t bytebits_to_byte(uint8_t* src, size_t numbits) +uint32_t bytebits_to_byte(uint8_t *src, size_t numbits) { uint32_t num = 0; for(int i = 0 ; i < numbits ; i++) @@ -590,6 +558,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; @@ -618,11 +597,28 @@ int IOdemodFSK(uint8_t *dest, size_t size) return (int) startIdx; } return -5; +} + +// by marshmellow +// find viking preamble 0xF200 in already demoded data +int VikingDemod_AM(uint8_t *dest, size_t *size) { + if (justNoise(dest, *size)) return -1; + //make sure buffer has data + if (*size < 64*2) return -2; + + size_t startIdx = 0; + uint8_t preamble[] = {1,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); + if (errChk == 0) return -4; //preamble not found + + if (*size != 64) return -5; + //return start position + return (int) startIdx; } // 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; @@ -632,9 +628,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; } @@ -643,6 +643,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) @@ -686,12 +701,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 @@ -763,15 +765,15 @@ int DetectStrongAskClock(uint8_t dest[], size_t size) int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) { size_t i=1; - uint8_t clk[]={255,8,16,32,40,50,64,100,128,255}; + 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 (size <= loopCnt) return -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; + for (;i1; 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] + 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; - //test each valid clock from smallest to greatest to see which lines up - uint8_t clkEnd=9; - if (clockFnd>0) clkEnd=clockFnd+1; - else clockFnd=1; - for(clkCnt=clockFnd; clkCnt < clkEnd; clkCnt++){ - if (clk[clkCnt] == 32){ + 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 < clkEnd; clkCnt++){ + if (clk[clkCnt] <= 32){ tol=1; }else{ tol=0; } //if no errors allowed - keep start within the first clock - if (!maxErr && size > clk[clkCnt]*3 + tol) loopCnt=clk[clkCnt]*2; + 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 few clocks) - for (ii=0; ii < loopCnt-clk[clkCnt]; ii++){ + for (ii=0; ii < loopCnt; ii++){ if (dest[ii] < peak && dest[ii] > low) continue; errCnt=0; @@ -826,11 +834,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 @@ -840,9 +848,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 (!clockFnd) *clock = clk[best]; return bestStart[best]; }