+ uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
+ uint16_t loopCnt = 4096; //don't need to loop through entire array...
+ if (size == 0) return 0;
+ if (size<loopCnt) loopCnt = size-20;
+
+ //if we already have a valid clock quit
+ size_t i=1;
+ for (; i < 8; ++i)
+ if (clk[i] == clock) return clock;
+
+ size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
+ uint8_t clkCnt, fc=0, fullWaveLen=0, tol=1;
+ 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};
+ fc = countFC(dest, size, 0);
+ if (fc!=2 && fc!=4 && fc!=8) return -1;
+ if (g_debugMode==2) prnt("DEBUG PSK: FC: %d",fc);
+
+ //find first full wave
+ for (i=160; i<loopCnt; i++){
+ if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
+ if (waveStart == 0) {
+ waveStart = i+1;
+ //prnt("DEBUG: waveStart: %d",waveStart);
+ } else {
+ waveEnd = i+1;
+ //prnt("DEBUG: waveEnd: %d",waveEnd);
+ waveLenCnt = waveEnd-waveStart;
+ if (waveLenCnt > fc){
+ firstFullWave = waveStart;
+ fullWaveLen=waveLenCnt;
+ break;
+ }
+ waveStart=0;
+ }
+ }
+ }
+ if (g_debugMode ==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
+
+ //test each valid clock from greatest to smallest to see which lines up
+ for(clkCnt=7; clkCnt >= 1 ; clkCnt--){
+ lastClkBit = firstFullWave; //set end of wave as clock align
+ waveStart = 0;
+ errCnt=0;
+ peakcnt=0;
+ if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
+
+ for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){
+ //top edge of wave = start of new wave
+ if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
+ if (waveStart == 0) {
+ waveStart = i+1;
+ waveLenCnt=0;
+ } else { //waveEnd
+ waveEnd = i+1;
+ waveLenCnt = waveEnd-waveStart;
+ if (waveLenCnt > fc){
+ //if this wave is a phase shift
+ if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,i+1,fc);
+ if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
+ peakcnt++;
+ lastClkBit+=clk[clkCnt];
+ } else if (i<lastClkBit+8){
+ //noise after a phase shift - ignore
+ } else { //phase shift before supposed to based on clock
+ errCnt++;
+ }
+ } else if (i+1 > lastClkBit + clk[clkCnt] + tol + fc){
+ lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
+ }
+ waveStart=i+1;
+ }
+ }
+ }
+ if (errCnt == 0){
+ return clk[clkCnt];
+ }
+ if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt]=errCnt;
+ if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt]=peakcnt;
+ }
+ //all tested with errors
+ //return the highest clk with the most peaks found
+ uint8_t best=7;
+ for (i=7; i>=1; i--){
+ if (peaksdet[i] > peaksdet[best]) {
+ best = i;
+ }
+ if (g_debugMode == 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[i],peaksdet[i],bestErr[i],clk[best]);
+ }
+ return clk[best];
+}
+
+int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low){
+ //find shortest transition from high to low
+ size_t i = 0;
+ size_t transition1 = 0;
+ int lowestTransition = 255;
+ bool lastWasHigh = false;
+
+ //find first valid beginning of a high or low wave
+ while ((dest[i] >= peak || dest[i] <= low) && (i < size))
+ ++i;
+ while ((dest[i] < peak && dest[i] > low) && (i < size))
+ ++i;
+ lastWasHigh = (dest[i] >= peak);
+
+ if (i==size) return 0;
+ transition1 = i;
+
+ for (;i < size; i++) {
+ if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) {
+ lastWasHigh = (dest[i] >= peak);
+ if (i-transition1 < lowestTransition) lowestTransition = i-transition1;
+ transition1 = i;
+ }
+ }
+ if (lowestTransition == 255) lowestTransition = 0;
+ if (g_debugMode==2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d",lowestTransition);
+ return lowestTransition;
+}
projects / proxmark3-svn / blobdiff