X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/cc15a1187b698d185a42fe956c0b68b9384eafdd..415274a7c3253b71b582c2f563bb54080c2790be:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index 5bbf8a66..5d19c897 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -112,7 +112,8 @@ 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; @@ -163,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; } @@ -216,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++; @@ -254,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; } @@ -268,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 @@ -307,88 +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 @@ -420,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 @@ -440,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; @@ -577,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++) @@ -588,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; @@ -616,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; @@ -630,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; } @@ -641,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) @@ -684,7 +701,8 @@ int PyramiddemodFSK(uint8_t *dest, size_t *size) return (int)startIdx; } - +// 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; @@ -792,7 +810,7 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) //test each valid clock from smallest to greatest to see which lines up for(; clkCnt < clkEnd; clkCnt++){ - if (clk[clkCnt] == 32){ + if (clk[clkCnt] <= 32){ tol=1; }else{ tol=0;