+
+ 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 0;
+ if (*fc!=2 && *fc!=4 && *fc!=8) return 0;
+
+ //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];
+}
+
+//by marshmellow
+//detects the bit clock for FSK given the high and low Field Clocks
+uint8_t detectFSKClk(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;
+
+ for (; i < size-20; i++){
+ 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 it is inbetween set it to the last counter
+ if (fcCounter < fcHigh && fcCounter > fcLow)
+ fcCounter = lastFCcnt;
+ else 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-4) && rfLens[ii] <= (rfCounter+4)){
+ rfCnts[ii]++;
+ rfCounter = 0;
+ break;
+ }
+ }
+ if (rfCounter > 0 && rfLensFnd < 15){
+ //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
+ rfCnts[rfLensFnd]++;
+ rfLens[rfLensFnd++] = rfCounter;
+ }
+ } else {
+ *firstClockEdge = i;
+ firstBitFnd++;
+ }
+ rfCounter=0;
+ lastFCcnt=fcCounter;
+ }
+ fcCounter=0;
+ }
+ uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15;
+
+ for (i=0; i<15; i++){
+ //get highest 2 RF values (might need to get more values to compare or compare all?)
+ if (rfCnts[i]>rfCnts[rfHighest]){
+ rfHighest3=rfHighest2;
+ rfHighest2=rfHighest;
+ rfHighest=i;
+ } else if(rfCnts[i]>rfCnts[rfHighest2]){
+ rfHighest3=rfHighest2;
+ rfHighest2=i;
+ } else if(rfCnts[i]>rfCnts[rfHighest3]){
+ rfHighest3=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]);
+
+ // loop to find the highest clock that has a remainder less than the tolerance
+ // compare samples counted divided by
+ // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less)
+ int ii=7;
+ for (; ii>=2; ii--){
+ 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]);
+ break;
+ }
+ }
+ }
+ }
+
+ if (ii<2) return 0; // oops we went too far
+
+ return clk[ii];
+}
+
+//**********************************************************************************************
+//--------------------Modulation Demods &/or Decoding Section-----------------------------------
+//**********************************************************************************************
+
+// look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index...
+bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) {
+ if (buffSize < *i+4) return false;
+
+ for (; *i < buffSize - 4; *i+=1) {
+ *stStartIdx += lowToLowWaveLen[*i]; //caution part of this wave may be data and part may be ST.... to be accounted for in main function for now...
+ if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior
+ if (lowToLowWaveLen[*i+1] >= clk*2-tol && lowToLowWaveLen[*i+1] <= clk*2+tol && highToLowWaveLen[*i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2
+ if (lowToLowWaveLen[*i+2] >= (clk*3)/2-tol && lowToLowWaveLen[*i+2] <= clk*2+tol && highToLowWaveLen[*i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave
+ if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
+ *stStopLoc = *i + 3;
+ return true;
+ }
+ }
+ }
+ }
+ }
+ return false;
+}
+//by marshmellow
+//attempt to identify a Sequence Terminator in ASK modulated raw wave
+bool DetectST(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststart, size_t *stend) {
+ size_t bufsize = *size;
+ //need to loop through all samples and identify our clock, look for the ST pattern
+ int clk = 0;
+ int tol = 0;
+ int j=0, high, low, skip=0, start=0, end=0, minClk=255;
+ size_t i = 0;
+ //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
+ int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured
+ int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed...
+ //size_t testsize = (bufsize < 512) ? bufsize : 512;
+ int phaseoff = 0;
+ high = low = 128;
+ memset(tmpbuff, 0, sizeof(tmpbuff));
+ memset(waveLen, 0, sizeof(waveLen));
+
+ if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false;
+ // set clock - might be able to get this externally and remove this work...
+ clk = getClosestClock(minClk);
+ // clock not found - ERROR
+ if (clk == 0) {
+ if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
+ return false;
+ }
+ *foundclock = clk;
+
+ tol = clk/8;
+ if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) {
+ // first ST not found - ERROR
+ if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
+ return false;
+ } else {
+ if (g_debugMode==2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j);
+ }
+ if (waveLen[i+2] > clk*1+tol)
+ phaseoff = 0;
+ else
+ phaseoff = clk/2;
+
+ // skip over the remainder of ST
+ skip += clk*7/2; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point
+
+ // now do it again to find the end
+ int dummy1 = 0;
+ end = skip;
+ i+=3;
+ if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) {
+ //didn't find second ST - ERROR
+ if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting");
+ return false;
+ }
+ end -= phaseoff;
+ if (g_debugMode==2) prnt("DEBUG STT: start of data: %d end of data: %d, datalen: %d, clk: %d, bits: %d, phaseoff: %d", skip, end, end-skip, clk, (end-skip)/clk, phaseoff);
+ //now begin to trim out ST so we can use normal demod cmds
+ start = skip;
+ size_t datalen = end - start;
+ // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock
+ if ( clk - (datalen % clk) <= clk/8) {
+ // padd the amount off - could be problematic... but shouldn't happen often
+ datalen += clk - (datalen % clk);
+ } else if ( (datalen % clk) <= clk/8 ) {
+ // padd the amount off - could be problematic... but shouldn't happen often
+ datalen -= datalen % clk;
+ } else {
+ if (g_debugMode==2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk);
+ return false;
+ }
+ // if datalen is less than one t55xx block - ERROR
+ if (datalen/clk < 8*4) {
+ if (g_debugMode==2) prnt("DEBUG STT: datalen is less than 1 full t55xx block - quitting");
+ return false;
+ }
+ size_t dataloc = start;
+ if (buffer[dataloc-(clk*4)-(clk/4)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {
+ //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start
+ for ( i=0; i <= (clk/4); ++i ) {
+ if ( buffer[dataloc - (clk*4) - i] <= low ) {
+ dataloc -= i;
+ break;
+ }
+ }
+ }
+
+ size_t newloc = 0;
+ i=0;
+ if (g_debugMode==2) prnt("DEBUG STT: Starting STT trim - start: %d, datalen: %d ",dataloc, datalen);
+ bool firstrun = true;
+ // warning - overwriting buffer given with raw wave data with ST removed...
+ while ( dataloc < bufsize-(clk/2) ) {
+ //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part)
+ if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+clk/4]<high && buffer[dataloc+clk/4]>low) {
+ for(i=0; i < clk/2-tol; ++i) {
+ buffer[dataloc+i] = high+5;
+ }
+ } //test for small spike outlier (high between two lows) in the case of very strong waves
+ if (buffer[dataloc] > low && buffer[dataloc+clk/4] <= low) {
+ for(i=0; i < clk/4; ++i) {
+ buffer[dataloc+i] = buffer[dataloc+clk/4];
+ }
+ }
+ if (firstrun) {
+ *stend = dataloc;
+ *ststart = dataloc-(clk*4);
+ firstrun=false;
+ }
+ for (i=0; i<datalen; ++i) {
+ if (i+newloc < bufsize) {
+ if (i+newloc < dataloc)
+ buffer[i+newloc] = buffer[dataloc];
+
+ dataloc++;
+ }
+ }
+ newloc += i;
+ //skip next ST - we just assume it will be there from now on...
+ if (g_debugMode==2) prnt("DEBUG STT: skipping STT at %d to %d", dataloc, dataloc+(clk*4));
+ dataloc += clk*4;
+ }
+ *size = newloc;
+ return true;
+}
+
+//by marshmellow
+//take 11 10 01 11 00 and make 01100 ... miller decoding
+//check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
+//decodes miller encoded binary
+//NOTE askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!
+int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) {
+ if (*size < 16) return -1;
+ uint16_t MaxBits = 512, errCnt = 0;
+ size_t i, bitCnt=0;
+ uint8_t alignCnt = 0, curBit = BitStream[0], alignedIdx = 0;
+ uint8_t halfClkErr = 0;
+ //find alignment, needs 4 1s or 0s to properly align
+ for (i=1; i < *size-1; i++) {
+ alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0;
+ curBit = BitStream[i];
+ if (alignCnt == 4) break;
+ }
+ // for now error if alignment not found. later add option to run it with multiple offsets...
+ if (alignCnt != 4) {
+ if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it");
+ return -1;
+ }
+ alignedIdx = (i-1) % 2;
+ for (i=alignedIdx; i < *size-3; i+=2) {
+ halfClkErr = (uint8_t)((halfClkErr << 1 | BitStream[i]) & 0xFF);
+ if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) {
+ errCnt++;
+ BitStream[bitCnt++] = 7;
+ continue;
+ }
+ BitStream[bitCnt++] = BitStream[i] ^ BitStream[i+1] ^ invert;
+
+ if (bitCnt > MaxBits) break;
+ }
+ *size = bitCnt;
+ return errCnt;
+}
+
+//by marshmellow
+//take 01 or 10 = 1 and 11 or 00 = 0
+//check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
+//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;
+ uint16_t errCnt = 0;
+ size_t i = *offset;
+ uint16_t MaxBits=512;
+ //if not enough samples - error
+ if (*size < 51) return -1;
+ //check for phase change faults - skip one sample if faulty
+ uint8_t offsetA = 1, offsetB = 1;
+ for (; i<48; i+=2){
+ if (BitStream[i+1]==BitStream[i+2]) offsetA=0;
+ if (BitStream[i+2]==BitStream[i+3]) offsetB=0;
+ }
+ if (!offsetA && offsetB) *offset+=1;
+ for (i=*offset; i<*size-3; i+=2){
+ //check for phase error
+ if (BitStream[i+1]==BitStream[i+2]) {
+ BitStream[bitnum++]=7;
+ errCnt++;
+ }
+ if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
+ BitStream[bitnum++]=1^invert;
+ } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
+ BitStream[bitnum++]=invert;
+ } else {
+ BitStream[bitnum++]=7;
+ errCnt++;
+ }
+ if(bitnum>MaxBits) break;
+ }
+ *size=bitnum;
+ return errCnt;
+}
+
+//by marshmellow
+//take 10 and 01 and manchester decode
+//run through 2 times and take least errCnt
+int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert, uint8_t *alignPos) {
+ uint16_t bitnum=0, MaxBits = 512, errCnt = 0;
+ size_t i, ii;
+ uint16_t bestErr = 1000, bestRun = 0;
+ if (*size < 16) return -1;
+ //find correct start position [alignment]
+ for (ii=0;ii<2;++ii){
+ for (i=ii; i<*size-3; i+=2)
+ if (BitStream[i]==BitStream[i+1])
+ errCnt++;
+
+ if (bestErr>errCnt){
+ bestErr=errCnt;
+ bestRun=ii;
+ }
+ errCnt=0;
+ }
+ *alignPos=bestRun;
+ //decode
+ for (i=bestRun; i < *size-3; i+=2){
+ if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
+ BitStream[bitnum++]=invert;
+ } else if((BitStream[i] == 0) && BitStream[i+1] == 1){
+ BitStream[bitnum++]=invert^1;
+ } else {
+ BitStream[bitnum++]=7;
+ }
+ if(bitnum>MaxBits) break;
+ }
+ *size=bitnum;
+ return bestErr;
projects / proxmark3-svn / blobdiff