+
+// by marshmellow
+// convert psk1 demod to psk2 demod
+// only transition waves are 1s
+void psk1TOpsk2(uint8_t *BitStream, size_t size)
+{
+ size_t i=1;
+ uint8_t lastBit=BitStream[0];
+ for (; i<size; i++){
+ if (lastBit!=BitStream[i]){
+ lastBit=BitStream[i];
+ BitStream[i]=1;
+ } else {
+ BitStream[i]=0;
+ }
+ }
+ return;
+}
+
+// by marshmellow
+// convert psk2 demod to psk1 demod
+// from only transition waves are 1s to phase shifts change bit
+void psk2TOpsk1(uint8_t *BitStream, size_t size)
+{
+ uint8_t phase=0;
+ for (size_t i=0; i<size; i++){
+ if (BitStream[i]==1){
+ phase ^=1;
+ }
+ BitStream[i]=phase;
+ }
+ return;
+}
+
+// redesigned by marshmellow adjusted from existing decode functions
+// indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
+int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert)
+{
+ //26 bit 40134 format (don't know other formats)
+ int i;
+ int long_wait=29;//29 leading zeros in format
+ int start;
+ int first = 0;
+ int first2 = 0;
+ int bitCnt = 0;
+ int ii;
+ // Finding the start of a UID
+ for (start = 0; start <= *size - 250; start++) {
+ first = bitStream[start];
+ for (i = start; i < start + long_wait; i++) {
+ if (bitStream[i] != first) {
+ break;
+ }
+ }
+ if (i == (start + long_wait)) {
+ break;
+ }
+ }
+ if (start == *size - 250 + 1) {
+ // did not find start sequence
+ return -1;
+ }
+ // Inverting signal if needed
+ if (first == 1) {
+ for (i = start; i < *size; i++) {
+ bitStream[i] = !bitStream[i];
+ }
+ *invert = 1;
+ }else *invert=0;
+
+ int iii;
+ //found start once now test length by finding next one
+ for (ii=start+29; ii <= *size - 250; ii++) {
+ first2 = bitStream[ii];
+ for (iii = ii; iii < ii + long_wait; iii++) {
+ if (bitStream[iii] != first2) {
+ break;
+ }
+ }
+ if (iii == (ii + long_wait)) {
+ break;
+ }
+ }
+ if (ii== *size - 250 + 1){
+ // did not find second start sequence
+ return -2;
+ }
+ bitCnt=ii-start;
+
+ // Dumping UID
+ i = start;
+ for (ii = 0; ii < bitCnt; ii++) {
+ bitStream[ii] = bitStream[i++];
+ }
+ *size=bitCnt;
+ return 1;
+}
+
+// by marshmellow - demodulate NRZ wave (both similar enough)
+// peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
+// there probably is a much simpler way to do this....
+int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
+{
+ if (justNoise(dest, *size)) return -1;
+ *clk = DetectNRZClock(dest, *size, *clk);
+ if (*clk==0) return -2;
+ uint32_t i;
+ int high, low, ans;
+ ans = getHiLo(dest, 1260, &high, &low, 75, 75); //25% fuzz on high 25% fuzz on low
+ if (ans<1) return -2; //just noise
+ uint32_t gLen = 256;
+ if (gLen>*size) gLen = *size;
+ int lastBit = 0; //set first clock check
+ uint32_t bitnum = 0; //output counter
+ uint8_t tol = 1; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
+ uint32_t iii = 0;
+ uint16_t errCnt =0;
+ uint16_t MaxBits = 1000;
+ uint32_t bestErrCnt = maxErr+1;
+ uint32_t bestPeakCnt = 0;
+ uint32_t bestPeakStart=0;
+ uint8_t curBit=0;
+ uint8_t bitHigh=0;
+ uint8_t errBitHigh=0;
+ uint16_t peakCnt=0;
+ uint8_t ignoreWindow=4;
+ uint8_t ignoreCnt=ignoreWindow; //in case of noice near peak
+ //loop to find first wave that works - align to clock
+ for (iii=0; iii < gLen; ++iii){
+ if ((dest[iii]>=high) || (dest[iii]<=low)){
+ lastBit=iii-*clk;
+ peakCnt=0;
+ errCnt=0;
+ bitnum=0;
+ //loop through to see if this start location works
+ for (i = iii; i < *size; ++i) {
+ //if we found a high bar and we are at a clock bit
+ if ((dest[i]>=high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
+ bitHigh=1;
+ lastBit+=*clk;
+ bitnum++;
+ peakCnt++;
+ errBitHigh=0;
+ ignoreCnt=ignoreWindow;
+ //else if low bar found and we are at a clock point
+ }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
+ bitHigh=1;
+ lastBit+=*clk;
+ bitnum++;
+ peakCnt++;
+ errBitHigh=0;
+ ignoreCnt=ignoreWindow;
+ //else if no bars found
+ }else if(dest[i] < high && dest[i] > low) {
+ if (ignoreCnt==0){
+ bitHigh=0;
+ if (errBitHigh==1){
+ errCnt++;
+ }
+ errBitHigh=0;
+ } else {
+ ignoreCnt--;
+ }
+ //if we are past a clock point
+ if (i >= lastBit+*clk+tol){ //clock val
+ lastBit+=*clk;
+ bitnum++;
+ }
+ //else if bar found but we are not at a clock bit and we did not just have a clock bit
+ }else if ((dest[i]>=high || dest[i]<=low) && (i<lastBit+*clk-tol || i>lastBit+*clk+tol) && (bitHigh==0)){
+ //error bar found no clock...
+ errBitHigh=1;
+ }
+ if (bitnum>=MaxBits) break;
+ }
+ //we got more than 64 good bits and not all errors
+ if (bitnum > (64) && (errCnt <= (maxErr))) {
+ //possible good read
+ if (errCnt == 0){
+ //bestStart = iii;
+ bestErrCnt = errCnt;
+ bestPeakCnt = peakCnt;
+ bestPeakStart = iii;
+ break; //great read - finish
+ }
+ if (errCnt < bestErrCnt){ //set this as new best run
+ bestErrCnt = errCnt;
+ //bestStart = iii;
+ }
+ if (peakCnt > bestPeakCnt){
+ bestPeakCnt=peakCnt;
+ bestPeakStart=iii;
+ }
+ }
+ }
+ }
+ //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart);
+ if (bestErrCnt <= maxErr){
+ //best run is good enough set to best run and set overwrite BinStream
+ iii=bestPeakStart;
+ lastBit=bestPeakStart-*clk;
+ bitnum=0;
+ for (i = iii; i < *size; ++i) {
+ //if we found a high bar and we are at a clock bit
+ if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
+ bitHigh=1;
+ lastBit+=*clk;
+ curBit=1-*invert;
+ dest[bitnum]=curBit;
+ bitnum++;
+ errBitHigh=0;
+ ignoreCnt=ignoreWindow;
+ //else if low bar found and we are at a clock point
+ }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
+ bitHigh=1;
+ lastBit+=*clk;
+ curBit=*invert;
+ dest[bitnum]=curBit;
+ bitnum++;
+ errBitHigh=0;
+ ignoreCnt=ignoreWindow;
+ //else if no bars found
+ }else if(dest[i]<high && dest[i]>low) {
+ if (ignoreCnt==0){
+ bitHigh=0;
+ //if peak is done was it an error peak?
+ if (errBitHigh==1){
+ dest[bitnum]=77;
+ bitnum++;
+ errCnt++;
+ }
+ errBitHigh=0;
+ } else {
+ ignoreCnt--;
+ }
+ //if we are past a clock point
+ if (i>=lastBit+*clk+tol){ //clock val
+ lastBit+=*clk;
+ dest[bitnum]=curBit;
+ bitnum++;
+ }
+ //else if bar found but we are not at a clock bit and we did not just have a clock bit
+ }else if ((dest[i]>=high || dest[i]<=low) && ((i<lastBit+*clk-tol) || (i>lastBit+*clk+tol)) && (bitHigh==0)){
+ //error bar found no clock...
+ errBitHigh=1;
+ }
+ if (bitnum >= MaxBits) break;
+ }
+ *size=bitnum;
+ } else{
+ *size=bitnum;
+ return -1;
+ }
+
+ if (bitnum>16){
+ *size=bitnum;
+ } else return -1;
+ return errCnt;
+}
+
+//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)
+{
+ 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;
+ uint32_t fcCounter = 0;
+ uint16_t rfCounter = 0;
+ uint8_t firstBitFnd = 0;
+ size_t i;
+ if (size == 0) return 0;
+
+ uint8_t fcTol = (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 up transition
+ for (i = 1; i < size-1; i++)
+ if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1])
+ break;
+
+ for (; i < size-1; i++){
+ if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1]){
+ // new peak
+ fcCounter++;
+ rfCounter++;
+ // if we got less than the small fc + tolerance then set it to the small fc
+ 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){
+ rfCnts[ii]++;
+ rfCounter=0;
+ break;
+ }
+ }
+ if (rfCounter>0 && rfLensFnd<15){
+ //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
+ rfCnts[rfLensFnd]++;
+ rfLens[rfLensFnd++]=rfCounter;
+ }
+ } else {
+ firstBitFnd++;
+ }
+ rfCounter=0;
+ lastFCcnt=fcCounter;
+ }
+ fcCounter=0;
+ } else {
+ // count sample
+ fcCounter++;
+ rfCounter++;
+ }
+ }
+ uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15;
+
+ for (i=0; i<15; i++){
+ //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[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;
+ }
+ }
+ // 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;
+
+ //PrintAndLog("DEBUG: hightest: 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
+ int ii=7;
+ for (; ii>=0; 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){
+ break;
+ }
+ }
+ }
+ }
+
+ if (ii<0) return 0; // oops we went too far
+
+ return clk[ii];
+}
+
+//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 *mostFC)
+{
+ uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0};
+ uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0};
+ uint8_t fcLensFnd = 0;
+ uint8_t lastFCcnt=0;
+ uint32_t fcCounter = 0;
+ size_t i;
+ if (size == 0) return 0;
+
+ // prime i to first up transition
+ for (i = 1; i < size-1; i++)
+ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
+ break;
+
+ for (; i < size-1; i++){
+ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
+ // new up transition
+ fcCounter++;
+
+ //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 odd and not rc/5 add one (for when we get a fc 9 instead of 10)
+ if ((fcCounter==9 && fcCounter & 1) || fcCounter==4) fcCounter++;
+
+ // save last field clock count (fc/xx)
+ // find which fcLens to save it to:
+ for (int ii=0; ii<10; ii++){
+ if (fcLens[ii]==fcCounter){
+ fcCnts[ii]++;
+ fcCounter=0;
+ break;
+ }
+ }
+ if (fcCounter>0 && fcLensFnd<10){
+ //add new fc length
+ fcCnts[fcLensFnd]++;
+ fcLens[fcLensFnd++]=fcCounter;
+ }
+ fcCounter=0;
+ } else {
+ // count sample
+ fcCounter++;
+ }
+ }
+
+ uint8_t best1=9, best2=9, best3=9;
+ uint16_t maxCnt1=0;
+ // go through fclens and find which ones are bigest 2
+ for (i=0; i<10; i++){
+ // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt);
+ // 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;
+ }
+ }
+ uint8_t fcH=0, fcL=0;
+ if (fcLens[best1]>fcLens[best2]){
+ fcH=fcLens[best1];
+ fcL=fcLens[best2];
+ } else{
+ fcH=fcLens[best2];
+ fcL=fcLens[best1];
+ }
+
+ *mostFC=fcLens[best1];
+ // TODO: take top 3 answers and compare to known Field clocks to get top 2
+
+ uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
+ // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
+
+ return fcs;
+}
+
+//by marshmellow
+//countPSK_FC is to detect the psk carrier clock length.
+//counts and returns the 1 most common wave length
+uint8_t countPSK_FC(uint8_t *BitStream, size_t size)
+{
+ uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0};
+ uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0};
+ uint8_t fcLensFnd = 0;
+ uint32_t fcCounter = 0;
+ size_t i;
+ if (size == 0) return 0;
+
+ // prime i to first up transition
+ for (i = 1; i < size-1; i++)
+ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
+ break;
+
+ for (; i < size-1; i++){
+ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
+ // new up transition
+ fcCounter++;
+
+ // save last field clock count (fc/xx)
+ // find which fcLens to save it to:
+ for (int ii=0; ii<10; ii++){
+ if (fcLens[ii]==fcCounter){
+ fcCnts[ii]++;
+ fcCounter=0;
+ break;
+ }
+ }
+ if (fcCounter>0 && fcLensFnd<10){
+ //add new fc length
+ fcCnts[fcLensFnd]++;
+ fcLens[fcLensFnd++]=fcCounter;
+ }
+ fcCounter=0;
+ } else {
+ // count sample
+ fcCounter++;
+ }
+ }
+
+ uint8_t best1=9;
+ uint16_t maxCnt1=0;
+ // go through fclens and find which ones are bigest
+ for (i=0; i<10; i++){
+ //PrintAndLog("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]);
+ // get the best FC value
+ if (fcCnts[i]>maxCnt1) {
+ maxCnt1=fcCnts[i];
+ best1=i;
+ }
+ }
+ return fcLens[best1];
+}
+
+//by marshmellow - demodulate PSK1 wave
+//uses wave lengths (# Samples)
+int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
+{
+ uint16_t loopCnt = 4096; //don't need to loop through entire array...
+ if (size == 0) return -1;
+ if (*size<loopCnt) loopCnt = *size;
+
+ uint8_t curPhase = *invert;
+ size_t i, waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
+ uint8_t fc=0, fullWaveLen=0, tol=1;
+ uint16_t errCnt=0, waveLenCnt=0;
+ fc = countPSK_FC(dest, *size);
+ if (fc!=2 && fc!=4 && fc!=8) return -1;
+ //PrintAndLog("DEBUG: FC: %d",fc);
+ *clock = DetectPSKClock(dest, *size, *clock);
+ if (*clock==0) return -1;
+ int avgWaveVal=0, lastAvgWaveVal=0;
+ //find first full wave
+ for (i=0; i<loopCnt; i++){
+ if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
+ if (waveStart == 0) {
+ waveStart = i+1;
+ avgWaveVal=dest[i+1];
+ //PrintAndLog("DEBUG: waveStart: %d",waveStart);
+ } else {
+ waveEnd = i+1;
+ //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
+ waveLenCnt = waveEnd-waveStart;
+ lastAvgWaveVal = avgWaveVal/waveLenCnt;
+ if (waveLenCnt > fc){
+ firstFullWave = waveStart;
+ fullWaveLen=waveLenCnt;
+ //if average wave value is > graph 0 then it is an up wave or a 1
+ if (lastAvgWaveVal > 128) curPhase^=1;
+ break;
+ }
+ waveStart=0;
+ avgWaveVal=0;
+ }
+ }
+ avgWaveVal+=dest[i+1];
+ }
+ //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
+ lastClkBit = firstFullWave; //set start of wave as clock align
+ waveStart = 0;
+ errCnt=0;
+ size_t numBits=0;
+ //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
+
+ for (i = firstFullWave+fullWaveLen-1; i < *size-3; i++){
+ //top edge of wave = start of new wave
+ if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
+ if (waveStart == 0) {
+ waveStart = i+1;
+ waveLenCnt=0;
+ avgWaveVal = dest[i+1];
+ } else { //waveEnd
+ waveEnd = i+1;
+ waveLenCnt = waveEnd-waveStart;
+ lastAvgWaveVal = avgWaveVal/waveLenCnt;
+ if (waveLenCnt > fc){
+ //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
+ //if this wave is a phase shift
+ //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
+ if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
+ curPhase^=1;
+ dest[numBits] = curPhase;
+ numBits++;
+ lastClkBit += *clock;
+ } else if (i<lastClkBit+10){
+ //noise after a phase shift - ignore
+ } else { //phase shift before supposed to based on clock
+ errCnt++;
+ dest[numBits] = 77;
+ numBits++;
+ }
+ } else if (i+1 > lastClkBit + *clock + tol + fc){
+ lastClkBit += *clock; //no phase shift but clock bit
+ dest[numBits] = curPhase;
+ numBits++;
+ }
+ avgWaveVal=0;
+ waveStart=i+1;
+ }
+ }
+ avgWaveVal+=dest[i+1];
+ }
+ *size = numBits;
+ return errCnt;
+}
projects / proxmark3-svn / blobdiff