From: marshmellow42 Date: Fri, 27 Mar 2015 04:02:30 +0000 (-0400) Subject: lfdemods streamline & bug fixes X-Git-Tag: v2.1.0~22^2~4 X-Git-Url: http://cvs.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/commitdiff_plain/2eec55c8a4331daf5d523a1050e3381501b36b34 lfdemods streamline & bug fixes removed duplicate code countPSK_FC - adjusted countFC to work. bug fixes & streamlined code in: lf hid fskdemod lf em em410xdemod cleaned up lfdemod.c some --- diff --git a/armsrc/lfops.c b/armsrc/lfops.c index 797bc51c..e5a40b2e 100644 --- a/armsrc/lfops.c +++ b/armsrc/lfops.c @@ -755,8 +755,8 @@ void CmdPSKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream) void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol) { uint8_t *dest = BigBuf_get_addr(); - const size_t sizeOfBigBuff = BigBuf_max_traceLen(); - size_t size = 0; + //const size_t sizeOfBigBuff = BigBuf_max_traceLen(); + size_t size; uint32_t hi2=0, hi=0, lo=0; int idx=0; // Configure to go in 125Khz listen mode @@ -769,16 +769,16 @@ void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol) DoAcquisition_default(-1,true); // FSK demodulator - size = sizeOfBigBuff; //variable size will change after demod so re initialize it before use + //size = sizeOfBigBuff; //variable size will change after demod so re initialize it before use + size = 50*128*2; //big enough to catch 2 sequences of largest format idx = HIDdemodFSK(dest, &size, &hi2, &hi, &lo); - if (idx>0 && lo>0){ - // final loop, go over previously decoded manchester data and decode into usable tag ID - // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0 - if (hi2 != 0){ //extra large HID tags + if (idx>0 && lo>0 && (size==96 || size==192)){ + // go over previously decoded manchester data and decode into usable tag ID + if (hi2 != 0){ //extra large HID tags 88/192 bits Dbprintf("TAG ID: %x%08x%08x (%d)", (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF); - }else { //standard HID tags <38 bits + }else { //standard HID tags 44/96 bits //Dbprintf("TAG ID: %x%08x (%d)",(unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF); //old print cmd uint8_t bitlen = 0; uint32_t fc = 0; @@ -833,8 +833,8 @@ void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol) return; } // reset - hi2 = hi = lo = 0; } + hi2 = hi = lo = idx = 0; WDT_HIT(); } DbpString("Stopped"); @@ -859,49 +859,42 @@ void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol) DoAcquisition_default(-1,true); size = BigBuf_max_traceLen(); - //Dbprintf("DEBUG: Buffer got"); //askdemod and manchester decode + if (size > 16385) size = 16385; //big enough to catch 2 sequences of largest format errCnt = askmandemod(dest, &size, &clk, &invert, maxErr); - //Dbprintf("DEBUG: ASK Got"); WDT_HIT(); - if (errCnt>=0){ - errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo); - //Dbprintf("DEBUG: EM GOT"); - if (errCnt){ - if (size>64){ - Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)", - hi, - (uint32_t)(lo>>32), - (uint32_t)lo, - (uint32_t)(lo&0xFFFF), - (uint32_t)((lo>>16LL) & 0xFF), - (uint32_t)(lo & 0xFFFFFF)); - } else { - Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)", - (uint32_t)(lo>>32), - (uint32_t)lo, - (uint32_t)(lo&0xFFFF), - (uint32_t)((lo>>16LL) & 0xFF), - (uint32_t)(lo & 0xFFFFFF)); - } + if (errCnt<0) continue; + + errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo); + if (errCnt){ + if (size>64){ + Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)", + hi, + (uint32_t)(lo>>32), + (uint32_t)lo, + (uint32_t)(lo&0xFFFF), + (uint32_t)((lo>>16LL) & 0xFF), + (uint32_t)(lo & 0xFFFFFF)); + } else { + Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)", + (uint32_t)(lo>>32), + (uint32_t)lo, + (uint32_t)(lo&0xFFFF), + (uint32_t)((lo>>16LL) & 0xFF), + (uint32_t)(lo & 0xFFFFFF)); } + if (findone){ if (ledcontrol) LED_A_OFF(); *high=lo>>32; *low=lo & 0xFFFFFFFF; return; } - } else{ - //Dbprintf("DEBUG: No Tag"); } WDT_HIT(); - hi = 0; - lo = 0; - clk=0; - invert=0; - errCnt=0; - size=0; + hi = lo = size = idx = 0; + clk = invert = errCnt = 0; } DbpString("Stopped"); if (ledcontrol) LED_A_OFF(); @@ -925,47 +918,47 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol) //fskdemod and get start index WDT_HIT(); idx = IOdemodFSK(dest, BigBuf_max_traceLen()); - if (idx>0){ - //valid tag found - - //Index map - //0 10 20 30 40 50 60 - //| | | | | | | - //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23 - //----------------------------------------------------------------------------- - //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11 - // - //XSF(version)facility:codeone+codetwo - //Handle the data - if(findone){ //only print binary if we are doing one - Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx], dest[idx+1], dest[idx+2],dest[idx+3],dest[idx+4],dest[idx+5],dest[idx+6],dest[idx+7],dest[idx+8]); - Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+9], dest[idx+10],dest[idx+11],dest[idx+12],dest[idx+13],dest[idx+14],dest[idx+15],dest[idx+16],dest[idx+17]); - Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+18],dest[idx+19],dest[idx+20],dest[idx+21],dest[idx+22],dest[idx+23],dest[idx+24],dest[idx+25],dest[idx+26]); - Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+27],dest[idx+28],dest[idx+29],dest[idx+30],dest[idx+31],dest[idx+32],dest[idx+33],dest[idx+34],dest[idx+35]); - Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+36],dest[idx+37],dest[idx+38],dest[idx+39],dest[idx+40],dest[idx+41],dest[idx+42],dest[idx+43],dest[idx+44]); - Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+45],dest[idx+46],dest[idx+47],dest[idx+48],dest[idx+49],dest[idx+50],dest[idx+51],dest[idx+52],dest[idx+53]); - Dbprintf("%d%d%d%d%d%d%d%d %d%d",dest[idx+54],dest[idx+55],dest[idx+56],dest[idx+57],dest[idx+58],dest[idx+59],dest[idx+60],dest[idx+61],dest[idx+62],dest[idx+63]); - } - code = bytebits_to_byte(dest+idx,32); - code2 = bytebits_to_byte(dest+idx+32,32); - version = bytebits_to_byte(dest+idx+27,8); //14,4 - facilitycode = bytebits_to_byte(dest+idx+18,8) ; - number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9 - - Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2); - // if we're only looking for one tag - if (findone){ - if (ledcontrol) LED_A_OFF(); - //LED_A_OFF(); - *high=code; - *low=code2; - return; - } - code=code2=0; - version=facilitycode=0; - number=0; - idx=0; + if (idx<0) continue; + //valid tag found + + //Index map + //0 10 20 30 40 50 60 + //| | | | | | | + //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23 + //----------------------------------------------------------------------------- + //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11 + // + //XSF(version)facility:codeone+codetwo + //Handle the data + if(findone){ //only print binary if we are doing one + Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx], dest[idx+1], dest[idx+2],dest[idx+3],dest[idx+4],dest[idx+5],dest[idx+6],dest[idx+7],dest[idx+8]); + Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+9], dest[idx+10],dest[idx+11],dest[idx+12],dest[idx+13],dest[idx+14],dest[idx+15],dest[idx+16],dest[idx+17]); + Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+18],dest[idx+19],dest[idx+20],dest[idx+21],dest[idx+22],dest[idx+23],dest[idx+24],dest[idx+25],dest[idx+26]); + Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+27],dest[idx+28],dest[idx+29],dest[idx+30],dest[idx+31],dest[idx+32],dest[idx+33],dest[idx+34],dest[idx+35]); + Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+36],dest[idx+37],dest[idx+38],dest[idx+39],dest[idx+40],dest[idx+41],dest[idx+42],dest[idx+43],dest[idx+44]); + Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+45],dest[idx+46],dest[idx+47],dest[idx+48],dest[idx+49],dest[idx+50],dest[idx+51],dest[idx+52],dest[idx+53]); + Dbprintf("%d%d%d%d%d%d%d%d %d%d",dest[idx+54],dest[idx+55],dest[idx+56],dest[idx+57],dest[idx+58],dest[idx+59],dest[idx+60],dest[idx+61],dest[idx+62],dest[idx+63]); } + code = bytebits_to_byte(dest+idx,32); + code2 = bytebits_to_byte(dest+idx+32,32); + version = bytebits_to_byte(dest+idx+27,8); //14,4 + facilitycode = bytebits_to_byte(dest+idx+18,8); + number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9 + + Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2); + // if we're only looking for one tag + if (findone){ + if (ledcontrol) LED_A_OFF(); + //LED_A_OFF(); + *high=code; + *low=code2; + return; + } + code=code2=0; + version=facilitycode=0; + number=0; + idx=0; + WDT_HIT(); } DbpString("Stopped"); diff --git a/client/cmddata.c b/client/cmddata.c index faea2dc3..153f87bd 100644 --- a/client/cmddata.c +++ b/client/cmddata.c @@ -1146,7 +1146,7 @@ int FSKrawDemod(const char *Cmd, bool verbose) if (strlen(Cmd)>0 && strlen(Cmd)<=2) { if (rfLen==1){ - invert=1; //if invert option only is used + invert = 1; //if invert option only is used rfLen = 0; } } @@ -1156,9 +1156,8 @@ int FSKrawDemod(const char *Cmd, bool verbose) if (BitLen==0) return 0; //get field clock lengths uint16_t fcs=0; - uint8_t dummy=0; if (fchigh==0 || fclow == 0){ - fcs = countFC(BitStream, BitLen, &dummy); + fcs = countFC(BitStream, BitLen, 1); if (fcs==0){ fchigh=10; fclow=8; @@ -1822,7 +1821,7 @@ int PSKDemod(const char *Cmd, bool verbose) uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0}; size_t BitLen = getFromGraphBuf(BitStream); if (BitLen==0) return -1; - uint8_t carrier=countPSK_FC(BitStream, BitLen); + uint8_t carrier=countFC(BitStream, BitLen, 0); if (carrier!=2 && carrier!=4 && carrier!=8){ //invalid carrier return 0; @@ -1957,7 +1956,7 @@ int NRZrawDemod(const char *Cmd, bool verbose) if (g_debugMode) PrintAndLog("Too many errors found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt); return 0; } - if (errCnt<0|| BitLen<16){ //throw away static - allow 1 and -1 (in case of threshold command first) + if (errCnt<0 || BitLen<16){ //throw away static - allow 1 and -1 (in case of threshold command first) if (g_debugMode) PrintAndLog("no data found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt); return 0; } diff --git a/client/graph.c b/client/graph.c index 190dfe8f..f4acc579 100644 --- a/client/graph.c +++ b/client/graph.c @@ -146,7 +146,7 @@ uint8_t GetPskCarrier(const char str[], bool printAns, bool verbose) } //uint8_t countPSK_FC(uint8_t *BitStream, size_t size) - carrier = countPSK_FC(grph,size); + carrier = countFC(grph,size,0); // Only print this message if we're not looping something if (printAns){ PrintAndLog("Auto-detected PSK carrier rate: %d", carrier); @@ -232,8 +232,7 @@ uint8_t fskClocks(uint8_t *fc1, uint8_t *fc2, uint8_t *rf1, bool verbose) uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0}; size_t size = getFromGraphBuf(BitStream); if (size==0) return 0; - uint8_t dummy = 0; - uint16_t ans = countFC(BitStream, size, &dummy); + uint16_t ans = countFC(BitStream, size, 1); if (ans==0) { if (verbose) PrintAndLog("DEBUG: No data found"); return 0; diff --git a/common/lfdemod.c b/common/lfdemod.c index fae61206..d3c2a01c 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -11,8 +11,6 @@ #include #include #include "lfdemod.h" - - uint8_t justNoise(uint8_t *BitStream, size_t size) { static const uint8_t THRESHOLD = 123; @@ -31,7 +29,7 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi *high=0; *low=255; // get high and low thresholds - for (int i=0; i < size; i++){ + for (size_t i=0; i < size; i++){ if (BitStream[i] > *high) *high = BitStream[i]; if (BitStream[i] < *low) *low = BitStream[i]; } @@ -102,11 +100,8 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ idx = *startIdx + 9; for (i=0; i> 63); @@ -125,118 +120,89 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_ //prints binary found and saves in graphbuffer for further commands int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr) { - int i; - //int clk2=*clk; + size_t i; int start = DetectASKClock(BinStream, *size, clk, 20); //clock default - if (*clk==0) return -3; - if (start < 0) return -3; - // if autodetected too low then adjust //MAY NEED ADJUSTMENT - //if (clk2==0 && *clk<8) *clk =64; - //if (clk2==0 && *clk<32) *clk=32; - if (*invert != 0 && *invert != 1) *invert=0; - uint32_t initLoopMax = 200; - if (initLoopMax > *size) initLoopMax=*size; + if (*clk==0 || start < 0) return -3; + if (*invert != 1) *invert=0; + uint8_t initLoopMax = 255; + if (initLoopMax > *size) initLoopMax = *size; // Detect high and lows // 25% fuzz in case highs and lows aren't clipped [marshmellow] - int high, low, ans; - ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75); - if (ans<1) return -2; //just noise + int high, low; + if (getHiLo(BinStream, initLoopMax, &high, &low, 75, 75) < 1) return -2; //just noise // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low); int lastBit = 0; //set first clock check - uint32_t bitnum = 0; //output counter - int tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave - if (*clk<=32) tol=1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely - int iii = 0; - uint32_t gLen = *size; - if (gLen > 3000) gLen=3000; + uint16_t bitnum = 0; //output counter + uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave + if (*clk <= 32) tol=1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely + size_t iii = 0; //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr) gLen=*clk*2; - uint8_t errCnt =0; - uint16_t MaxBits = 500; - uint32_t bestStart = *size; - int bestErrCnt = maxErr+1; + if (!maxErr) initLoopMax = *clk * 2; + uint16_t errCnt = 0, MaxBits = 512; + uint16_t bestStart = start; + uint16_t bestErrCnt = 0; // PrintAndLog("DEBUG - lastbit - %d",lastBit); - // loop to find first wave that works - for (iii=0; iii < gLen; ++iii){ - if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){ - lastBit=iii-*clk; - errCnt=0; + // if best start position not already found by detect clock then + if (start <= 0 || start > initLoopMax){ + bestErrCnt = maxErr+1; + // loop to find first wave that works + for (iii=0; iii < initLoopMax; ++iii){ + // if no peak skip + if (BinStream[iii] < high && BinStream[iii] > low) continue; + + lastBit = iii - *clk; // loop through to see if this start location works for (i = iii; i < *size; ++i) { - if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){ - lastBit+=*clk; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - } else { - //mid value found or no bar supposed to be here - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - - //debug - //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit); - - errCnt++; - lastBit+=*clk;//skip over until hit too many errors - if (errCnt>(maxErr)) break; //allow 1 error for every 1000 samples else start over - } + if ((i-lastBit) > (*clk-tol) && (BinStream[i] >= high || BinStream[i] <= low)) { + lastBit += *clk; + } else if ((i-lastBit) > (*clk+tol)) { + errCnt++; + lastBit += *clk; } - if ((i-iii) >(MaxBits * *clk)) break; //got plenty of bits + if ((i-iii) > (MaxBits * *clk) || errCnt > maxErr) break; //got plenty of bits or too many errors } //we got more than 64 good bits and not all errors if ((((i-iii)/ *clk) > (64)) && (errCnt<=maxErr)) { //possible good read - if (errCnt==0){ - bestStart=iii; - bestErrCnt=errCnt; - break; //great read - finish - } - if (errCnt= high) && ((i-lastBit) > (*clk-tol))){ - lastBit += *clk; - BinStream[bitnum] = *invert; - bitnum++; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - BinStream[bitnum] = 1-*invert; - bitnum++; - } else { - //mid value found or no bar supposed to be here - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - - //debug - //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit); - if (bitnum > 0){ - BinStream[bitnum]=77; - bitnum++; - } - - lastBit+=*clk;//skip over error - } - } - if (bitnum >=MaxBits) break; - } - *size=bitnum; - } else{ - *invert=bestStart; - *clk=iii; + if (bestErrCnt > maxErr){ + *invert = bestStart; + *clk = iii; return -1; + } + //best run is good enough set to best run and set overwrite BinStream + lastBit = bestStart - *clk; + errCnt = 0; + for (i = bestStart; i < *size; ++i) { + if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){ + //high found and we are expecting a bar + lastBit += *clk; + BinStream[bitnum++] = *invert; + } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ + //low found and we are expecting a bar + lastBit += *clk; + BinStream[bitnum++] = *invert ^ 1; + } else if ((i-lastBit)>(*clk+tol)){ + //should have hit a high or low based on clock!! + //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit); + if (bitnum > 0) { + BinStream[bitnum++] = 77; + errCnt++; + } + lastBit += *clk;//skip over error + } + if (bitnum >= MaxBits) break; } + *size = bitnum; return bestErrCnt; } @@ -266,39 +232,30 @@ int manrawdecode(uint8_t * BitStream, size_t *size) uint16_t bestErr = 1000, bestRun = 0; if (size == 0) return -1; for (ii=0;ii<2;++ii){ - i=0; - for (i=i+ii;i<*size-2;i+=2){ - if(BitStream[i]==1 && (BitStream[i+1]==0)){ - } else if((BitStream[i]==0)&& BitStream[i+1]==1){ - } else { + for (i=ii; i<*size-2; i+=2) + if (BitStream[i]==BitStream[i+1]) errCnt++; - } - if(bitnum>MaxBits) break; - } + if (bestErr>errCnt){ bestErr=errCnt; bestRun=ii; } errCnt=0; } - errCnt=bestErr; - if (errCnt<20){ - ii=bestRun; - i=0; - for (i=i+ii; i < *size-2; i+=2){ + if (bestErr<20){ + for (i=bestRun; i < *size-2; i+=2){ if(BitStream[i] == 1 && (BitStream[i+1] == 0)){ BitStream[bitnum++]=0; } else if((BitStream[i] == 0) && BitStream[i+1] == 1){ BitStream[bitnum++]=1; } else { BitStream[bitnum++]=77; - //errCnt++; } if(bitnum>MaxBits) break; } *size=bitnum; } - return errCnt; + return bestErr; } //by marshmellow @@ -307,9 +264,9 @@ int manrawdecode(uint8_t * BitStream, size_t *size) //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) { - uint16_t bitnum=0; - uint32_t errCnt =0; - size_t i=offset; + uint16_t bitnum = 0; + uint16_t errCnt = 0; + size_t i = offset; uint16_t MaxBits=512; //if not enough samples - error if (*size < 51) return -1; @@ -345,7 +302,7 @@ void askAmp(uint8_t *BitStream, size_t size) { int shift = 127; int shiftedVal=0; - for(int i = 1; i=30) //large jump up shift=127; else if(BitStream[i]-BitStream[i-1]<=-20) //large jump down @@ -362,6 +319,7 @@ void askAmp(uint8_t *BitStream, size_t size) return; } +// demodulates strong heavily clipped samples int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low) { size_t bitCnt=0, smplCnt=0, errCnt=0; @@ -417,159 +375,114 @@ int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int //attempts to demodulate ask only int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp) { - uint32_t i; if (*size==0) return -1; int start = DetectASKClock(BinStream, *size, clk, 20); //clock default - if (*clk==0) return -1; - if (start<0) return -1; - if (*invert != 0 && *invert != 1) *invert =0; + if (*clk==0 || start < 0) return -1; + if (*invert != 1) *invert = 0; if (amp==1) askAmp(BinStream, *size); - uint32_t initLoopMax = 200; + uint8_t initLoopMax = 255; if (initLoopMax > *size) initLoopMax=*size; // Detect high and lows //25% clip in case highs and lows aren't clipped [marshmellow] - uint8_t clip = 75; - int high, low, ans; - ans = getHiLo(BinStream, initLoopMax, &high, &low, clip, clip); - if (ans<1) return -1; //just noise + int high, low; + if (getHiLo(BinStream, initLoopMax, &high, &low, 75, 75) < 1) + return -1; //just noise - if (DetectCleanAskWave(BinStream, *size, high, low)) { - //PrintAndLog("Clean"); + // if clean clipped waves detected run alternate demod + if (DetectCleanAskWave(BinStream, *size, high, low)) return cleanAskRawDemod(BinStream, size, *clk, *invert, high, low); - } - //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low); - int lastBit = 0; //set first clock check - uint32_t bitnum = 0; //output counter - uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock - // if they fall + or - this value + clock from last valid wave - if (*clk == 32) tol=0; //clock tolerance may not be needed anymore currently set to - // + or - 1 but could be increased for poor waves or removed entirely - uint32_t iii = 0; - uint32_t gLen = *size; - if (gLen > 500) gLen=500; - //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance - if (!maxErr) gLen = *clk * 2; - uint8_t errCnt =0; - uint32_t bestStart = *size; - uint32_t bestErrCnt = maxErr; //(*size/1000); - uint8_t midBit=0; - uint16_t MaxBits=1000; - - //PrintAndLog("DEBUG - lastbit - %d",lastBit); - //loop to find first wave that works - for (iii=start; iii < gLen; ++iii){ - if ((BinStream[iii]>=high) || (BinStream[iii]<=low)){ - lastBit=iii-*clk; - errCnt=0; - //loop through to see if this start location works - for (i = iii; i < *size; ++i) { - if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){ - lastBit+=*clk; - midBit=0; - } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - midBit=0; - } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){ - //no mid bar found - midBit=1; - } else { - //mid value found or no bar supposed to be here + int lastBit = 0; //set first clock check - can go negative + size_t i, iii = 0; + size_t errCnt = 0, bitnum = 0; //output counter + uint8_t midBit = 0; + size_t bestStart = start, bestErrCnt = 0; //(*size/1000); + size_t MaxBits = 1024; - if ((i-lastBit)>(*clk+tol)){ + //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance + if (!maxErr) initLoopMax = *clk * 2; + //if best start not already found by detectclock + if (start <= 0 || start > initLoopMax){ + bestErrCnt = maxErr+1; + //PrintAndLog("DEBUG - lastbit - %d",lastBit); + //loop to find first wave that works + for (iii=0; iii < initLoopMax; ++iii){ + if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){ + lastBit = iii - *clk; + //loop through to see if this start location works + for (i = iii; i < *size; ++i) { + if (i-lastBit > *clk && (BinStream[i] >= high || BinStream[i] <= low)){ + lastBit += *clk; + midBit = 0; + } else if (i-lastBit > (*clk/2) && midBit == 0) { + midBit = 1; + } else if ((i-lastBit) > *clk) { //should have hit a high or low based on clock!! - //debug //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit); - errCnt++; - lastBit+=*clk;//skip over until hit too many errors - if (errCnt > maxErr){ - //errCnt=0; + lastBit += *clk;//skip over until hit too many errors + if (errCnt > maxErr) break; - } } + if ((i-iii)>(MaxBits * *clk)) break; //got enough bits } - if ((i-iii)>(MaxBits * *clk)) break; //got enough bits - } - //we got more than 64 good bits and not all errors - if ((((i-iii)/ *clk) > (64)) && (errCnt<=maxErr)) { - //possible good read - if (errCnt==0){ - bestStart=iii; - bestErrCnt=errCnt; - break; //great read - finish - } - if (errCnt 64) && (errCnt<=maxErr)) { + //possible good read + if (errCnt==0){ + bestStart=iii; + bestErrCnt=errCnt; + break; //great read - finish + } + if (errCnt= high) && ((i-lastBit) > (*clk-tol))){ - lastBit += *clk; - BinStream[bitnum] = *invert; - bitnum++; - midBit=0; - } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){ - //low found and we are expecting a bar - lastBit+=*clk; - BinStream[bitnum] = 1 - *invert; - bitnum++; - midBit=0; - } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - BinStream[bitnum] = 1 - *invert; - bitnum++; - } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){ - //mid bar? - midBit=1; - BinStream[bitnum] = *invert; - bitnum++; - } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){ - //no mid bar found - midBit=1; - if (bitnum!=0) BinStream[bitnum] = BinStream[bitnum-1]; - bitnum++; - + if (bestErrCnt > maxErr){ + *invert = bestStart; + *clk = iii; + return -1; + } + //best run is good enough - set to best run and overwrite BinStream + lastBit = bestStart - *clk - 1; + errCnt = 0; + + for (i = bestStart; i < *size; ++i) { + if (i - lastBit > *clk){ + if (BinStream[i] >= high) { + BinStream[bitnum++] = *invert; + } else if (BinStream[i] <= low) { + BinStream[bitnum++] = *invert ^ 1; } else { - //mid value found or no bar supposed to be here - if ((i-lastBit)>(*clk+tol)){ - //should have hit a high or low based on clock!! - - //debug - //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit); - if (bitnum > 0){ - BinStream[bitnum]=77; - bitnum++; - } - lastBit+=*clk;//skip over error - } + if (bitnum > 0) { + BinStream[bitnum++]=77; + errCnt++; + } + } + midBit = 0; + lastBit += *clk; + } else if (i-lastBit > (*clk/2) && midBit == 0){ + if (BinStream[i] >= high) { + BinStream[bitnum++] = *invert; + } else if (BinStream[i] <= low) { + BinStream[bitnum++] = *invert ^ 1; + } else { + + BinStream[bitnum] = BinStream[bitnum-1]; + bitnum++; } - if (bitnum >= MaxBits) break; + midBit = 1; } - *size=bitnum; - } else{ - *invert=bestStart; - *clk=iii; - return -1; + if (bitnum >= MaxBits) break; } - return bestErrCnt; + *size = bitnum; + return errCnt; } // demod gProxIIDemod @@ -596,8 +509,8 @@ int gProxII_Demod(uint8_t BitStream[], size_t *size) //translate wave to 11111100000 (1 for each short wave 0 for each long wave) size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow) { - uint32_t last_transition = 0; - uint32_t idx = 1; + size_t last_transition = 0; + size_t idx = 1; //uint32_t maxVal=0; if (fchigh==0) fchigh=10; if (fclow==0) fclow=8; @@ -626,82 +539,64 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow if ((idx-last_transition)<(fclow-2)){ //0-5 = garbage noise //do nothing with extra garbage } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves - dest[numBits]=1; + dest[numBits++]=1; } else if ((idx-last_transition) > (fchigh+1) && !numBits) { //12 + and first bit = garbage //do nothing with beginning garbage } else { //9+ = 10 waves - dest[numBits]=0; + dest[numBits++]=0; } last_transition = idx; - numBits++; } } return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0 } -uint32_t myround2(float f) -{ - if (f >= 2000) return 2000;//something bad happened - return (uint32_t) (f + (float)0.5); -} - //translate 11111100000 to 10 -size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxConsequtiveBits, +size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow) { uint8_t lastval=dest[0]; - uint32_t idx=0; + size_t idx=0; size_t numBits=0; uint32_t n=1; - float lowWaves = (((float)(rfLen))/((float)fclow)); - float highWaves = (((float)(rfLen))/((float)fchigh)); + uint16_t lowWaves = ((rfLen*100/fclow)); // (((float)(rfLen))/((float)fclow)); + uint16_t highWaves = ((rfLen*100/fchigh)); // (((float)(rfLen))/((float)fchigh)); for( idx=1; idx < size; idx++) { - - if (dest[idx]==lastval) { - n++; - continue; - } n++; + if (dest[idx]==lastval) continue; + //if lastval was 1, we have a 1->0 crossing if (dest[idx-1]==1) { - if (!numBits && n < (uint8_t)lowWaves) { + if (!numBits && n < lowWaves/100) { n=0; lastval = dest[idx]; continue; } - n=myround2(((float)n)/lowWaves); + n = (size_t)((((n*1000)/lowWaves)+5)/10); } else {// 0->1 crossing //test first bitsample too small - if (!numBits && n < (uint8_t)highWaves) { + if (!numBits && n < highWaves/100) { n=0; lastval = dest[idx]; continue; } - n = myround2(((float)n)/highWaves); //-1 for fudge factor + n = (((n*1000)/highWaves)+5)/10; } if (n == 0) n = 1; - if(n < maxConsequtiveBits) //Consecutive - { - if(invert==0){ //invert bits - memset(dest+numBits, dest[idx-1] , n); - }else{ - memset(dest+numBits, dest[idx-1]^1 , n); - } - numBits += n; - } + memset(dest+numBits, dest[idx-1]^invert , n); + numBits += n; n=0; lastval=dest[idx]; }//end for - // if valid extra bits at the end were all the same frequency - add them in - if (n > lowWaves && n > highWaves) { + if (n > highWaves/100) { if (dest[idx-2]==1) { - n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow)); + n=(((n*1000)/lowWaves)+5)/10; } else { - n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh)); //-1 for fudge factor + n=(((n*1000)/highWaves)+5)/10; } - memset(dest, dest[idx-1]^invert , n); + memset(dest+numBits, dest[idx-1]^invert , n); numBits += n; } return numBits; @@ -712,7 +607,7 @@ int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t { // FSK demodulator size = fsk_wave_demod(dest, size, fchigh, fclow); - size = aggregate_bits(dest, size, rfLen, 192, invert, fchigh, fclow); + size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow); return size; } @@ -724,7 +619,7 @@ int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32 size_t numStart=0, size2=*size, startIdx=0; // FSK demodulator *size = fskdemod(dest, size2,50,1,10,8); //fsk2a - if (*size < 96) return -2; + if (*size < 96*2) return -2; // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1 uint8_t preamble[] = {0,0,0,1,1,1,0,1}; // find bitstring in array @@ -906,9 +801,12 @@ uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, int high, int low) return allPeaks; } +// by marshmellow +// to help detect clocks on heavily clipped samples +// based on counts between zero crossings int DetectStrongAskClock(uint8_t dest[], size_t size) { - int clk[]={0,8,16,32,40,50,64,100,128,256}; + int clk[]={0,8,16,32,40,50,64,100,128}; size_t idx = 40; uint8_t high=0; size_t cnt = 0; @@ -960,11 +858,10 @@ int DetectStrongAskClock(uint8_t dest[], size_t size) // return start index of best starting position for that clock and return clock (by reference) int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) { - int i=0; - int clk[]={8,16,32,40,50,64,100,128,256}; - int loopCnt = 256; //don't need to loop through entire array... - if (size == 0) return -1; - if (size0; i--){ if (clk[i] == ans) { - *clock=ans; + *clock = ans; return 0; } } } - int ii; - int clkCnt; - int tol = 0; - int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000}; - int bestStart[]={0,0,0,0,0,0,0,0,0}; - int errCnt=0; + uint8_t ii; + uint8_t clkCnt, tol = 0; + uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000}; + uint8_t bestStart[]={0,0,0,0,0,0,0,0,0}; + size_t errCnt = 0; + size_t arrLoc, loopEnd; //test each valid clock from smallest to greatest to see which lines up for(clkCnt=0; clkCnt < 8; clkCnt++){ if (clk[clkCnt] == 32){ @@ -1001,45 +898,47 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) bestErr[clkCnt]=1000; //try lining up the peaks by moving starting point (try first 256) for (ii=0; ii < loopCnt; ii++){ - if ((dest[ii] >= peak) || (dest[ii] <= low)){ - errCnt=0; - // now that we have the first one lined up test rest of wave array - for (i=0; i<((int)((size-ii-tol)/clk[clkCnt])-1); ++i){ - if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){ - }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){ - }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){ - }else{ //error no peak detected - errCnt++; - } - } - //if we found no errors then we can stop here - // this is correct one - return this clock - //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i); - if(errCnt==0 && clkCnt<6) { - *clock = clk[clkCnt]; - return ii; - } - //if we found errors see if it is lowest so far and save it as best run - if(errCnt low) continue; + + 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){ + arrLoc = ii + (i * clk[clkCnt]); + if (dest[arrLoc] >= peak || dest[arrLoc] <= low){ + }else if (dest[arrLoc-tol] >= peak || dest[arrLoc-tol] <= low){ + }else if (dest[arrLoc+tol] >= peak || dest[arrLoc+tol] <= low){ + }else{ //error no peak detected + errCnt++; } } + //if we found no errors then we can stop here + // this is correct one - return this clock + //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i); + if(errCnt==0 && clkCnt<6) { + *clock = clk[clkCnt]; + return ii; + } + //if we found errors see if it is lowest so far and save it as best run + if(errCntmaxErr) return -1; - *clock=clk[best]; + if (bestErr[best] > maxErr) return -1; + *clock = clk[best]; return bestStart[best]; } @@ -1063,7 +962,8 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) uint16_t peakcnt=0, errCnt=0, waveLenCnt=0; uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000}; uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0}; - countFC(dest, size, &fc); + fc = countFC(dest, size, 0); + if (fc!=2 && fc!=4 && fc!=8) return -1; //PrintAndLog("DEBUG: FC: %d",fc); //find first full wave @@ -1144,9 +1044,9 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) //detect nrz clock by reading #peaks vs no peaks(or errors) int DetectNRZClock(uint8_t dest[], size_t size, int clock) { - int i=0; - int clk[]={8,16,32,40,50,64,100,128,256}; - int loopCnt = 4096; //don't need to loop through entire array... + 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... if (size == 0) return 0; if (size= peak || dest[i] <= low){ @@ -1199,7 +1099,7 @@ int DetectNRZClock(uint8_t dest[], size_t size, int clock) } } int iii=7; - int best=0; + uint8_t best=0; for (iii=7; iii > 0; iii--){ if (peaksdet[iii] > peaksdet[best]){ best = iii; @@ -1316,28 +1216,20 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr) if (justNoise(dest, *size)) return -1; *clk = DetectNRZClock(dest, *size, *clk); if (*clk==0) return -2; - uint32_t i; - uint32_t gLen = 4096; + size_t i, gLen = 4096; if (gLen>*size) gLen = *size; int high, low; if (getHiLo(dest, gLen, &high, &low, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low int lastBit = 0; //set first clock check - uint32_t bitnum = 0; //output counter + size_t iii = 0, bitnum = 0; //bitnum counter + uint16_t errCnt = 0, MaxBits = 1000; + size_t bestErrCnt = maxErr+1; + size_t bestPeakCnt = 0, bestPeakStart = 0; + uint8_t bestFirstPeakHigh=0, firstPeakHigh=0, curBit=0, bitHigh=0, errBitHigh=0; uint8_t tol = 1; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave - uint32_t iii = 0; - uint16_t errCnt =0; - uint16_t MaxBits = 1000; - uint32_t bestErrCnt = maxErr+1; - uint32_t bestPeakCnt = 0; - uint32_t bestPeakStart=0; - uint8_t bestFirstPeakHigh=0; - uint8_t firstPeakHigh=0; - uint8_t curBit=0; - uint8_t bitHigh=0; - uint8_t errBitHigh=0; uint16_t peakCnt=0; uint8_t ignoreWindow=4; - uint8_t ignoreCnt=ignoreWindow; //in case of noice near peak + uint8_t ignoreCnt=ignoreWindow; //in case of noise near peak //loop to find first wave that works - align to clock for (iii=0; iii < gLen; ++iii){ if ((dest[iii]>=high) || (dest[iii]<=low)){ @@ -1346,135 +1238,93 @@ int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr) lastBit=iii-*clk; peakCnt=0; errCnt=0; - bitnum=0; //loop through to see if this start location works for (i = iii; i < *size; ++i) { - //if we found a high bar and we are at a clock bit - if ((dest[i]>=high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - bitnum++; - peakCnt++; - errBitHigh=0; - ignoreCnt=ignoreWindow; - //else if low bar found and we are at a clock point - }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - bitnum++; - peakCnt++; - errBitHigh=0; - ignoreCnt=ignoreWindow; + // if we are at a clock bit + if ((i >= lastBit + *clk - tol) && (i <= lastBit + *clk + tol)) { + //test high/low + if (dest[i] >= high || dest[i] <= low) { + bitHigh = 1; + peakCnt++; + errBitHigh = 0; + ignoreCnt = ignoreWindow; + lastBit += *clk; + } else if (i == lastBit + *clk + tol) { + lastBit += *clk; + } //else if no bars found - }else if(dest[i] < high && dest[i] > low) { + } else if (dest[i] < high && dest[i] > low){ if (ignoreCnt==0){ bitHigh=0; - if (errBitHigh==1){ - errCnt++; - } + if (errBitHigh==1) errCnt++; errBitHigh=0; } else { ignoreCnt--; } - //if we are past a clock point - if (i >= lastBit+*clk+tol){ //clock val - lastBit+=*clk; - bitnum++; - } - //else if bar found but we are not at a clock bit and we did not just have a clock bit - }else if ((dest[i]>=high || dest[i]<=low) && (ilastBit+*clk+tol) && (bitHigh==0)){ + } else if ((dest[i]>=high || dest[i]<=low) && (bitHigh==0)) { //error bar found no clock... errBitHigh=1; } - if (bitnum>=MaxBits) break; + if (((i-iii) / *clk)>=MaxBits) break; } //we got more than 64 good bits and not all errors - if (bitnum > (64) && (errCnt <= (maxErr))) { + if (((i-iii) / *clk) > 64 && (errCnt <= (maxErr))) { //possible good read - if (errCnt == 0){ - //bestStart = iii; + if (!errCnt || peakCnt > bestPeakCnt){ bestFirstPeakHigh=firstPeakHigh; bestErrCnt = errCnt; bestPeakCnt = peakCnt; bestPeakStart = iii; - break; //great read - finish - } - if (errCnt < bestErrCnt){ //set this as new best run - bestErrCnt = errCnt; - //bestStart = iii; + if (!errCnt) break; //great read - finish } - if (peakCnt > bestPeakCnt){ - bestFirstPeakHigh=firstPeakHigh; - bestPeakCnt=peakCnt; - bestPeakStart=iii; - } } } } //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart); - if (bestErrCnt <= maxErr){ - //best run is good enough set to best run and set overwrite BinStream - iii=bestPeakStart; - lastBit=bestPeakStart-*clk; - bitnum=0; - memset(dest, bestFirstPeakHigh^1, bestPeakStart / *clk); - bitnum += (bestPeakStart / *clk); - for (i = iii; i < *size; ++i) { - //if we found a high bar and we are at a clock bit - if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - curBit=1-*invert; - dest[bitnum]=curBit; - bitnum++; - errBitHigh=0; - ignoreCnt=ignoreWindow; - //else if low bar found and we are at a clock point - }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){ - bitHigh=1; - lastBit+=*clk; - curBit=*invert; - dest[bitnum]=curBit; - bitnum++; - errBitHigh=0; - ignoreCnt=ignoreWindow; - //else if no bars found - }else if(dest[i]low) { - if (ignoreCnt==0){ - bitHigh=0; - //if peak is done was it an error peak? - if (errBitHigh==1){ - dest[bitnum]=77; - bitnum++; - errCnt++; - } - errBitHigh=0; - } else { - ignoreCnt--; - } - //if we are past a clock point - if (i>=lastBit+*clk+tol){ //clock val - lastBit+=*clk; - dest[bitnum]=curBit; - bitnum++; + if (bestErrCnt > maxErr) return bestErrCnt; + + //best run is good enough set to best run and set overwrite BinStream + lastBit = bestPeakStart - *clk; + memset(dest, bestFirstPeakHigh^1, bestPeakStart / *clk); + bitnum += (bestPeakStart / *clk); + for (i = bestPeakStart; i < *size; ++i) { + // if expecting a clock bit + if ((i >= lastBit + *clk - tol) && (i <= lastBit + *clk + tol)) { + // test high/low + if (dest[i] >= high || dest[i] <= low) { + peakCnt++; + bitHigh = 1; + errBitHigh = 0; + ignoreCnt = ignoreWindow; + curBit = *invert; + if (dest[i] >= high) curBit ^= 1; + dest[bitnum++] = curBit; + lastBit += *clk; + //else no bars found in clock area + } else if (i == lastBit + *clk + tol) { + dest[bitnum++] = curBit; + lastBit += *clk; + } + //else if no bars found + } else if (dest[i] < high && dest[i] > low){ + if (ignoreCnt == 0){ + bitHigh = 0; + if (errBitHigh == 1){ + dest[bitnum++] = 77; + errCnt++; } - //else if bar found but we are not at a clock bit and we did not just have a clock bit - }else if ((dest[i]>=high || dest[i]<=low) && ((ilastBit+*clk+tol)) && (bitHigh==0)){ - //error bar found no clock... - errBitHigh=1; + errBitHigh=0; + } else { + ignoreCnt--; } - if (bitnum >= MaxBits) break; + } else if ((dest[i] >= high || dest[i] <= low) && (bitHigh == 0)) { + //error bar found no clock... + errBitHigh=1; } - *size=bitnum; - } else{ - *size=bitnum; - return bestErrCnt; + if (bitnum >= MaxBits) break; } - - if (bitnum>16){ - *size=bitnum; - } else return -5; - return errCnt; + *size = bitnum; + return bestErrCnt; } //by marshmellow @@ -1485,8 +1335,8 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc 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 lastFCcnt=0; - uint32_t fcCounter = 0; + uint8_t lastFCcnt = 0; + uint16_t fcCounter = 0; uint16_t rfCounter = 0; uint8_t firstBitFnd = 0; size_t i; @@ -1504,45 +1354,41 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc break; for (; i < size-1; i++){ - if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1]){ - // new peak - fcCounter++; - rfCounter++; - // 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<15; ii++){ - if (rfLens[ii]==rfCounter){ - rfCnts[ii]++; - rfCounter=0; - break; - } - } - if (rfCounter>0 && rfLensFnd<15){ - //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); - rfCnts[rfLensFnd]++; - rfLens[rfLensFnd++]=rfCounter; + fcCounter++; + rfCounter++; + + if (BitStream[i] <= BitStream[i-1] || BitStream[i] < BitStream[i+1]) + continue; + // else new peak + // 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 ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)){ + //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; } - } else { - firstBitFnd++; } - rfCounter=0; - lastFCcnt=fcCounter; + if (rfCounter > 0 && rfLensFnd < 15){ + //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); + rfCnts[rfLensFnd]++; + rfLens[rfLensFnd++] = rfCounter; + } + } else { + firstBitFnd++; } - fcCounter=0; - } else { - // count sample - fcCounter++; - rfCounter++; + rfCounter=0; + lastFCcnt=fcCounter; } + fcCounter=0; } uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15; @@ -1588,13 +1434,13 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc //countFC is to detect the field clock lengths. //counts and returns the 2 most common wave lengths //mainly used for FSK field clock detection -uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC) +uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) { 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; + uint8_t fcCounter = 0; size_t i; if (size == 0) return 0; @@ -1607,13 +1453,14 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC) 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++; - + if (fskAdj){ + //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 fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5) + if ((fcCounter==9) || fcCounter==4) fcCounter++; // save last field clock count (fc/xx) + lastFCcnt = fcCounter; + } // find which fcLens to save it to: for (int ii=0; ii<10; ii++){ if (fcLens[ii]==fcCounter){ @@ -1661,89 +1508,31 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC) fcL=fcLens[best1]; } - *mostFC=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; -} - -//by marshmellow -//countPSK_FC is to detect the psk carrier clock length. -//counts and returns the 1 most common wave length -uint8_t countPSK_FC(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; - uint32_t fcCounter = 0; - size_t i; - if (size == 0) return 0; - - // 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++; - - // 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; - uint16_t maxCnt1=0; - // go through fclens and find which ones are bigest - for (i=0; i<10; i++){ - //PrintAndLog("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]); - // get the best FC value - if (fcCnts[i]>maxCnt1) { - maxCnt1=fcCnts[i]; - best1=i; - } - } - return fcLens[best1]; + if (fskAdj) return fcs; + return fcLens[best1]; } //by marshmellow - demodulate PSK1 wave //uses wave lengths (# Samples) int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) { - uint16_t loopCnt = 4096; //don't need to loop through entire array... if (size == 0) return -1; + uint16_t loopCnt = 4096; //don't need to loop through entire array... if (*size graph 0 then it is an up wave or a 1 - if (lastAvgWaveVal > 123) curPhase^=1; //fudge graph 0 a little 123 vs 128 + if (lastAvgWaveVal > 123) curPhase ^= 1; //fudge graph 0 a little 123 vs 128 break; } waveStart = i+1; avgWaveVal = 0; } - avgWaveVal+=dest[i+2]; + avgWaveVal += dest[i+2]; } //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); lastClkBit = firstFullWave; //set start of wave as clock align //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit); waveStart = 0; - errCnt=0; size_t numBits=0; //set skipped bits - memset(dest,curPhase^1,firstFullWave / *clock); + memset(dest, curPhase^1, firstFullWave / *clock); numBits += (firstFullWave / *clock); dest[numBits++] = curPhase; //set first read bit - for (i = firstFullWave+fullWaveLen-1; i < *size-3; i++){ + for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){ //top edge of wave = start of new wave if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){ if (waveStart == 0) { waveStart = i+1; - waveLenCnt=0; + waveLenCnt = 0; avgWaveVal = dest[i+1]; } else { //waveEnd waveEnd = i+1; @@ -1787,13 +1575,13 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) lastAvgWaveVal = avgWaveVal/waveLenCnt; if (waveLenCnt > fc){ //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal); - //if this wave is a phase shift + //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 - curPhase^=1; + curPhase ^= 1; dest[numBits++] = curPhase; lastClkBit += *clock; - } else if (i