+//search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
+uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx) {
+ return (preambleSearchEx(BitStream, preamble, pLen, size, startIdx, false)) ? 1 : 0;
+}
+
+// find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.
+size_t findModStart(uint8_t dest[], size_t size, uint8_t expWaveSize) {
+ size_t i = 0;
+ size_t waveSizeCnt = 0;
+ uint8_t thresholdCnt = 0;
+ bool isAboveThreshold = dest[i++] >= FSK_PSK_THRESHOLD;
+ for (; i < size-20; i++ ) {
+ if(dest[i] < FSK_PSK_THRESHOLD && isAboveThreshold) {
+ thresholdCnt++;
+ if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
+ isAboveThreshold = false;
+ waveSizeCnt = 0;
+ } else if (dest[i] >= FSK_PSK_THRESHOLD && !isAboveThreshold) {
+ thresholdCnt++;
+ if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
+ isAboveThreshold = true;
+ waveSizeCnt = 0;
+ } else {
+ waveSizeCnt++;
+ }
+ if (thresholdCnt > 10) break;
+ }
+ if (g_debugMode == 2) prnt("DEBUG: threshold Count reached at %u, count: %u",i, thresholdCnt);
+ return i;
+}
+
+int getClosestClock(int testclk) {
+ uint8_t fndClk[] = {8,16,32,40,50,64,128};
+
+ for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++)
+ if (testclk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && testclk <= fndClk[clkCnt]+1)
+ return fndClk[clkCnt];
+
+ return 0;
+}
+
+void getNextLow(uint8_t samples[], size_t size, int low, size_t *i) {
+ while ((samples[*i] > low) && (*i < size))
+ *i+=1;
+}
+
+void getNextHigh(uint8_t samples[], size_t size, int high, size_t *i) {
+ while ((samples[*i] < high) && (*i < size))
+ *i+=1;
+}
+
+// load wave counters
+bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) {
+ size_t i=0, firstLow, firstHigh;
+ size_t testsize = (size < 512) ? size : 512;
+
+ if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) {
+ if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
+ return false; //just noise
+ }
+
+ // get to first full low to prime loop and skip incomplete first pulse
+ getNextHigh(samples, size, *high, &i);
+ getNextLow(samples, size, *low, &i);
+ *skip = i;
+
+ // populate tmpbuff buffer with pulse lengths
+ while (i < size) {
+ // measure from low to low
+ firstLow = i;
+ //find first high point for this wave
+ getNextHigh(samples, size, *high, &i);
+ firstHigh = i;
+
+ getNextLow(samples, size, *low, &i);
+
+ if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK))
+ break;
+
+ highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low
+ lowToLowWaveLen[*waveCnt] = i - firstLow;
+ *waveCnt += 1;
+ if (i-firstLow < *minClk && i < size) {
+ *minClk = i - firstLow;
+ }
+ }
+ return true;
+}
+
+size_t pskFindFirstPhaseShift(uint8_t samples[], size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) {
+ uint16_t loopCnt = (size+3 < 4096) ? size : 4096; //don't need to loop through entire array...
+
+ uint16_t avgWaveVal=0, lastAvgWaveVal=0;
+ size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave;
+ for (; i<loopCnt; i++) {
+ // find peak // was "samples[i] + fc" but why? must have been used to weed out some wave error... removed..
+ if (samples[i] < samples[i+1] && samples[i+1] >= samples[i+2]){
+ waveEnd = i+1;
+ if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart);
+ waveLenCnt = waveEnd-waveStart;
+ if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack
+ lastAvgWaveVal = avgWaveVal/(waveLenCnt);
+ firstFullWave = waveStart;
+ *fullWaveLen = waveLenCnt;
+ //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
+ if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1;
+ return firstFullWave;
+ }
+ waveStart = i+1;
+ avgWaveVal = 0;
+ }
+ avgWaveVal += samples[i+2];
+ }
+ return 0;
+}
+
+//by marshmellow
+//amplify based on ask edge detection - not accurate enough to use all the time
+void askAmp(uint8_t *BitStream, size_t size) {
+ uint8_t Last = 128;
+ for(size_t i = 1; i<size; i++){
+ if (BitStream[i]-BitStream[i-1]>=30) //large jump up
+ Last = 255;
+ else if(BitStream[i-1]-BitStream[i]>=20) //large jump down
+ Last = 0;
+
+ BitStream[i-1] = Last;
+ }
+ return;
+}
+
+uint32_t manchesterEncode2Bytes(uint16_t datain) {
+ uint32_t output = 0;
+ uint8_t curBit = 0;
+ for (uint8_t i=0; i<16; i++) {
+ curBit = (datain >> (15-i) & 1);
+ output |= (1<<(((15-i)*2)+curBit));
+ }
+ return output;
+}
+
+//by marshmellow
+//encode binary data into binary manchester
+//NOTE: BitStream must have triple the size of "size" available in memory to do the swap
+int ManchesterEncode(uint8_t *BitStream, size_t size) {
+ //allow up to 4K out (means BitStream must be at least 2048+4096 to handle the swap)
+ size = (size>2048) ? 2048 : size;
+ size_t modIdx = size;
+ size_t i;
+ for (size_t idx=0; idx < size; idx++){
+ BitStream[idx+modIdx++] = BitStream[idx];
+ BitStream[idx+modIdx++] = BitStream[idx]^1;
+ }
+ for (i=0; i<(size*2); i++){
+ BitStream[i] = BitStream[i+size];
+ }
+ return i;
+}
+
+// by marshmellow
+// to detect a wave that has heavily clipped (clean) samples
+uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low) {
+ bool allArePeaks = true;
+ uint16_t cntPeaks=0;
+ size_t loopEnd = 512+160;
+ if (loopEnd > size) loopEnd = size;
+ for (size_t i=160; i<loopEnd; i++){
+ if (dest[i]>low && dest[i]<high)
+ allArePeaks = false;
+ else
+ cntPeaks++;
+ }
+ if (!allArePeaks){
+ if (cntPeaks > 300) return true;
+ }
+ return allArePeaks;
+}
+
+//**********************************************************************************************
+//-------------------Clock / Bitrate Detection Section------------------------------------------
+//**********************************************************************************************
+
+// by marshmellow
+// to help detect clocks on heavily clipped samples
+// based on count of low to low
+int DetectStrongAskClock(uint8_t dest[], size_t size, int high, int low, int *clock) {
+ size_t startwave;
+ size_t i = 100;
+ size_t minClk = 255;
+ int shortestWaveIdx = 0;
+ // get to first full low to prime loop and skip incomplete first pulse
+ getNextHigh(dest, size, high, &i);
+ getNextLow(dest, size, low, &i);
+
+ // loop through all samples
+ while (i < size) {
+ // measure from low to low
+ startwave = i;
+
+ getNextHigh(dest, size, high, &i);
+ getNextLow(dest, size, low, &i);
+ //get minimum measured distance
+ if (i-startwave < minClk && i < size) {
+ minClk = i - startwave;
+ shortestWaveIdx = startwave;
+ }
+ }
+ // set clock
+ if (g_debugMode==2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d",minClk);
+ *clock = getClosestClock(minClk);
+ if (*clock == 0)
+ return 0;
+
+ return shortestWaveIdx;
+}
+
+// by marshmellow
+// not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
+// maybe somehow adjust peak trimming value based on samples to fix?
+// 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) {
+ size_t i=1;
+ uint8_t clk[] = {255,8,16,32,40,50,64,100,128,255};
+ uint8_t clkEnd = 9;
+ uint8_t loopCnt = 255; //don't need to loop through entire array...
+ if (size <= loopCnt+60) return -1; //not enough samples
+ size -= 60; //sometimes there is a strange end wave - filter out this....
+ //if we already have a valid clock
+ uint8_t clockFnd=0;
+ for (;i<clkEnd;++i)
+ if (clk[i] == *clock) clockFnd = i;
+ //clock found but continue to find best startpos
+
+ //get high and low peak
+ int peak, low;
+ if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return -1;
+
+ //test for large clean peaks
+ if (!clockFnd){
+ if (DetectCleanAskWave(dest, size, peak, low)==1){
+ int ans = DetectStrongAskClock(dest, size, peak, low, clock);
+ if (g_debugMode==2) prnt("DEBUG ASK: detectaskclk Clean Ask Wave Detected: clk %i, ShortestWave: %i",clock, ans);
+ if (ans > 0) {
+ return ans; //return shortest wave start position
+ }
+ }
+ }
+ 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;
+
+ if (clockFnd>0) {
+ clkCnt = clockFnd;
+ clkEnd = clockFnd+1;
+ }
+ else clkCnt=1;
+
+ //test each valid clock from smallest to greatest to see which lines up
+ for(; clkCnt < clkEnd; clkCnt++){
+ if (clk[clkCnt] <= 32){
+ tol=1;
+ }else{
+ tol=0;
+ }
+ //if no errors allowed - keep start within the first clock
+ if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt]<128) loopCnt=clk[clkCnt]*2;
+ bestErr[clkCnt]=1000;
+ //try lining up the peaks by moving starting point (try first few clocks)
+ for (ii=0; ii < loopCnt; ii++){
+ if (dest[ii] < peak && dest[ii] > 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 and a low clock (common clocks)
+ // this is correct one - return this clock
+ if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d",clk[clkCnt],errCnt,ii,i);
+ if(errCnt==0 && clkCnt<7) {
+ if (!clockFnd) *clock = clk[clkCnt];
+ return ii;
+ }
+ //if we found errors see if it is lowest so far and save it as best run
+ if(errCnt<bestErr[clkCnt]){
+ bestErr[clkCnt]=errCnt;
+ bestStart[clkCnt]=ii;
+ }
+ }
+ }
+ uint8_t iii;
+ uint8_t best=0;
+ for (iii=1; iii<clkEnd; ++iii){
+ if (bestErr[iii] < bestErr[best]){
+ if (bestErr[iii] == 0) bestErr[iii]=1;
+ // current best bit to error ratio vs new bit to error ratio
+ if ( (size/clk[best])/bestErr[best] < (size/clk[iii])/bestErr[iii] ){
+ best = iii;
+ }
+ }
+ if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d",clk[iii],bestErr[iii],clk[best],bestStart[best]);
+ }
+ if (!clockFnd) *clock = clk[best];
+ return bestStart[best];
+}
+
+int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low, bool *strong) {
+ //find shortest transition from high to low
+ *strong = false;
+ size_t i = 0;
+ size_t transition1 = 0;
+ int lowestTransition = 255;
+ bool lastWasHigh = false;
+ size_t transitionSampleCount = 0;
+ //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;
+ } else if (dest[i] < peak && dest[i] > low) {
+ transitionSampleCount++;
+ }
+ }
+ if (lowestTransition == 255) lowestTransition = 0;
+ if (g_debugMode==2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d",lowestTransition);
+ // if less than 10% of the samples were not peaks (or 90% were peaks) then we have a strong wave
+ if (transitionSampleCount / size < 10) {
+ *strong = true;
+ lowestTransition = getClosestClock(lowestTransition);
+ }
+ return lowestTransition;
+}
+
+//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;
+
+ bool strong = false;
+ int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low, &strong);
+ if (strong) return lowestTransition;
+ 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];
projects / proxmark3-svn / blobdiff