From 30ceea4aeacc7c6eb9a88f194d81345a6efecef8 Mon Sep 17 00:00:00 2001 From: iceman1001 Date: Thu, 9 Nov 2017 19:15:56 +0100 Subject: [PATCH] chg: cleaning up some function calls. fix: check to see clock is bigger than zero. --- client/cmddata.c | 47 +++++++++++++++++++------ client/cmdlf.c | 12 +++---- client/cmdlfem4x.c | 4 +-- client/cmdlfhitag.c | 4 +-- client/cmdlft55xx.c | 16 ++++----- client/graph.c | 84 +++++++++++++++++++++------------------------ client/graph.h | 12 +++---- common/lfdemod.c | 20 +++++++---- 8 files changed, 114 insertions(+), 85 deletions(-) diff --git a/client/cmddata.c b/client/cmddata.c index 19aaf212e..d84da9474 100644 --- a/client/cmddata.c +++ b/client/cmddata.c @@ -12,8 +12,8 @@ uint8_t DemodBuffer[MAX_DEMOD_BUF_LEN]; uint8_t g_debugMode = 0; size_t DemodBufferLen = 0; -int g_DemodStartIdx=0; -int g_DemodClock=0; +int g_DemodStartIdx = 0; +int g_DemodClock = 0; static int CmdHelp(const char *Cmd); @@ -26,7 +26,6 @@ int usage_data_printdemodbuf(void){ PrintAndLog(" l enter length to print in # of bits or hex characters respectively"); return 0; } - int usage_data_manrawdecode(void){ PrintAndLog("Usage: data manrawdecode [invert] [maxErr]"); PrintAndLog(" Takes 10 and 01 and converts to 0 and 1 respectively"); @@ -225,7 +224,6 @@ int usage_data_buffclear(void){ PrintAndLog(" h This help"); return 0; } - int usage_data_fsktonrz() { PrintAndLog("Usage: data fsktonrz c l f "); PrintAndLog("Options: "); @@ -236,7 +234,6 @@ int usage_data_fsktonrz() { return 0; } - //set the demod buffer with given array of binary (one bit per byte) //by marshmellow void setDemodBuf(uint8_t *buf, size_t size, size_t startIdx) { @@ -262,6 +259,36 @@ bool getDemodBuf(uint8_t *buf, size_t *size) { return true; } +// include +// Root mean square +double rms(double *v, int n) { + double sum = 0.0; + for(int i = 0; i < n; i++) + sum += v[i] * v[i]; + return sqrt(sum / n); +} +int cmp_int( const void *a, const void *b) { + if (*(const int *)a < *(const int *)b) + return -1; + else + return *(const int *)a > *(const int *)b; +} +int cmp_uint8( const void *a, const void *b) { + if (*(const uint8_t *)a < *(const uint8_t *)b) + return -1; + else + return *(const uint8_t *)a > *(const uint8_t *)b; +} +// Median of a array of values +double median_int( int *src, size_t size ) { + qsort( src, size, sizeof(int), cmp_int); + return 0.5 * ( src[size/2] + src[(size-1)/2]); +} +double median_uint8( uint8_t *src, size_t size ) { + qsort( src, size, sizeof(uint8_t), cmp_uint8); + return 0.5 * ( src[size/2] + src[(size-1)/2]); +} + // option '1' to save DemodBuffer any other to restore void save_restoreDB(uint8_t saveOpt) { static uint8_t SavedDB[MAX_DEMOD_BUF_LEN]; @@ -843,19 +870,19 @@ int CmdDetectClockRate(const char *Cmd) switch ( cmdp ) { case 'a' : case 'A' : - clock = GetAskClock(Cmd+1, true, false); + clock = GetAskClock(Cmd+1, true); break; case 'f' : case 'F' : - clock = GetFskClock("", true, false); + clock = GetFskClock("", true); break; case 'n' : case 'N' : - clock = GetNrzClock("", true, false); + clock = GetNrzClock("", true); break; case 'p' : case 'P' : - clock = GetPskClock("", true, false); + clock = GetPskClock("", true); break; default : PrintAndLog ("Please specify a valid modulation to detect the clock of - see option h for help"); @@ -1791,7 +1818,7 @@ int FSKToNRZ(int *data, int *dataLen, int clk, int LowToneFC, int HighToneFC) { uint8_t ans=0; if (clk == 0 || LowToneFC == 0 || HighToneFC == 0) { int firstClockEdge=0; - ans = fskClocks((uint8_t *) &LowToneFC, (uint8_t *) &HighToneFC, (uint8_t *) &clk, false, &firstClockEdge); + ans = fskClocks((uint8_t *) &LowToneFC, (uint8_t *) &HighToneFC, (uint8_t *) &clk, &firstClockEdge); if (g_debugMode > 1) { PrintAndLog ("DEBUG FSKtoNRZ: detected clocks: fc_low %i, fc_high %i, clk %i, firstClockEdge %i, ans %u", LowToneFC, HighToneFC, clk, firstClockEdge, ans); } diff --git a/client/cmdlf.c b/client/cmdlf.c index 35b2af55c..0b3930620 100644 --- a/client/cmdlf.c +++ b/client/cmdlf.c @@ -504,7 +504,7 @@ int CmdLFfskSim(const char *Cmd) { int firstClockEdge = 0; if (dataLen == 0){ //using DemodBuffer if (clk == 0 || fcHigh == 0 || fcLow == 0){ //manual settings must set them all - uint8_t ans = fskClocks(&fcHigh, &fcLow, &clk, 0, &firstClockEdge); + uint8_t ans = fskClocks(&fcHigh, &fcLow, &clk, &firstClockEdge); if (ans==0){ if (!fcHigh) fcHigh = 10; if (!fcLow) fcLow = 8; @@ -604,7 +604,7 @@ int CmdLFaskSim(const char *Cmd) { if (dataLen == 0){ //using DemodBuffer if (clk == 0) - clk = GetAskClock("0", false, false); + clk = GetAskClock("0", false); } else { setDemodBuf(data, dataLen, 0); } @@ -700,10 +700,10 @@ int CmdLFpskSim(const char *Cmd) { if (dataLen == 0){ //using DemodBuffer PrintAndLog("Getting Clocks"); - if (clk==0) clk = GetPskClock("", false, false); + if (clk==0) clk = GetPskClock("", false); PrintAndLog("clk: %d",clk); - if (!carrier) carrier = GetPskCarrier("", false, false); + if (!carrier) carrier = GetPskCarrier("", false); PrintAndLog("carrier: %d", carrier); } else { @@ -943,8 +943,8 @@ int CmdLFfind(const char *Cmd) { } //fsk - if ( GetFskClock("",false,false) ) { - if ( FSKrawDemod("",true) ) { + if ( GetFskClock("", false) ) { + if ( FSKrawDemod("", true) ) { PrintAndLog("\nUnknown FSK Modulated Tag Found!"); goto out; } } diff --git a/client/cmdlfem4x.c b/client/cmdlfem4x.c index 507593c75..719bd5304 100644 --- a/client/cmdlfem4x.c +++ b/client/cmdlfem4x.c @@ -1027,7 +1027,7 @@ bool doPreambleSearch(size_t *startIdx){ bool detectFSK(){ // detect fsk clock - if (!GetFskClock("", false, false)) { + if (!GetFskClock("", false)) { if (g_debugMode) PrintAndLog("DEBUG: Error - EM: FSK clock failed"); return false; } @@ -1041,7 +1041,7 @@ bool detectFSK(){ } // PSK clocks should be easy to detect ( but difficult to demod a non-repeating pattern... ) bool detectPSK(){ - int ans = GetPskClock("", false, false); + int ans = GetPskClock("", false); if (ans <= 0) { if (g_debugMode) PrintAndLog("DEBUG: Error - EM: PSK clock failed"); return false; diff --git a/client/cmdlfhitag.c b/client/cmdlfhitag.c index ab4ff1019..7172219cc 100644 --- a/client/cmdlfhitag.c +++ b/client/cmdlfhitag.c @@ -201,12 +201,12 @@ int CmdLFHitagReader(const char *Cmd) { switch (htf) { case 01: { //RHTSF_CHALLENGE - c = (UsbCommand){ CMD_READ_HITAG_S }; + c.cmd = CMD_READ_HITAG_S; num_to_bytes(param_get32ex(Cmd, 1, 0, 16), 4, htd->auth.NrAr); num_to_bytes(param_get32ex(Cmd, 2, 0, 16), 4, htd->auth.NrAr+4); } break; case 02: { //RHTSF_KEY - c = (UsbCommand){ CMD_READ_HITAG_S }; + c.cmd = CMD_READ_HITAG_S; num_to_bytes(param_get64ex(Cmd, 1, 0, 16), 6, htd->crypto.key); } break; case RHT2F_PASSWORD: { diff --git a/client/cmdlft55xx.c b/client/cmdlft55xx.c index 822e20fc8..663a6216e 100644 --- a/client/cmdlft55xx.c +++ b/client/cmdlft55xx.c @@ -511,7 +511,7 @@ bool tryDetectModulation(){ int bitRate=0; uint8_t fc1 = 0, fc2 = 0, ans = 0; int clk = 0, firstClockEdge = 0; - ans = fskClocks(&fc1, &fc2, (uint8_t *)&clk, false, &firstClockEdge); + ans = fskClocks(&fc1, &fc2, (uint8_t *)&clk, &firstClockEdge); if (ans && ((fc1==10 && fc2==8) || (fc1==8 && fc2==5))) { if ( FSKrawDemod("0 0", false) && test(DEMOD_FSK, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)){ tests[hits].modulation = DEMOD_FSK; @@ -538,7 +538,7 @@ bool tryDetectModulation(){ ++hits; } } else { - clk = GetAskClock("", false, false); + clk = GetAskClock("", false); if (clk>0) { tests[hits].ST = true; // "0 0 1 " == clock auto, invert false, maxError 1. @@ -583,7 +583,7 @@ bool tryDetectModulation(){ ++hits; } } - clk = GetNrzClock("", false, false); + clk = GetNrzClock("", false); if (clk>8) { //clock of rf/8 is likely a false positive, so don't use it. if ( NRZrawDemod("0 0 1", false) && test(DEMOD_NRZ, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_NRZ; @@ -604,7 +604,7 @@ bool tryDetectModulation(){ } } - clk = GetPskClock("", false, false); + clk = GetPskClock("", false); if (clk>0) { // allow undo save_restoreGB(GRAPH_SAVE); @@ -1753,7 +1753,7 @@ bool tryDetectP1(bool getData) { } // try fsk clock detect. if successful it cannot be any other type of modulation... (in theory...) - ans = fskClocks(&fc1, &fc2, (uint8_t *)&clk, false, &firstClockEdge); + ans = fskClocks(&fc1, &fc2, (uint8_t *)&clk, &firstClockEdge); if (ans && ((fc1==10 && fc2==8) || (fc1==8 && fc2==5))) { if ( FSKrawDemod("0 0", false) && preambleSearchEx(DemodBuffer, preamble,sizeof(preamble), &DemodBufferLen, &startIdx, false) && @@ -1769,7 +1769,7 @@ bool tryDetectP1(bool getData) { } // try psk clock detect. if successful it cannot be any other type of modulation... (in theory...) - clk = GetPskClock("", false, false); + clk = GetPskClock("", false); if (clk > 0) { // allow undo // save_restoreGB(1); @@ -1803,7 +1803,7 @@ bool tryDetectP1(bool getData) { } // try ask clock detect. it could be another type even if successful. - clk = GetAskClock("", false, false); + clk = GetAskClock("", false); if (clk>0) { if ( ASKDemod_ext("0 0 1", false, false, 1, &st) && preambleSearchEx(DemodBuffer, preamble, sizeof(preamble), &DemodBufferLen, &startIdx, false) && @@ -1829,7 +1829,7 @@ bool tryDetectP1(bool getData) { } // try NRZ clock detect. it could be another type even if successful. - clk = GetNrzClock("", false, false); //has the most false positives :( + clk = GetNrzClock("", false); //has the most false positives :( if (clk > 0) { if ( NRZrawDemod("0 0 1", false) && preambleSearchEx(DemodBuffer, preamble, sizeof(preamble), &DemodBufferLen, &startIdx, false) && diff --git a/client/graph.c b/client/graph.c index acb81a3f6..b51a04f16 100644 --- a/client/graph.c +++ b/client/graph.c @@ -77,8 +77,9 @@ size_t getFromGraphBuf(uint8_t *buf) { if (buf == NULL ) return 0; uint32_t i; for (i=0; i < GraphTraceLen; ++i){ - if (GraphBuffer[i] > 127) GraphBuffer[i] = 127; //trim - if (GraphBuffer[i] < -127) GraphBuffer[i] = -127; //trim + //trim + if (GraphBuffer[i] > 127) GraphBuffer[i] = 127; + if (GraphBuffer[i] < -127) GraphBuffer[i] = -127; buf[i] = (uint8_t)(GraphBuffer[i] + 128); } return i; @@ -94,19 +95,17 @@ bool HasGraphData(){ } // Get or auto-detect ask clock rate -int GetAskClock(const char str[], bool printAns, bool verbose) { - int clock; - sscanf(str, "%i", &clock); - if (!strcmp(str, "")) - clock = 0; +int GetAskClock(const char *str, bool printAns) { - if (clock != 0) return clock; + int clock = param_get32ex(str, 0, 0, 10); + if (clock > 0) + return clock; // Auto-detect clock uint8_t grph[MAX_GRAPH_TRACE_LEN] = {0}; size_t size = getFromGraphBuf(grph); if (size == 0) { - if (verbose) + if (g_debugMode) PrintAndLog("Failed to copy from graphbuffer"); return -1; } @@ -119,18 +118,18 @@ int GetAskClock(const char str[], bool printAns, bool verbose) { } setClockGrid(clock, start); // Only print this message if we're not looping something - if (printAns || g_debugMode) { + if (printAns || g_debugMode) PrintAndLog("Auto-detected clock rate: %d, Best Starting Position: %d", clock, start); - } + return clock; } -uint8_t GetPskCarrier(const char str[], bool printAns, bool verbose) { +uint8_t GetPskCarrier(const char *str, bool printAns) { uint8_t carrier = 0; uint8_t grph[MAX_GRAPH_TRACE_LEN] = {0}; size_t size = getFromGraphBuf(grph); if ( size == 0 ) { - if (verbose) + if (g_debugMode) PrintAndLog("Failed to copy from graphbuffer"); return 0; } @@ -144,18 +143,16 @@ uint8_t GetPskCarrier(const char str[], bool printAns, bool verbose) { return carrier; } -int GetPskClock(const char str[], bool printAns, bool verbose) { - int clock; - sscanf(str, "%i", &clock); - if (!strcmp(str, "")) - clock = 0; - - if (clock != 0) return clock; +int GetPskClock(const char* str, bool printAns) { + int clock = param_get32ex(str, 0, 0, 10); + if (clock != 0) + return clock; + // Auto-detect clock uint8_t grph[MAX_GRAPH_TRACE_LEN] = {0}; size_t size = getFromGraphBuf(grph); if ( size == 0 ) { - if (verbose) PrintAndLog("Failed to copy from graphbuffer"); + if (g_debugMode) PrintAndLog("Failed to copy from graphbuffer"); return -1; } size_t firstPhaseShiftLoc = 0; @@ -163,25 +160,22 @@ int GetPskClock(const char str[], bool printAns, bool verbose) { clock = DetectPSKClock(grph, size, 0, &firstPhaseShiftLoc, &curPhase, &fc); setClockGrid(clock, firstPhaseShiftLoc); // Only print this message if we're not looping something - if (printAns){ + if (printAns) PrintAndLog("Auto-detected clock rate: %d", clock); - } return clock; } -uint8_t GetNrzClock(const char str[], bool printAns, bool verbose) { - int clock; - sscanf(str, "%i", &clock); - if (!strcmp(str, "")) - clock = 0; +int GetNrzClock(const char* str, bool printAns) { + int clock = param_get32ex(str, 0, 0, 10); if (clock != 0) return clock; + // Auto-detect clock uint8_t grph[MAX_GRAPH_TRACE_LEN] = {0}; size_t size = getFromGraphBuf(grph); if ( size == 0 ) { - if (verbose) + if (g_debugMode) PrintAndLog("Failed to copy from graphbuffer"); return -1; } @@ -189,44 +183,44 @@ uint8_t GetNrzClock(const char str[], bool printAns, bool verbose) { clock = DetectNRZClock(grph, size, 0, &clkStartIdx); setClockGrid(clock, clkStartIdx); // Only print this message if we're not looping something - if (printAns){ + if (printAns) PrintAndLog("Auto-detected clock rate: %d", clock); - } return clock; } //by marshmellow //attempt to detect the field clock and bit clock for FSK -uint8_t GetFskClock(const char str[], bool printAns, bool verbose) { - int clock; - sscanf(str, "%i", &clock); - if (!strcmp(str, "")) - clock = 0; - if (clock != 0) return (uint8_t)clock; +int GetFskClock(const char* str, bool printAns) { + int clock = param_get32ex(str, 0, 0, 10); + if (clock != 0) + return clock; - uint8_t fc1=0, fc2=0, rf1=0; + uint8_t fc1 = 0, fc2 = 0, rf1 = 0; int firstClockEdge = 0; - uint8_t ans = fskClocks(&fc1, &fc2, &rf1, verbose, &firstClockEdge); - if (ans == 0) return 0; + int ans = fskClocks(&fc1, &fc2, &rf1, &firstClockEdge); + if (ans == 0) + return 0; + if ((fc1==10 && fc2==8) || (fc1==8 && fc2==5)){ if (printAns) PrintAndLog("Detected Field Clocks: FC/%d, FC/%d - Bit Clock: RF/%d", fc1, fc2, rf1); setClockGrid(rf1, firstClockEdge); return rf1; } - if (verbose){ + if (g_debugMode) { PrintAndLog("DEBUG: unknown fsk field clock detected"); PrintAndLog("Detected Field Clocks: FC/%d, FC/%d - Bit Clock: RF/%d", fc1, fc2, rf1); } return 0; } -uint8_t fskClocks(uint8_t *fc1, uint8_t *fc2, uint8_t *rf1, bool verbose, int *firstClockEdge) { +int fskClocks(uint8_t *fc1, uint8_t *fc2, uint8_t *rf1, int *firstClockEdge) { uint8_t bits[MAX_GRAPH_TRACE_LEN] = {0}; size_t size = getFromGraphBuf(bits); - if (size == 0) return 0; + if (size == 0) + return 0; uint16_t ans = countFC(bits, size, 1); if (ans == 0) { - if (verbose || g_debugMode) PrintAndLog("DEBUG: No data found"); + if (g_debugMode) PrintAndLog("DEBUG: No data found"); return 0; } @@ -235,7 +229,7 @@ uint8_t fskClocks(uint8_t *fc1, uint8_t *fc2, uint8_t *rf1, bool verbose, int *f //int firstClockEdge = 0; *rf1 = detectFSKClk(bits, size, *fc1, *fc2, firstClockEdge); if (*rf1 == 0) { - if (verbose || g_debugMode) PrintAndLog("DEBUG: Clock detect error"); + if (g_debugMode) PrintAndLog("DEBUG: Clock detect error"); return 0; } return 1; diff --git a/client/graph.h b/client/graph.h index 3690ce252..50e3b70d5 100644 --- a/client/graph.h +++ b/client/graph.h @@ -21,12 +21,12 @@ void AppendGraph(int redraw, int clock, int bit); int ClearGraph(int redraw); size_t getFromGraphBuf(uint8_t *buff); -int GetAskClock(const char str[], bool printAns, bool verbose); -int GetPskClock(const char str[], bool printAns, bool verbose); -uint8_t GetPskCarrier(const char str[], bool printAns, bool verbose); -uint8_t GetNrzClock(const char str[], bool printAns, bool verbose); -uint8_t GetFskClock(const char str[], bool printAns, bool verbose); -uint8_t fskClocks(uint8_t *fc1, uint8_t *fc2, uint8_t *rf1, bool verbose, int *firstClockEdge); +int GetAskClock(const char *str, bool printAns); +int GetPskClock(const char *str, bool printAns); +uint8_t GetPskCarrier(const char *str, bool printAns); +int GetNrzClock(const char *str, bool printAns); +int GetFskClock(const char *str, bool printAns); +int fskClocks(uint8_t *fc1, uint8_t *fc2, uint8_t *rf1, int *firstClockEdge); void setGraphBuf(uint8_t *buff, size_t size); void save_restoreGB(uint8_t saveOpt); diff --git a/common/lfdemod.c b/common/lfdemod.c index d4c07d621..359900b5f 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -163,6 +163,9 @@ int getHiLo(uint8_t *bits, size_t size, int *high, int *low, uint8_t fuzzHi, uin // add fuzz. *high = ((signalprop.high - 128) * fuzzHi + 12800)/100; *low = ((signalprop.low - 128) * fuzzLo + 12800)/100; + + if (g_debugMode == 1) + prnt("getHiLo fuzzed: High %d | Low %d", *high, *low); return 1; } @@ -533,7 +536,8 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) { } } // test clock if given as cmd parameter - clk[0] = *clock; + if ( *clock > 0 ) + clk[0] = *clock; uint16_t ii; uint8_t clkCnt, tol = 0; @@ -965,7 +969,8 @@ int DetectPSKClock(uint8_t *dest, size_t size, int clock, size_t *firstPhaseShif //detects the bit clock for FSK given the high and low Field Clocks uint8_t detectFSKClk(uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, int *firstClockEdge) { - if (size == 0) return 0; + if (size == 0) + return 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}; @@ -1038,13 +1043,15 @@ uint8_t detectFSKClk(uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, } else if(rfCnts[i] > rfCnts[rfHighest3]){ rfHighest3 = i; } - if (g_debugMode==2) prnt("DEBUG FSK: RF %d, cnts %d", rfLens[i], rfCnts[i]); + if (g_debugMode==2) + prnt("DEBUG FSK: RF %d, cnts %d", rfLens[i], rfCnts[i]); } // 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; - if (g_debugMode==2) prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d", rfLens[rfHighest], rfLens[rfHighest2], rfLens[rfHighest3]); + if (g_debugMode==2) + prnt("DEBUG FSK: most counted rf values: 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 @@ -1054,7 +1061,8 @@ uint8_t detectFSKClk(uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){ if (rfLens[rfHighest2] % clk[ii] < tol1 || rfLens[rfHighest2] % clk[ii] > clk[ii]-tol1){ if (rfLens[rfHighest3] % clk[ii] < tol1 || rfLens[rfHighest3] % clk[ii] > clk[ii]-tol1){ - if (g_debugMode==2) prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance",clk[ii]); + if (g_debugMode==2) + prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance",clk[ii]); break; } } @@ -1403,7 +1411,7 @@ int askdemod_ext(uint8_t *bits, size_t *size, int *clk, int *invert, int maxErr, //start pos from detect ask clock is 1/2 clock offset // NOTE: can be negative (demod assumes rest of wave was there) *startIdx = start - (*clk/2); - uint16_t initLoopMax = 1500; + uint16_t initLoopMax = 1024; if (initLoopMax > *size) initLoopMax = *size; // Detect high and lows //25% clip in case highs and lows aren't clipped [marshmellow]