X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/56d0fb8e4d4f4a963928de36a12c519a09673bd1..bd09006a8c21bea8d185018aff73c0214d15dcc7:/common/lfdemod.c diff --git a/common/lfdemod.c b/common/lfdemod.c index 27bef1dd..55241143 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -7,33 +7,29 @@ //----------------------------------------------------------------------------- // Low frequency demod/decode commands //----------------------------------------------------------------------------- - -#include #include "lfdemod.h" -#include //un_comment to allow debug print calls when used not on device void dummy(char *fmt, ...){} + #ifndef ON_DEVICE -#include "ui.h" -#include "cmdparser.h" -#include "cmddata.h" -#define prnt PrintAndLog +# include "ui.h" +# include "cmdparser.h" +# include "cmddata.h" +# define prnt PrintAndLog #else uint8_t g_debugMode=0; -#define prnt dummy +# 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 @@ -62,7 +58,7 @@ 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); + //prnt("DEBUG: ans: %d, ptype: %d",ans,pType); return (ans == pType); } @@ -73,7 +69,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 +77,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 @@ -148,6 +143,9 @@ uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits) //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) { + // Sanity check. If preamble length is bigger than bitstream length. + if ( *size <= pLen ) return 0; + uint8_t foundCnt = 0; for (int idx = 0; idx < *size - pLen; idx++){ if (memcmp(BitStream+idx, preamble, pLen) == 0){ @@ -167,13 +165,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 @@ -184,14 +182,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); @@ -217,11 +216,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 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; @@ -276,10 +276,11 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr { 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 (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: clk %d, beststart %d, amp %d", *clk, start, amp); uint8_t initLoopMax = 255; if (initLoopMax > *size) initLoopMax = *size; @@ -296,8 +297,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"); @@ -344,38 +345,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; - uint16_t bestErr = 1000, bestRun = 0; +int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert){ + int errCnt = 0, bestErr = 1000; + uint16_t bitnum = 0, MaxBits = 512, bestRun = 0; + size_t i, k; 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; @@ -537,7 +536,7 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint32_t n=1; for( idx=1; idx < size; idx++) { n++; - if (dest[idx]==lastval) continue; + if (dest[idx]==lastval) continue; //skip until we hit a transition //find out how many bits (n) we collected //if lastval was 1, we have a 1->0 crossing @@ -582,7 +581,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; @@ -601,10 +600,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; } @@ -614,7 +614,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; @@ -676,7 +676,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); @@ -692,67 +691,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) 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) 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; } @@ -791,7 +800,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 @@ -812,6 +821,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) @@ -916,24 +938,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){ @@ -947,29 +973,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 BitStream[i-1] && BitStream[i]>=BitStream[i+1]) @@ -1359,7 +1385,7 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc } } if (rfCounter > 0 && rfLensFnd < 15){ - //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); + //prnt("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); rfCnts[rfLensFnd]++; rfLens[rfLensFnd++] = rfCounter; } @@ -1517,7 +1543,7 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) uint16_t errCnt=0, waveLenCnt=0; fc = countFC(dest, *size, 0); if (fc!=2 && fc!=4 && fc!=8) return -1; - //PrintAndLog("DEBUG: FC: %d",fc); + //prnt("DEBUG: FC: %d",fc); *clock = DetectPSKClock(dest, *size, *clock); if (*clock == 0) return -1; int avgWaveVal=0, lastAvgWaveVal=0; @@ -1525,7 +1551,7 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) for (i=0; i= dest[i+2]){ waveEnd = i+1; - //PrintAndLog("DEBUG: waveEnd: %d",waveEnd); + //prnt("DEBUG: waveEnd: %d",waveEnd); waveLenCnt = waveEnd-waveStart; if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+2)){ //not first peak and is a large wave but not out of whack lastAvgWaveVal = avgWaveVal/(waveLenCnt); @@ -1568,9 +1594,9 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) waveLenCnt = waveEnd-waveStart; lastAvgWaveVal = avgWaveVal/waveLenCnt; if (waveLenCnt > fc){ - //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal); + //prnt("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); + //prnt("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; @@ -1605,13 +1631,15 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { int tol = 0; int i, j, skip, start, end, low, high, minClk, waveStart; bool complete = false; - int tmpbuff[bufsize / 64]; - int waveLen[bufsize / 64]; + int tmpbuff[bufsize / 32]; //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured + int waveLen[bufsize / 32]; // 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 @@ -1638,7 +1666,7 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { waveStart = i; while ((buffer[i] > low) && (i < bufsize)) ++i; - if (j >= (bufsize/64)) { + if (j >= (bufsize/32)) { break; } waveLen[j] = i - waveStart; //first high to first low @@ -1684,6 +1712,8 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { if (start < 0) { 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 (waveLen[i+2] > clk*1+tol) phaseoff = 0; @@ -1697,7 +1727,7 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { end = skip; for (i += 3; i < j - 4; ++i) { end += tmpbuff[i]; - if (tmpbuff[i] >= clk*1-tol && tmpbuff[i] <= (clk*2)+tol) { //1 to 2 clocks depending on 2 bits prior + if (tmpbuff[i] >= clk*1-tol && tmpbuff[i] <= (clk*2)+tol && waveLen[i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior if (tmpbuff[i+1] >= clk*2-tol && tmpbuff[i+1] <= clk*2+tol && waveLen[i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2 if (tmpbuff[i+2] >= (clk*3)/2-tol && tmpbuff[i+2] <= clk*2+tol && waveLen[i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave if (tmpbuff[i+3] >= clk*1-tol && tmpbuff[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit @@ -1719,12 +1749,15 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { start = skip; size_t datalen = end - start; // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock - if (datalen % clk > clk/8) { + if ( clk - (datalen % clk) <= clk/8) { + // padd the amount off - could be problematic... but shouldn't happen often + datalen += clk - (datalen % clk); + } else if ( (datalen % clk) <= clk/8 ) { + // padd the amount off - could be problematic... but shouldn't happen often + datalen -= datalen % clk; + } else { if (g_debugMode==2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk); return false; - } else { - // padd the amount off - could be problematic... but shouldn't happen often - datalen += datalen % clk; } // if datalen is less than one t55xx block - ERROR if (datalen/clk < 8*4) { @@ -1732,8 +1765,20 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { return false; } size_t dataloc = start; + if (buffer[dataloc-(clk*4)-(clk/8)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) { + //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start + for ( i=0; i <= (clk/8); ++i ) { + if ( buffer[dataloc - (clk*4) - i] <= low ) { + dataloc -= i; + break; + } + } + } + size_t newloc = 0; i=0; + if (g_debugMode==2) prnt("DEBUG STT: Starting STT trim - start: %d, datalen: %d ",dataloc, datalen); + // warning - overwriting buffer given with raw wave data with ST removed... while ( dataloc < bufsize-(clk/2) ) { //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part) @@ -1752,6 +1797,7 @@ bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) { } newloc += i; //skip next ST - we just assume it will be there from now on... + if (g_debugMode==2) prnt("DEBUG STT: skipping STT at %d to %d", dataloc, dataloc+(clk*4)); dataloc += clk*4; } *size = newloc;