From 03e6bb4aed0ffd8e19897062071e18945c703d9d Mon Sep 17 00:00:00 2001 From: marshmellow42 Date: Thu, 22 Jan 2015 14:24:03 -0500 Subject: [PATCH] lf FSK demod tools/fixes added full ability to detect FSK clocks applied autodetect of fsk clock to data fskrawdemod this finished data fskfcdetect (now detects field clocks and bit clock) --- client/cmddata.c | 46 +++++++-- common/lfdemod.c | 245 ++++++++++++++++++++++++++--------------------- common/lfdemod.h | 3 +- 3 files changed, 172 insertions(+), 122 deletions(-) diff --git a/client/cmddata.c b/client/cmddata.c index 578587bc..998f5ebb 100644 --- a/client/cmddata.c +++ b/client/cmddata.c @@ -604,22 +604,40 @@ int CmdFSKrawdemod(const char *Cmd) { //raw fsk demod no manchester decoding no start bit finding just get binary from wave //set defaults - int rfLen = 50; + int rfLen = 0; int invert=0; - int fchigh=10; - int fclow=8; + int fchigh=0; + int fclow=0; //set options from parameters entered with the command sscanf(Cmd, "%i %i %i %i", &rfLen, &invert, &fchigh, &fclow); if (strlen(Cmd)>0 && strlen(Cmd)<=2) { if (rfLen==1){ invert=1; //if invert option only is used - rfLen = 50; - } else if(rfLen==0) rfLen=50; + rfLen = 0; + } } - PrintAndLog("Args invert: %d - Clock:%d - fchigh:%d - fclow: %d",invert,rfLen,fchigh, fclow); + uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0}; size_t BitLen = getFromGraphBuf(BitStream); + //get field clock lengths + uint16_t fcs=0; + if (fchigh==0 || fclow == 0){ + fcs=countFC(BitStream, BitLen); + if (fcs==0){ + fchigh=10; + fclow=8; + }else{ + fchigh = (fcs >> 8) & 0xFF; + fclow = fcs & 0xFF; + } + } + //get bit clock length + if (rfLen==0){ + rfLen = detectFSKClk(BitStream, BitLen, fchigh, fclow); + if (rfLen == 0) rfLen = 50; + } + PrintAndLog("Args invert: %d - Clock:%d - fchigh:%d - fclow: %d",invert,rfLen,fchigh, fclow); int size = fskdemod(BitStream,BitLen,(uint8_t)rfLen,(uint8_t)invert,(uint8_t)fchigh,(uint8_t)fclow); if (size>0){ PrintAndLog("FSK decoded bitstream:"); @@ -1159,12 +1177,20 @@ int CmdFSKfcDetect(const char *Cmd) uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0}; size_t size = getFromGraphBuf(BitStream); - - uint32_t ans = countFC(BitStream, size); - int fc1, fc2, rf1; + uint16_t ans = countFC(BitStream, size); + if (ans==0) { + if (g_debugMode) PrintAndLog("DEBUG: No data found"); + return 0; + } + uint8_t fc1, fc2; fc1 = (ans >> 8) & 0xFF; fc2 = ans & 0xFF; - rf1 = (ans >>16) & 0xFF; + + uint8_t rf1 = detectFSKClk(BitStream, size, fc1, fc2); + if (rf1==0) { + if (g_debugMode) PrintAndLog("DEBUG: Clock detect error"); + return 0; + } PrintAndLog("Detected Field Clocks: FC/%d, FC/%d - Bit Clock: RF/%d", fc1, fc2, rf1); return 1; } diff --git a/common/lfdemod.c b/common/lfdemod.c index 8f1a3764..34194394 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -1200,96 +1200,64 @@ int pskNRZrawDemod(uint8_t *dest, size_t *size, int *clk, int *invert) //by marshmellow -//countFC is to detect the field clock and bit clock rates. -//for fsk or ask not psk or nrz -uint32_t countFC(uint8_t *BitStream, size_t size) +//detects the bit clock for FSK given the high and low Field Clocks +uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow) { - // get high/low thresholds - int high, low; - getHiLo(BitStream,10, &high, &low, 100, 100); - // get zero crossing - uint8_t zeroC = (high-low)/2+low; - uint8_t clk[]={8,16,32,40,50,64,100,128}; - uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0}; - uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0}; - uint8_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0}; - // uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0}; - uint8_t fcLensFnd = 0; + uint8_t clk[] = {8,16,32,40,50,64,100,128,0}; + uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; uint8_t rfLensFnd = 0; - uint8_t lastBit=0; - uint8_t curBit=0; uint8_t lastFCcnt=0; - uint32_t errCnt=0; uint32_t fcCounter = 0; - uint32_t rfCounter = 0; + uint16_t rfCounter = 0; uint8_t firstBitFnd = 0; - int i; - + size_t i; + + uint8_t fcTol = (uint8_t)(0.5+(float)(fcHigh-fcLow)/2); + rfLensFnd=0; + fcCounter=0; + rfCounter=0; + firstBitFnd=0; + //PrintAndLog("DEBUG: fcTol: %d",fcTol); // prime i to first up transition - for (i = 1; i < size; i++) - if (BitStream[i]>=zeroC && BitStream[i-1] BitStream[i-1] && BitStream[i]>=BitStream[i+1]) break; - for (; i < size; i++){ - curBit = BitStream[i]; - lastBit = BitStream[i-1]; - if (lastBit= zeroC){ - // new up transition + for (; i < size-1; i++){ + if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1]){ + // new peak fcCounter++; rfCounter++; - if (fcCounter > 3 && fcCounter < 256){ - //we've counted enough that it could be a valid field clock - - //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) - if (lastFCcnt==5 && fcCounter==9) fcCounter--; - //if odd and not rc/5 add one (for when we get a fc 9 instead of 10) - if ((fcCounter==9 && fcCounter & 1) || fcCounter==4) fcCounter++; + // if we got less than the small fc + tolerance then set it to the small fc + if (fcCounter < fcLow+fcTol) + fcCounter = fcLow; + else //set it to the large fc + fcCounter = fcHigh; - //look for bit clock (rf/xx) - if ((fcCounterlastFCcnt)){ - //not the same size as the last wave - start of new bit sequence - - if (firstBitFnd>1){ //skip first wave change - probably not a complete bit - for (int ii=0; ii<10; ii++){ - if (rfLens[ii]==rfCounter){ - //rfCnts[ii]++; - rfCounter=0; - break; - } + //look for bit clock (rf/xx) + if ((fcCounterlastFCcnt)){ + //not the same size as the last wave - start of new bit sequence + + if (firstBitFnd>1){ //skip first wave change - probably not a complete bit + for (int ii=0; ii<15; ii++){ + if (rfLens[ii]==rfCounter){ + rfCnts[ii]++; + rfCounter=0; + break; } - if (rfCounter>0 && rfLensFnd<10){ - //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); - //rfCnts[rfLensFnd]++; - rfLens[rfLensFnd++]=rfCounter; - } - } else { - //PrintAndLog("DEBUG i: %d",i); - firstBitFnd++; } - rfCounter=0; - lastFCcnt=fcCounter; - } - - // save last field clock count (fc/xx) - // find which fcLens to save it to: - for (int ii=0; ii<10; ii++){ - if (fcLens[ii]==fcCounter){ - fcCnts[ii]++; - fcCounter=0; - break; + if (rfCounter>0 && rfLensFnd<15){ + //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); + rfCnts[rfLensFnd]++; + rfLens[rfLensFnd++]=rfCounter; } + } else { + firstBitFnd++; } - if (fcCounter>0 && fcLensFnd<10){ - //add new fc length - //PrintAndLog("FCCntr %d",fcCounter); - fcCnts[fcLensFnd]++; - fcLens[fcLensFnd++]=fcCounter; - } - } else{ - // hmmm this should not happen often - count them - errCnt++; + rfCounter=0; + lastFCcnt=fcCounter; } - // reset counter fcCounter=0; } else { // count sample @@ -1297,46 +1265,30 @@ uint32_t countFC(uint8_t *BitStream, size_t size) rfCounter++; } } - // if too many errors return errors as negative number (IS THIS NEEDED?) - if (errCnt>100) return -1*errCnt; - - uint8_t maxCnt1=0, best1=9, best2=9, best3=9, rfHighest=10, rfHighest2=10, rfHighest3=10; + uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15; - // go through fclens and find which ones are bigest 2 - for (i=0; i<10; i++){ - // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d, RF %d",fcLens[i],fcCnts[i],errCnt,rfLens[i]); - - // get the 3 best FC values - if (fcCnts[i]>maxCnt1) { - best3=best2; - best2=best1; - maxCnt1=fcCnts[i]; - best1=i; - } else if(fcCnts[i]>fcCnts[best2]){ - best3=best2; - best2=i; - } else if(fcCnts[i]>fcCnts[best3]){ - best3=i; - } + for (i=0; i<15; i++){ + //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]); //get highest 2 RF values (might need to get more values to compare or compare all?) - if (rfLens[i]>rfLens[rfHighest]){ + if (rfCnts[i]>rfCnts[rfHighest]){ rfHighest3=rfHighest2; rfHighest2=rfHighest; rfHighest=i; - } else if(rfLens[i]>rfLens[rfHighest2]){ + } else if(rfCnts[i]>rfCnts[rfHighest2]){ rfHighest3=rfHighest2; rfHighest2=i; - } else if(rfLens[i]>rfLens[rfHighest3]){ + } else if(rfCnts[i]>rfCnts[rfHighest3]){ rfHighest3=i; } - } - - // set allowed clock remainder tolerance to be 1 large field clock length - // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off - int tol1 = (fcLens[best1]>fcLens[best2]) ? fcLens[best1] : fcLens[best2]; + } + // set allowed clock remainder tolerance to be 1 large field clock length+1 + // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off + uint8_t tol1 = fcHigh+1; + //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]); + // loop to find the highest clock that has a remainder less than the tolerance - // compare samples counted divided by + // compare samples counted divided by int ii=7; for (; ii>=0; ii--){ if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){ @@ -1348,19 +1300,90 @@ uint32_t countFC(uint8_t *BitStream, size_t size) } } - if (ii<0) ii=7; // oops we went too far + if (ii<0) return 0; // oops we went too far - // TODO: take top 3 answers and compare to known Field clocks to get top 2 + return clk[ii]; +} - uint32_t fcs=0; - // PrintAndLog("DEBUG: Best %d best2 %d best3 %d, clk %d, clk2 %d",fcLens[best1],fcLens[best2],fcLens[best3],clk[i],clk[ii]); - // +//by marshmellow +//countFC is to detect the field clock lengths. +//counts and returns the 2 most common wave lengths +uint16_t countFC(uint8_t *BitStream, size_t size) +{ + uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0}; + uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0}; + uint8_t fcLensFnd = 0; + uint8_t lastFCcnt=0; + uint32_t fcCounter = 0; + size_t i; + + // prime i to first up transition + for (i = 1; i < size-1; i++) + if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]) + break; + for (; i < size-1; i++){ + if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){ + // new up transition + fcCounter++; + + //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) + if (lastFCcnt==5 && fcCounter==9) fcCounter--; + //if odd and not rc/5 add one (for when we get a fc 9 instead of 10) + if ((fcCounter==9 && fcCounter & 1) || fcCounter==4) fcCounter++; + + // save last field clock count (fc/xx) + // find which fcLens to save it to: + for (int ii=0; ii<10; ii++){ + if (fcLens[ii]==fcCounter){ + fcCnts[ii]++; + fcCounter=0; + break; + } + } + if (fcCounter>0 && fcLensFnd<10){ + //add new fc length + fcCnts[fcLensFnd]++; + fcLens[fcLensFnd++]=fcCounter; + } + fcCounter=0; + } else { + // count sample + fcCounter++; + } + } + + uint8_t best1=9, best2=9, best3=9; + uint16_t maxCnt1=0; + // go through fclens and find which ones are bigest 2 + for (i=0; i<10; i++){ + // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt); + // get the 3 best FC values + if (fcCnts[i]>maxCnt1) { + best3=best2; + best2=best1; + maxCnt1=fcCnts[i]; + best1=i; + } else if(fcCnts[i]>fcCnts[best2]){ + best3=best2; + best2=i; + } else if(fcCnts[i]>fcCnts[best3]){ + best3=i; + } + } + uint8_t fcH=0, fcL=0; if (fcLens[best1]>fcLens[best2]){ - fcs = (((uint32_t)clk[ii])<<16) | (((uint32_t)fcLens[best1])<<8) | ((fcLens[best2])); - } else { - fcs = (((uint32_t)clk[ii])<<16) | (((uint32_t)fcLens[best2])<<8) | ((fcLens[best1])); + fcH=fcLens[best1]; + fcL=fcLens[best2]; + } else{ + fcH=fcLens[best2]; + fcL=fcLens[best1]; } + + // TODO: take top 3 answers and compare to known Field clocks to get top 2 + uint16_t fcs = (((uint16_t)fcH)<<8) | fcL; + // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]); + return fcs; } diff --git a/common/lfdemod.h b/common/lfdemod.h index 9698d8bd..ca50d4f2 100644 --- a/common/lfdemod.h +++ b/common/lfdemod.h @@ -33,7 +33,8 @@ void pskCleanWave(uint8_t *bitStream, size_t size); int PyramiddemodFSK(uint8_t *dest, size_t size); int AWIDdemodFSK(uint8_t *dest, size_t size); size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen); -uint32_t countFC(uint8_t *BitStream, size_t size); +uint16_t countFC(uint8_t *BitStream, size_t size); +uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow); int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo); size_t ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo); -- 2.39.2