X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/d87bf1567a5520ae035b2cd710992fa05a5c6439..51b614aa75073617fa7f56196c3eef3e2145c786:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index d7524dc9..c56301c0 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -35,10 +35,13 @@ #include // for memset, memcmp and size_t #include // for uint_32+ #include // for bool +#include "parity.h" // for parity test //********************************************************************************************** //---------------------------------Utilities Section-------------------------------------------- //********************************************************************************************** +#define LOWEST_DEFAULT_CLOCK 32 +#define FSK_PSK_THRESHOLD 123 //to allow debug print calls when used not on device void dummy(char *fmt, ...){} @@ -53,11 +56,10 @@ void dummy(char *fmt, ...){} #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; + justNoise1 = BitStream[idx] < FSK_PSK_THRESHOLD; } return justNoise1; } @@ -72,7 +74,7 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi if (BitStream[i] > *high) *high = BitStream[i]; if (BitStream[i] < *low) *low = BitStream[i]; } - if (*high < 123) return -1; // just noise + if (*high < FSK_PSK_THRESHOLD) return -1; // just noise *high = ((*high-128)*fuzzHi + 12800)/100; *low = ((*low-128)*fuzzLo + 12800)/100; return 1; @@ -81,40 +83,35 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi // by marshmellow // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType // returns 1 if passed -uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) { - uint8_t ans = 0; - for (uint8_t i = 0; i < bitLen; i++){ - ans ^= ((bits >> i) & 1); - } - if (g_debugMode) prnt("DEBUG: ans: %d, ptype: %d, bits: %08X",ans,pType,bits); - return (ans == pType); +bool parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) { + return oddparity32(bits) ^ pType; } // 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; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run) +// takes a array of binary values, start position, length of bits per parity (includes parity bit - MAX 32), +// Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0'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; - size_t j = 0, bitCnt = 0; + size_t bitCnt = 0; for (int word = 0; word < (bLen); word+=pLen) { for (int bit=0; bit < pLen; bit++) { + if (word+bit >= bLen) break; parityWd = (parityWd << 1) | BitStream[startIdx+word+bit]; - BitStream[j++] = (BitStream[startIdx+word+bit]); + BitStream[bitCnt++] = (BitStream[startIdx+word+bit]); } if (word+pLen > bLen) break; - j--; // overwrite parity with next data + bitCnt--; // 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 + case 3: if (BitStream[bitCnt]==1) {return 0;} break; //should be 0 spacer bit + case 2: if (BitStream[bitCnt]==0) {return 0;} break; //should be 1 spacer bit default: if (parityTest(parityWd, pLen, pType) == 0) {return 0;} break; //test parity } - bitCnt+=(pLen-1); parityWd = 0; } // if we got here then all the parities passed - //return ID start index and size + //return size return bitCnt; } @@ -197,18 +194,18 @@ uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_ } // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup. -size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_t expWaveSize) { +size_t findModStart(uint8_t dest[], size_t size, uint8_t expWaveSize) { size_t i = 0; size_t waveSizeCnt = 0; uint8_t thresholdCnt = 0; - bool isAboveThreshold = dest[i++] >= threshold_value; + bool isAboveThreshold = dest[i++] >= FSK_PSK_THRESHOLD; for (; i < size-20; i++ ) { - if(dest[i] < threshold_value && isAboveThreshold) { + if(dest[i] < FSK_PSK_THRESHOLD && isAboveThreshold) { thresholdCnt++; if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break; isAboveThreshold = false; waveSizeCnt = 0; - } else if (dest[i] >= threshold_value && !isAboveThreshold) { + } else if (dest[i] >= FSK_PSK_THRESHOLD && !isAboveThreshold) { thresholdCnt++; if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break; isAboveThreshold = true; @@ -222,19 +219,29 @@ size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_ return i; } -void getNextLow(uint8_t samples[], size_t size, int low, int *i) { +int getClosestClock(int testclk) { + uint8_t fndClk[] = {8,16,32,40,50,64,128}; + + for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) + if (testclk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && testclk <= fndClk[clkCnt]+1) + return fndClk[clkCnt]; + + return 0; +} + +void getNextLow(uint8_t samples[], size_t size, int low, size_t *i) { while ((samples[*i] > low) && (*i < size)) *i+=1; } -void getNextHigh(uint8_t samples[], size_t size, int high, int *i) { +void getNextHigh(uint8_t samples[], size_t size, int high, size_t *i) { while ((samples[*i] < high) && (*i < size)) *i+=1; } // load wave counters bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) { - int i=0, start, waveStart; + size_t i=0, firstLow, firstHigh; size_t testsize = (size < 512) ? size : 512; if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) { @@ -250,23 +257,21 @@ bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int // populate tmpbuff buffer with pulse lengths while (i < size) { // measure from low to low - getNextLow(samples, size, *low, &i); - start = i; - + firstLow = i; //find first high point for this wave getNextHigh(samples, size, *high, &i); - waveStart = i; + firstHigh = i; getNextLow(samples, size, *low, &i); - if (*waveCnt >= (size/32)) + if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK)) break; - highToLowWaveLen[*waveCnt] = i - waveStart; //first high to first low - lowToLowWaveLen[*waveCnt] = i - start; + highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low + lowToLowWaveLen[*waveCnt] = i - firstLow; *waveCnt += 1; - if (i-start < *minClk && i < size) { - *minClk = i - start; + if (i-firstLow < *minClk && i < size) { + *minClk = i - firstLow; } } return true; @@ -299,15 +304,17 @@ uint32_t manchesterEncode2Bytes(uint16_t datain) { //by marshmellow //encode binary data into binary manchester -//NOTE: BitStream must have double the size available in memory to do the swap +//NOTE: BitStream must have triple the size of "size" available in memory to do the swap int ManchesterEncode(uint8_t *BitStream, size_t size) { - size_t modIdx=size, i=0; - if (size>modIdx) return -1; + //allow up to 4K out (means BitStream must be at least 2048+4096 to handle the swap) + size = (size>2048) ? 2048 : size; + size_t modIdx = size; + size_t 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++){ + for (i=0; i<(size*2); i++){ BitStream[i] = BitStream[i+size]; } return i; @@ -339,28 +346,22 @@ uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t lo // by marshmellow // to help detect clocks on heavily clipped samples // based on count of low to low -int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low, int *clock) { - uint8_t fndClk[] = {8,16,32,40,50,64,128}; +int DetectStrongAskClock(uint8_t dest[], size_t size, int high, int low, int *clock) { size_t startwave; size_t i = 100; size_t minClk = 255; int shortestWaveIdx = 0; // 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; + getNextHigh(dest, size, high, &i); + getNextLow(dest, size, low, &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; + + getNextHigh(dest, size, high, &i); + getNextLow(dest, size, low, &i); //get minimum measured distance if (i-startwave < minClk && i < size) { minClk = i - startwave; @@ -368,14 +369,12 @@ int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low, } } // set clock - if (g_debugMode==2) prnt("DEBUG ASK: detectstrongASKclk smallest wave: %d",minClk); - for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) { - if (minClk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && minClk <= fndClk[clkCnt]+1) { - *clock = fndClk[clkCnt]; - return shortestWaveIdx; - } - } - return 0; + if (g_debugMode==2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d",minClk); + *clock = getClosestClock(minClk); + if (*clock == 0) + return 0; + + return shortestWaveIdx; } // by marshmellow @@ -509,7 +508,7 @@ int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low){ //by marshmellow //detect nrz clock by reading #peaks vs no peaks(or errors) -int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) { +int DetectNRZClock(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) { size_t i=0; uint8_t clk[]={8,16,32,40,50,64,100,128,255}; size_t loopCnt = 4096; //don't need to loop through entire array... @@ -625,11 +624,6 @@ int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStar return clk[best]; } -int DetectNRZClock(uint8_t dest[], size_t size, int clock) { - size_t bestStart=0; - return DetectNRZClock_ext(dest, size, clock, &bestStart); -} - //by marshmellow //countFC is to detect the field clock lengths. //counts and returns the 2 most common wave lengths @@ -938,7 +932,7 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc //********************************************************************************************** // look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index... -bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, int *i) { +bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) { for (; *i < buffSize - 4; *i+=1) { *stStartIdx += lowToLowWaveLen[*i]; //caution part of this wave may be data and part may be ST.... to be accounted for in main function for now... if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior @@ -959,13 +953,13 @@ bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLo bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststart, size_t *stend) { size_t bufsize = *size; //need to loop through all samples and identify our clock, look for the ST pattern - uint8_t fndClk[] = {8,16,32,40,50,64,128}; int clk = 0; int tol = 0; - int i=0, j, skip, start, end, low, high, minClk=255; + int j, high, low, skip, start, end, minClk=255; + size_t i = 0; //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow] - int tmpbuff[bufsize / 32]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured - int waveLen[bufsize / 32]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed... + int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured + int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed... //size_t testsize = (bufsize < 512) ? bufsize : 512; int phaseoff = 0; high = low = 128; @@ -974,22 +968,15 @@ bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststa if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false; // set clock - might be able to get this externally and remove this work... - if (!clk) { - for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) { - tol = fndClk[clkCnt]/8; - if (minClk >= fndClk[clkCnt]-tol && minClk <= fndClk[clkCnt]+1) { - clk=fndClk[clkCnt]; - break; - } - } - // clock not found - ERROR - if (!clk) { - if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting"); - return false; - } - } else tol = clk/8; - + clk = getClosestClock(minClk); + // clock not found - ERROR + if (clk == 0) { + if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting"); + return false; + } *foundclock = clk; + + tol = clk/8; if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) { // first ST not found - ERROR if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting"); @@ -1341,9 +1328,10 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr // by marshmellow - demodulate NRZ wave - requires a read with strong signal // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak -int nrzRawDemod_ext(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) { +int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) { if (justNoise(dest, *size)) return -1; - *clk = DetectNRZClock(dest, *size, *clk); + size_t clkStartIdx = 0; + *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx); if (*clk==0) return -2; size_t i, gLen = 4096; if (gLen>*size) gLen = *size-20; @@ -1375,10 +1363,6 @@ int nrzRawDemod_ext(uint8_t *dest, size_t *size, int *clk, int *invert, int *sta *size = numBits; return 0; } -int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert) { - int startIdx = 0; - return nrzRawDemod_ext(dest, size, clk, invert, &startIdx); -} //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq]) size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) { @@ -1387,16 +1371,15 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow if (fchigh==0) fchigh=10; 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; if ( size < 1024 ) return 0; // not enough samples //find start of modulating data in trace - idx = findModStart(dest, size, threshold_value, fchigh); + idx = findModStart(dest, size, fchigh); // Need to threshold first sample - if(dest[idx] < threshold_value) dest[0] = 0; + if(dest[idx] < FSK_PSK_THRESHOLD) dest[0] = 0; else dest[0] = 1; last_transition = idx; @@ -1408,7 +1391,7 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow // (could also be fc/5 && fc/7 for fsk1 = 4-9) for(; idx < size; idx++) { // threshold current value - if (dest[idx] < threshold_value) dest[idx] = 0; + if (dest[idx] < FSK_PSK_THRESHOLD) dest[idx] = 0; else dest[idx] = 1; // Check for 0->1 transition @@ -1538,54 +1521,55 @@ void psk2TOpsk1(uint8_t *BitStream, size_t size) { return; } +size_t pskFindFirstPhaseShift(uint8_t samples[], size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) { + uint16_t loopCnt = (size+3 < 4096) ? size : 4096; //don't need to loop through entire array... + + uint16_t avgWaveVal=0, lastAvgWaveVal=0; + size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave; + for (; i= samples[i+2]){ + waveEnd = i+1; + if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart); + waveLenCnt = waveEnd-waveStart; + if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack + lastAvgWaveVal = avgWaveVal/(waveLenCnt); + firstFullWave = waveStart; + *fullWaveLen = waveLenCnt; + //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting) + if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1; + return firstFullWave; + } + waveStart = i+1; + avgWaveVal = 0; + } + avgWaveVal += samples[i+2]; + } + return 0; +} + //by marshmellow - demodulate PSK1 wave //uses wave lengths (# Samples) int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *startIdx) { - if (size == 0) return -1; - uint16_t loopCnt = 4096; //don't need to loop through entire array... - if (*size> 8; - if (fc2 == 10) return -1; //fsk found - quit + if ((fc >> 8) == 10) return -1; //fsk found - quit fc = fc & 0xFF; if (fc!=2 && fc!=4 && fc!=8) return -1; - //PrintAndLog("DEBUG: FC: %d",fc); *clock = DetectPSKClock(dest, *size, *clock); if (*clock == 0) return -1; //find start of modulating data in trace - uint8_t threshold_value = 123; //-5 - i = findModStart(dest, *size, threshold_value, fc); + i = findModStart(dest, *size, fc); //find first phase shift - int avgWaveVal=0, lastAvgWaveVal=0; - waveStart = i; - for (; i= dest[i+2]){ - waveEnd = i+1; - if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u",waveEnd, waveStart); - waveLenCnt = waveEnd-waveStart; - if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+3)){ //not first peak and is a large wave but not out of whack - lastAvgWaveVal = avgWaveVal/(waveLenCnt); - firstFullWave = waveStart; - fullWaveLen=waveLenCnt; - //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting) - if (lastAvgWaveVal > threshold_value) curPhase ^= 1; - break; - } - - waveStart = i+1; - avgWaveVal = 0; - } - avgWaveVal += dest[i+2]; - } + firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen); if (firstFullWave == 0) { // no phase shift detected - could be all 1's or 0's - doesn't matter where we start // so skip a little to ensure we are past any Start Signal @@ -1613,9 +1597,7 @@ int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int * } else { //waveEnd waveEnd = i+1; waveLenCnt = waveEnd-waveStart; - lastAvgWaveVal = avgWaveVal/waveLenCnt; if (waveLenCnt > fc){ - //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal); //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 @@ -1701,6 +1683,7 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ *hi = (bytebits_to_byte(BitStream, 24)); *lo = ((uint64_t)(bytebits_to_byte(BitStream + 24, 32)) << 32) | (bytebits_to_byte(BitStream + 24 + 32, 32)); } else { + if (g_debugMode) prnt("Error removing parity: %u", *size); return 0; } return 1; @@ -1717,6 +1700,8 @@ int FDXBdemodBI(uint8_t *dest, size_t *size) { uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx); if (errChk == 0) return -2; //preamble not found + if (*size != 128) return -3; //wrong size for fdxb + //return start position return (int)startIdx; }