X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/be2cf126bf74c3e0c60706dd2620c8a6d742e396..2147c307785639944f4843d7cf0d245778532ecf:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index 448195f2..46ac4924 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -25,7 +25,7 @@ uint8_t justNoise(uint8_t *BitStream, size_t size) } //by marshmellow -//get high and low with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise +//get high and low values of a wave with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo) { *high=0; @@ -50,12 +50,12 @@ uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) for (uint8_t i = 0; i < bitLen; i++){ ans ^= ((bits >> i) & 1); } - //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType); + //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType); return (ans == pType); } //by marshmellow -//search for given preamble in given BitStream and return startIndex and length +//search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx) { uint8_t foundCnt=0; @@ -78,7 +78,7 @@ uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_ //by marshmellow //takes 1s and 0s and searches for EM410x format - output EM ID -uint64_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx) +uint64_t Em410xDecodeOld(uint8_t *BitStream, size_t *size, size_t *startIdx) { //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 @@ -108,6 +108,7 @@ uint64_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx) errChk = 0; break; } + //set uint64 with ID from BitStream for (uint8_t ii=0; ii<4; ii++){ lo = (lo << 1LL) | (BitStream[(i*5)+ii+idx]); } @@ -119,6 +120,52 @@ uint64_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx) return 0; } +//by marshmellow +//takes 1s and 0s and searches for EM410x format - output EM ID +uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo) +{ + //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; + if (BitStream[1]>1){ //allow only 1s and 0s + // PrintAndLog("no data found"); + return 0; + } + // 111111111 bit pattern represent start of frame + uint8_t preamble[] = {1,1,1,1,1,1,1,1,1}; + uint32_t idx = 0; + uint32_t parityBits = 0; + uint8_t errChk = 0; + uint8_t FmtLen = 10; + *startIdx = 0; + for (uint8_t extraBitChk=0; extraBitChk<5; extraBitChk++){ + errChk = preambleSearch(BitStream+extraBitChk+*startIdx, preamble, sizeof(preamble), size, startIdx); + if (errChk == 0) return 0; + if (*size>64) FmtLen = 22; + idx = *startIdx + 9; + for (i=0; i> 63); + *lo = (*lo << 1) | (BitStream[(i*5)+ii+idx]); + } + } + if (errChk != 0) return 1; + //skip last 5 bit parity test for simplicity. + // *size = 64 | 128; + } + return 0; +} + //by marshmellow //takes 3 arguments - clock, invert, maxErr as integers //attempts to demodulate ask while decoding manchester @@ -150,6 +197,8 @@ int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max 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; @@ -244,14 +293,14 @@ 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; + 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 @@ -304,16 +353,29 @@ int manrawdecode(uint8_t * BitStream, size_t *size) } //by marshmellow -//take 01 or 10 = 0 and 11 or 00 = 1 +//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 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) { uint16_t bitnum=0; uint32_t errCnt =0; - uint32_t i; - uint16_t MaxBits=500; - i=offset; - if (size == 0) return -1; - for (;i<*size-2; i+=2){ + size_t i=offset; + uint16_t MaxBits=512; + //if not enough samples - error + if (*size < 51) return -1; + //check for phase change faults - skip one sample if faulty + uint8_t offsetA = 1, offsetB = 1; + for (; i<48; i+=2){ + if (BitStream[i+1]==BitStream[i+2]) offsetA=0; + if (BitStream[i+2]==BitStream[i+3]) offsetB=0; + } + if (!offsetA && offsetB) offset++; + for (i=offset; i<*size-3; i+=2){ + //check for phase error + if (i<*size-3 && BitStream[i+1]==BitStream[i+2]) { + BitStream[bitnum++]=77; + errCnt++; + } if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){ BitStream[bitnum++]=1^invert; } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){ @@ -331,29 +393,28 @@ int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) //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 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; } //by marshmellow //takes 3 arguments - clock, invert and maxErr as integers //attempts to demodulate ask only -//prints binary found and saves in graphbuffer for further commands int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp) { uint32_t i; @@ -381,6 +442,8 @@ int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max 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); @@ -500,6 +563,28 @@ int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int max } return bestErrCnt; } + +// demod gProxIIDemod +// error returns as -x +// success returns start position in BitStream +// BitStream must contain previously askrawdemod and biphasedemoded data +int gProxII_Demod(uint8_t BitStream[], size_t *size) +{ + size_t startIdx=0; + uint8_t preamble[] = {1,1,1,1,1,0}; + + uint8_t errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, &startIdx); + if (errChk == 0) return -3; //preamble not found + if (*size != 96) return -2; //should have found 96 bits + //check first 6 spacer bits to verify format + if (!BitStream[startIdx+5] && !BitStream[startIdx+10] && !BitStream[startIdx+15] && !BitStream[startIdx+20] && !BitStream[startIdx+25] && !BitStream[startIdx+30]){ + //confirmed proper separator bits found + //return start position + return (int) startIdx; + } + return -5; +} + //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) { @@ -712,7 +797,7 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p for (int word = 0; word < (bLen); word+=pLen){ for (int bit=0; bit < pLen; bit++){ parityWd = (parityWd << 1) | BitStream[startIdx+word+bit]; - BitStream[j++] = (BitStream[startIdx+word+bit]); + BitStream[j++] = (BitStream[startIdx+word+bit]); } j--; // if parity fails then return 0 @@ -750,17 +835,17 @@ int AWIDdemodFSK(uint8_t *dest, size_t *size) // FSK Demod then try to locate an Farpointe Data (pyramid) ID int PyramiddemodFSK(uint8_t *dest, size_t *size) { - //make sure buffer has data - if (*size < 128*50) return -5; + //make sure buffer has data + if (*size < 128*50) return -5; - //test samples are not just noise - if (justNoise(dest, *size)) return -1; + //test samples are not just noise + if (justNoise(dest, *size)) return -1; - // FSK demodulator - *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50 - if (*size < 128) return -2; //did we get a good demod? + // FSK demodulator + *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50 + if (*size < 128) return -2; //did we get a good demod? - uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1}; + uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1}; size_t startIdx = 0; uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); if (errChk == 0) return -4; //preamble not found @@ -794,7 +879,7 @@ 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 (size == 0) return -1; if (size0 && maxPeak < peakcnt){ - maxPeak = peakcnt; + maxPeak = peakcnt; } peakcnt=0; } @@ -1041,7 +1126,7 @@ int DetectNRZClock(uint8_t dest[], size_t size, int clock) int iii=7; int best=0; for (iii=7; iii > 0; iii--){ - if (peaksdet[iii] > peaksdet[best]){ + if (peaksdet[iii] > peaksdet[best]){ best = iii; } //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]); @@ -1057,7 +1142,9 @@ void psk1TOpsk2(uint8_t *BitStream, size_t size) size_t i=1; uint8_t lastBit=BitStream[0]; for (; ilastFCcnt)){ //not the same size as the last wave - start of new bit sequence @@ -1496,7 +1598,7 @@ uint8_t countPSK_FC(uint8_t *BitStream, size_t size) uint8_t fcLensFnd = 0; uint32_t fcCounter = 0; size_t i; - if (size == 0) return 0; + if (size == 0) return 0; // prime i to first up transition for (i = 1; i < size-1; i++) @@ -1548,11 +1650,11 @@ uint8_t countPSK_FC(uint8_t *BitStream, size_t size) 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; + if (size == 0) return -1; if (*size= dest[i+2]){ - if (waveStart == 0) { - waveStart = i+1; - avgWaveVal=dest[i+1]; - //PrintAndLog("DEBUG: waveStart: %d",waveStart); - } else { - waveEnd = i+1; - //PrintAndLog("DEBUG: waveEnd: %d",waveEnd); - waveLenCnt = waveEnd-waveStart; - lastAvgWaveVal = avgWaveVal/waveLenCnt; - if (waveLenCnt > fc){ - firstFullWave = waveStart; - fullWaveLen=waveLenCnt; - //if average wave value is > graph 0 then it is an up wave or a 1 - if (lastAvgWaveVal > 128) curPhase^=1; - break; - } - waveStart=0; - avgWaveVal=0; - } + waveEnd = i+1; + //PrintAndLog("DEBUG: waveEnd: %d",waveEnd); + waveLenCnt = waveEnd-waveStart; + if (waveLenCnt > fc && waveStart > fc){ //not first peak and is a large wave + lastAvgWaveVal = avgWaveVal/(waveLenCnt); + firstFullWave = waveStart; + fullWaveLen=waveLenCnt; + //if average wave value is > graph 0 then it is an up wave or a 1 + if (lastAvgWaveVal > 123) curPhase^=1; //fudge graph 0 a little 123 vs 128 + break; + } + waveStart = i+1; + avgWaveVal = 0; } - avgWaveVal+=dest[i+1]; + 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; - //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit); - + //set skipped bits + memset(dest+numBits,curPhase^1,firstFullWave / *clock); + numBits += (firstFullWave / *clock); + dest[numBits++] = curPhase; //set first read bit 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]){ @@ -1604,26 +1703,23 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) waveEnd = i+1; waveLenCnt = waveEnd-waveStart; lastAvgWaveVal = avgWaveVal/waveLenCnt; - if (waveLenCnt > fc){ + if (waveLenCnt > fc){ //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal); //if 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; - dest[numBits] = curPhase; - numBits++; + dest[numBits++] = curPhase; lastClkBit += *clock; - } else if (i lastClkBit + *clock + tol + fc){ lastClkBit += *clock; //no phase shift but clock bit - dest[numBits] = curPhase; - numBits++; + dest[numBits++] = curPhase; } avgWaveVal=0; waveStart=i+1;