X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/5735cfa58ee49398bcb494c1ea8e245baa51fac7..9aeda6cbfbfadd3be02f43165617b1ec4ff45425:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index f231cd69..4cf22d86 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -25,15 +25,13 @@ void dummy(char *fmt, ...){} #define prnt dummy #endif -uint8_t justNoise(uint8_t *BitStream, size_t size) -{ - static const uint8_t THRESHOLD = 123; - //test samples are not just noise - uint8_t justNoise1 = 1; - for(size_t idx=0; idx < size && justNoise1 ;idx++){ - justNoise1 = BitStream[idx] < THRESHOLD; - } - return justNoise1; +//test samples are not just noise +uint8_t justNoise(uint8_t *bits, size_t size) { + #define THRESHOLD 123 + uint8_t val = 1; + for(size_t idx=0; idx < size && val ;idx++) + val = bits[idx] < THRESHOLD; + return val; } //by marshmellow @@ -73,7 +71,7 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p { uint32_t parityWd = 0; size_t j = 0, bitCnt = 0; - for (int word = 0; word < (bLen); word+=pLen){ + 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]); @@ -81,12 +79,11 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p j--; // overwrite parity with next data // if parity fails then return 0 switch (pType) { - case 3: if (BitStream[j]==1) return 0; break; //should be 0 spacer bit - case 2: if (BitStream[j]==0) return 0; break; //should be 1 spacer bit - default: //test parity - if (parityTest(parityWd, pLen, pType) == 0) return 0; break; + case 3: if (BitStream[j]==1) { return 0; } break; //should be 0 spacer bit + case 2: if (BitStream[j]==0) { return 0; } break; //should be 1 spacer bit + default: if (parityTest(parityWd, pLen, pType) == 0) { return 0; } break; //test parity } - bitCnt+=(pLen-1); + bitCnt += (pLen-1); parityWd = 0; } // if we got here then all the parities passed @@ -170,13 +167,13 @@ 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 -uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo) +int 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) return 0; //allow only 1s and 0s + if (BitStream[1]>1) return -1; //allow only 1s and 0s // 111111111 bit pattern represent start of frame // include 0 in front to help get start pos @@ -187,14 +184,15 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ uint8_t FmtLen = 10; *startIdx = 0; errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, startIdx); - if (errChk == 0 || *size < 64) return 0; + if (errChk == 0 ) return -4; + if (*size < 64) return -3; if (*size > 64) FmtLen = 22; *startIdx += 1; //get rid of 0 from preamble idx = *startIdx + 9; for (i=0; i> 63); @@ -220,11 +218,12 @@ int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int 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 + if (smplCnt > clk + (clk/4)+2) { //too many samples errCnt++; if (g_debugMode==2) prnt("DEBUG ASK: Modulation Error at: %u", i); - BinStream[bitCnt++]=7; + BinStream[bitCnt++] = 7; } else if (waveHigh) { BinStream[bitCnt++] = invert; BinStream[bitCnt++] = invert; @@ -282,7 +281,7 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr if (*clk==0 || start < 0) return -3; if (*invert != 1) *invert = 0; if (amp==1) askAmp(BinStream, *size); - if (g_debugMode==2) prnt("DEBUG ASK: clk %d, beststart %d", *clk, start); + if (g_debugMode==2) prnt("DEBUG ASK: size %d, clk %d, beststart %d", *size, *clk, start); uint8_t initLoopMax = 255; if (initLoopMax > *size) initLoopMax = *size; @@ -299,8 +298,8 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr errCnt = cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); if (askType) //askman return manrawdecode(BinStream, size, 0); - else //askraw - return errCnt; + //askraw + return errCnt; } if (g_debugMode==2) prnt("DEBUG ASK: Weak Wave Detected - using weak wave demod"); @@ -347,38 +346,36 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr *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, uint8_t invert) -{ - uint16_t bitnum=0, MaxBits = 512, errCnt = 0; - size_t i, ii; +int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert){ + uint16_t bitnum = 0, MaxBits = 512, errCnt = 0; + size_t i, k; uint16_t bestErr = 1000, bestRun = 0; if (*size < 16) return -1; //find correct start position [alignment] - for (ii=0;ii<2;++ii){ - for (i=ii; i<*size-3; i+=2) - if (BitStream[i]==BitStream[i+1]) + for (k=0; k < 2; ++k){ + for (i=k; i<*size-3; i += 2) + if (BitStream[i] == BitStream[i+1]) errCnt++; - if (bestErr>errCnt){ - bestErr=errCnt; - bestRun=ii; + if (bestErr > errCnt){ + bestErr = errCnt; + bestRun = k; } errCnt=0; } //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; + 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; + BitStream[bitnum++] = 7; } - if(bitnum>MaxBits) break; + if (bitnum>MaxBits) break; } *size=bitnum; return bestErr; @@ -585,7 +582,7 @@ int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32 { if (justNoise(dest, *size)) return -1; - size_t numStart=0, size2=*size, startIdx=0; + size_t numStart=0, size2 = *size, startIdx=0; // FSK demodulator *size = fskdemod(dest, size2,50,1,10,8); //fsk2a if (*size < 96*2) return -2; @@ -604,10 +601,11 @@ int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32 *hi2 = (*hi2<<1)|(*hi>>31); *hi = (*hi<<1)|(*lo>>31); //Then, shift in a 0 or one into low + *lo <<= 1; if (dest[idx] && !dest[idx+1]) // 1 0 - *lo=(*lo<<1)|1; + *lo |= 1; else // 0 1 - *lo=(*lo<<1)|0; + *lo |= 0; } return (int)startIdx; } @@ -617,7 +615,7 @@ int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, ui { if (justNoise(dest, *size)) return -1; - size_t numStart=0, size2=*size, startIdx=0; + size_t numStart=0, size2 = *size, startIdx=0; // FSK demodulator *size = fskdemod(dest, size2,50,1,10,8); //fsk2a if (*size < 96) return -2; @@ -679,7 +677,6 @@ int IOdemodFSK(uint8_t *dest, size_t size) int VikingDemod_AM(uint8_t *dest, size_t *size) { //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); @@ -695,67 +692,77 @@ int VikingDemod_AM(uint8_t *dest, size_t *size) { if ( checkCalc != 0xA8 ) return -5; if (*size != 64) return -6; //return start position - return (int) startIdx; + return (int)startIdx; } +// by iceman +// find Visa2000 preamble in already demoded data +int Visa2kDemod_AM(uint8_t *dest, size_t *size) { + if (*size < 96*2) return -1; //make sure buffer has data + size_t startIdx = 0; + uint8_t preamble[] = {0,1,0,1,0,1,1,0,0,1,0,0,1,0,0,1,0,1,0,1,0,0,1,1,0,0,1,1,0,0,1,0}; + uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); + if (errChk == 0) return -2; //preamble not found + if (*size != 96) return -3; //wrong demoded size + //return start position + return (int)startIdx; +} +// by iceman +// find Noralsy preamble in already demoded data +int NoralsyDemod_AM(uint8_t *dest, size_t *size) { + if (*size < 96*2) return -1; //make sure buffer has data + size_t startIdx = 0; + uint8_t preamble[] = {1,0,1,1,1,0,1,1,0,0,0,0}; + uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); + if (errChk == 0) return -2; //preamble not found + if (*size != 96) return -3; //wrong demoded size + //return start position + return (int)startIdx; +} // find presco preamble 0x10D in already demoded data int PrescoDemod(uint8_t *dest, size_t *size) { - //make sure buffer has data - if (*size < 64*2) return -2; - + if (*size < 128*2) return -1; //make sure buffer has data size_t startIdx = 0; - uint8_t preamble[] = {1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t preamble[] = {0,0,0,1,0,0,0,0,1,1,0,1,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 (errChk == 0) return -2; //preamble not found + if (*size != 128) return -3; //wrong demoded size //return start position - return (int) startIdx; + return (int)startIdx; } // 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; - +int FDXBdemodBI(uint8_t *dest, size_t *size) { + if (*size < 128*2) return -1; //make sure buffer has enough data 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 + if (*size != 128) return -3; //wrong demoded size + //return start position return (int)startIdx; } // ASK/Diphase fc/64 (inverted Biphase) // Note: this i s not a demod, this is only a detection // the parameter *dest needs to be demoded before call +// 0xFFFF preamble, 64bits int JablotronDemod(uint8_t *dest, size_t *size){ - //make sure buffer has enough data - if (*size < 64) return -1; - + if (*size < 64*2) return -1; //make sure buffer has enough data size_t startIdx = 0; - // 0xFFFF preamble, 64bits - uint8_t preamble[] = { - 1,1,1,1, - 1,1,1,1, - 1,1,1,1, - 1,1,1,1, - 0 - }; - + uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0}; uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); - if (errChk == 0) return -4; //preamble not found - if (*size != 64) return -3; + if (errChk == 0) return -2; //preamble not found + if (*size != 64) return -3; // wrong demoded size uint8_t checkchksum = 0; for (int i=16; i < 56; i += 8) { checkchksum += bytebits_to_byte(dest+startIdx+i,8); } checkchksum ^= 0x3A; - uint8_t crc = bytebits_to_byte(dest+startIdx+56, 8); - - if ( checkchksum != crc ) return -5; + if ( checkchksum != crc ) return -5; return (int)startIdx; } @@ -794,7 +801,7 @@ int PyramiddemodFSK(uint8_t *dest, size_t *size) *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,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 @@ -815,6 +822,19 @@ int NedapDemod(uint8_t *dest, size_t *size) { return (int) startIdx; } +// Find IDTEC PSK1, RF Preamble == 0x4944544B, Demodsize 64bits +// by iceman +int IdteckDemodPSK(uint8_t *dest, size_t *size) { + //make sure buffer has data + if (*size < 64*2) return -1; + size_t startIdx = 0; + uint8_t preamble[] = {0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,0,0,1,0,0,1,0,1,1}; + uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); + if (errChk == 0) return -2; //preamble not found + if (*size != 64) return -3; // wrong demoded 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) @@ -919,24 +939,28 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) if (clockFnd>0) { clkCnt = clockFnd; clkEnd = clockFnd+1; + } else { + clkCnt=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){ + for(; clkCnt < clkEnd; clkCnt++) { + if (clk[clkCnt] <= 32) { tol=1; - }else{ + } else { tol=0; } //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; + 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; ii++){ if (dest[ii] < peak && dest[ii] > low) continue; - errCnt=0; + 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){ @@ -950,29 +974,29 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) } //if we found no errors then we can stop here and a low clock (common clocks) // this is correct one - return this clock - if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d",clk[clkCnt],errCnt,ii,i); - if(errCnt==0 && clkCnt<7) { + if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %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(errCnt