+//by marshmellow
+//detect nrz clock by reading #peaks vs no peaks(or errors)
+int DetectNRZClock(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) {
+ 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<loopCnt) loopCnt = size-20;
+ //if we already have a valid clock quit
+ for (; i < 8; ++i)
+ if (clk[i] == clock) return clock;
+
+ //get high and low peak
+ int peak, low;
+ if (getHiLo(dest, loopCnt, &peak, &low, 90, 90) < 1) return 0;
+
+ int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low);
+ size_t ii;
+ uint8_t clkCnt;
+ uint8_t tol = 0;
+ uint16_t smplCnt = 0;
+ int16_t peakcnt = 0;
+ int16_t peaksdet[] = {0,0,0,0,0,0,0,0};
+ uint16_t minPeak = 255;
+ bool firstpeak = true;
+ //test for large clipped waves - ignore first peak
+ for (i=0; i<loopCnt; i++) {
+ if (dest[i] >= peak || dest[i] <= low) {
+ if (firstpeak) continue;
+ smplCnt++;
+ } else {
+ firstpeak = false;
+ if (smplCnt > 0) {
+ if (minPeak > smplCnt && smplCnt > 7) minPeak = smplCnt;
+ peakcnt++;
+ if (g_debugMode == 2) prnt("DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d",minPeak,smplCnt,peakcnt);
+ smplCnt = 0;
+ }
+ }
+ }
+ if (minPeak < 8) return 0;
+ bool errBitHigh = 0;
+ bool bitHigh = 0;
+ uint8_t ignoreCnt = 0;
+ uint8_t ignoreWindow = 4;
+ bool lastPeakHigh = 0;
+ int lastBit = 0;
+ size_t bestStart[]={0,0,0,0,0,0,0,0,0};
+ peakcnt=0;
+ //test each valid clock from smallest to greatest to see which lines up
+ for(clkCnt=0; clkCnt < 8; ++clkCnt) {
+ //ignore clocks smaller than smallest peak
+ if (clk[clkCnt] < minPeak - (clk[clkCnt]/4)) continue;
+ //try lining up the peaks by moving starting point (try first 256)
+ for (ii=20; ii < loopCnt; ++ii) {
+ if ((dest[ii] >= peak) || (dest[ii] <= low)) {
+ peakcnt = 0;
+ bitHigh = false;
+ ignoreCnt = 0;
+ lastBit = ii-clk[clkCnt];
+ //loop through to see if this start location works
+ for (i = ii; i < size-20; ++i) {
+ //if we are at a clock bit
+ if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) {
+ //test high/low
+ if (dest[i] >= peak || dest[i] <= low) {
+ //if same peak don't count it
+ if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) {
+ peakcnt++;
+ }
+ lastPeakHigh = (dest[i] >= peak);
+ bitHigh = true;
+ errBitHigh = false;
+ ignoreCnt = ignoreWindow;
+ lastBit += clk[clkCnt];
+ } else if (i == lastBit + clk[clkCnt] + tol) {
+ lastBit += clk[clkCnt];
+ }
+ //else if not a clock bit and no peaks
+ } else if (dest[i] < peak && dest[i] > low) {
+ if (ignoreCnt==0) {
+ bitHigh=false;
+ if (errBitHigh==true) peakcnt--;
+ errBitHigh=false;
+ } else {
+ ignoreCnt--;
+ }
+ // else if not a clock bit but we have a peak
+ } else if ((dest[i]>=peak || dest[i]<=low) && (!bitHigh)) {
+ //error bar found no clock...
+ errBitHigh=true;
+ }
+ }
+ if(peakcnt>peaksdet[clkCnt]) {
+ bestStart[clkCnt]=ii;
+ peaksdet[clkCnt]=peakcnt;
+ }
+ }
+ }
+ }
+ int iii=7;
+ uint8_t best=0;
+ for (iii=7; iii > 0; iii--) {
+ if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) {
+ if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) {
+ best = iii;
+ }
+ } else if (peaksdet[iii] > peaksdet[best]) {
+ best = iii;
+ }
+ if (g_debugMode==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],minPeak, clk[best], lowestTransition);
+ }
+ *clockStartIdx = bestStart[best];
+ return clk[best];
+}
+
+//by marshmellow
+//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 fskAdj) {
+ uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+ uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+ uint8_t fcLensFnd = 0;
+ uint8_t lastFCcnt = 0;
+ uint8_t fcCounter = 0;
+ size_t i;
+ if (size < 180) return 0;
+
+ // prime i to first up transition
+ for (i = 160; i < size-20; i++)
+ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
+ break;
+
+ for (; i < size-20; i++){
+ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
+ // new up transition
+ 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<15; ii++){
+ if (fcLens[ii]==fcCounter){
+ fcCnts[ii]++;
+ fcCounter=0;
+ break;
+ }
+ }
+ if (fcCounter>0 && fcLensFnd<15){
+ //add new fc length
+ fcCnts[fcLensFnd]++;
+ fcLens[fcLensFnd++]=fcCounter;
+ }
+ fcCounter=0;
+ } else {
+ // count sample
+ fcCounter++;
+ }
+ }
+
+ uint8_t best1=14, best2=14, best3=14;
+ uint16_t maxCnt1=0;
+ // go through fclens and find which ones are bigest 2
+ for (i=0; i<15; 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;
+ }
+ if (g_debugMode==2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u",fcLens[i],fcCnts[i],fcLens[best1],fcLens[best2]);
+ if (fcLens[i]==0) break;
+ }
+ if (fcLens[best1]==0) return 0;
+ uint8_t fcH=0, fcL=0;
+ if (fcLens[best1]>fcLens[best2]){
+ fcH=fcLens[best1];
+ fcL=fcLens[best2];
+ } else{
+ fcH=fcLens[best2];
+ fcL=fcLens[best1];
+ }
+ if ((size-180)/fcH/3 > fcCnts[best1]+fcCnts[best2]) {
+ if (g_debugMode==2) prnt("DEBUG countfc: fc is too large: %u > %u. Not psk or fsk",(size-180)/fcH/3,fcCnts[best1]+fcCnts[best2]);
+ return 0; //lots of waves not psk or fsk
+ }
+ // TODO: take top 3 answers and compare to known Field clocks to get top 2
+
+ uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
+ if (fskAdj) return fcs;
+ return (uint16_t)fcLens[best2] << 8 | fcLens[best1];
+}
+
+//by marshmellow
+//detect psk clock by reading each phase shift
+// a phase shift is determined by measuring the sample length of each wave
+int DetectPSKClock(uint8_t dest[], size_t size, int clock, size_t *firstPhaseShift, uint8_t *curPhase, uint8_t *fc) {
+ 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+3<loopCnt) loopCnt = size-20;
+
+ uint16_t fcs = countFC(dest, size, 0);
+ *fc = fcs & 0xFF;
+ if (g_debugMode==2) prnt("DEBUG PSK: FC: %d, FC2: %d",*fc, fcs>>8);
+ if ((fcs>>8) == 10 && *fc == 8) return -1;
+ if (*fc!=2 && *fc!=4 && *fc!=8) return -1;
+
+ //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, tol=1;
+ uint16_t peakcnt=0, errCnt=0, waveLenCnt=0, fullWaveLen=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};
+
+ //find start of modulating data in trace
+ i = findModStart(dest, size, *fc);
+
+ firstFullWave = pskFindFirstPhaseShift(dest, size, curPhase, i, *fc, &fullWaveLen);
+ if (firstFullWave == 0) {
+ // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
+ // so skip a little to ensure we are past any Start Signal
+ firstFullWave = 160;
+ fullWaveLen = 0;
+ }
+
+ *firstPhaseShift = firstFullWave;
+ 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--) {
+ tol = *fc/2;
+ 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 DetectPSKClock(uint8_t dest[], size_t size, int clock) {
+// size_t firstPhaseShift = 0;
+// uint8_t curPhase = 0;
+// return DetectPSKClock_ext(dest, size, clock, &firstPhaseShift, &curPhase);
+//}
+
+//by marshmellow
+//detects the bit clock for FSK given the high and low Field Clocks
+uint8_t detectFSKClk_ext(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow, int *firstClockEdge) {
+ 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 lastFCcnt = 0;
+ uint16_t fcCounter = 0;
+ uint16_t rfCounter = 0;
+ uint8_t firstBitFnd = 0;
+ size_t i;
+ if (size == 0) return 0;
+
+ uint8_t fcTol = ((fcHigh*100 - fcLow*100)/2 + 50)/100; //(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 peak / up transition
+ for (i = 160; i < size-20; i++)
+ if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1])
+ break;
projects / proxmark3-svn / blobdiff