X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/d5051b981792695644f17f86cf4ed49bac2b5ff0..56de46b4667c468fa751747b6d1346b5be6f5bd3:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index fdcb475b..74e3e6b6 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -39,7 +39,7 @@ //********************************************************************************************** //---------------------------------Utilities Section-------------------------------------------- //********************************************************************************************** - +#define LOWEST_DEFAULT_CLOCK 32 //to allow debug print calls when used not on device void dummy(char *fmt, ...){} #ifndef ON_DEVICE @@ -222,6 +222,64 @@ size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_ return 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, 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) { + size_t i=0, firstLow, firstHigh; + size_t testsize = (size < 512) ? size : 512; + + if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) { + if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting"); + return false; //just noise + } + + // get to first full low to prime loop and skip incomplete first pulse + getNextHigh(samples, size, *high, &i); + getNextLow(samples, size, *low, &i); + *skip = i; + + // populate tmpbuff buffer with pulse lengths + while (i < size) { + // measure from low to low + firstLow = i; + //find first high point for this wave + getNextHigh(samples, size, *high, &i); + firstHigh = i; + + getNextLow(samples, size, *low, &i); + + if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK)) + break; + + highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low + lowToLowWaveLen[*waveCnt] = i - firstLow; + *waveCnt += 1; + if (i-firstLow < *minClk && i < size) { + *minClk = i - firstLow; + } + } + return true; +} + //by marshmellow //amplify based on ask edge detection - not accurate enough to use all the time void askAmp(uint8_t *BitStream, size_t size) { @@ -289,28 +347,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; @@ -318,14 +370,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 @@ -888,14 +938,14 @@ 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) { - for (; i < buffSize - 4; ++i) { - *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 - if (lowToLowWaveLen[i+1] >= clk*2-tol && lowToLowWaveLen[i+1] <= clk*2+tol && highToLowWaveLen[i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2 - if (lowToLowWaveLen[i+2] >= (clk*3)/2-tol && lowToLowWaveLen[i+2] <= clk*2+tol && highToLowWaveLen[i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave - if (lowToLowWaveLen[i+3] >= clk*1-tol && lowToLowWaveLen[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit - *stStopLoc = i + 3; +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 + if (lowToLowWaveLen[*i+1] >= clk*2-tol && lowToLowWaveLen[*i+1] <= clk*2+tol && highToLowWaveLen[*i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2 + if (lowToLowWaveLen[*i+2] >= (clk*3)/2-tol && lowToLowWaveLen[*i+2] <= clk*2+tol && highToLowWaveLen[*i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave + if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit + *stStopLoc = *i + 3; return true; } } @@ -909,77 +959,36 @@ 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, j, skip, start, end, low, high, minClk, waveStart; + 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... - size_t testsize = (bufsize < 512) ? bufsize : 512; + 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; memset(tmpbuff, 0, sizeof(tmpbuff)); + memset(waveLen, 0, sizeof(waveLen)); - if ( getHiLo(buffer, testsize, &high, &low, 80, 80) == -1 ) { - if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting"); - return false; //just noise - } - i = 0; - j = 0; - minClk = 255; - // get to first full low to prime loop and skip incomplete first pulse - while ((buffer[i] < high) && (i < bufsize)) - ++i; - while ((buffer[i] > low) && (i < bufsize)) - ++i; - skip = i; - - // populate tmpbuff buffer with pulse lengths - while (i < bufsize) { - // measure from low to low - while ((buffer[i] > low) && (i < bufsize)) - ++i; - start= i; - while ((buffer[i] < high) && (i < bufsize)) - ++i; - //first high point for this wave - waveStart = i; - while ((buffer[i] > low) && (i < bufsize)) - ++i; - if (j >= (bufsize/32)) { - break; - } - waveLen[j] = i - waveStart; //first high to first low - tmpbuff[j++] = i - start; - if (i-start < minClk && i < bufsize) { - minClk = i - start; - } - } + 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; - i=0; - if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, i)) { + + 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"); return false; } else { - if (g_debugMode==2) prnt("DEBUG STT: first STT found at: %d, j=%d",start, j); + if (g_debugMode==2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j); } if (waveLen[i+2] > clk*1+tol) phaseoff = 0; @@ -992,7 +1001,8 @@ bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststa // now do it again to find the end int dummy1 = 0; end = skip; - if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, i+3)) { + i+=3; + if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) { //didn't find second ST - ERROR if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting"); return false; @@ -1071,6 +1081,44 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { return DetectST_ext(buffer, size, foundclock, &ststart, &stend); } +//by marshmellow +//take 11 10 01 11 00 and make 01100 ... miller decoding +//check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row +//decodes miller encoded binary +//NOTE askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as! +int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) { + if (*size < 16) return -1; + uint16_t MaxBits = 512, errCnt = 0; + size_t i, bitCnt=0; + uint8_t alignCnt = 0, curBit = BitStream[0], alignedIdx = 0; + uint8_t halfClkErr = 0; + //find alignment, needs 4 1s or 0s to properly align + for (i=1; i < *size-1; i++) { + alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0; + curBit = BitStream[i]; + if (alignCnt == 4) break; + } + // for now error if alignment not found. later add option to run it with multiple offsets... + if (alignCnt != 4) { + if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it"); + return -1; + } + alignedIdx = (i-1) % 2; + for (i=alignedIdx; i < *size-3; i+=2) { + halfClkErr = (uint8_t)((halfClkErr << 1 | BitStream[i]) & 0xFF); + if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) { + errCnt++; + BitStream[bitCnt++] = 7; + continue; + } + BitStream[bitCnt++] = BitStream[i] ^ BitStream[i+1] ^ invert; + + if (bitCnt > MaxBits) break; + } + *size = bitCnt; + return errCnt; +} + //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 @@ -1850,7 +1898,6 @@ int VikingDemod_AM(uint8_t *dest, size_t *size) { return (int) startIdx; } - // by iceman // find Visa2000 preamble in already demoded data int Visa2kDemod_AM(uint8_t *dest, size_t *size) {