+
+//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=1, 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 phase shift
+ for (i=0; i<loopCnt; i++){
+ if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
+ waveEnd = i+1;
+ //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
+ waveLenCnt = waveEnd-waveStart;
+ if (waveLenCnt > fc && waveStart > fc){ //not first peak and is a large wave
+ lastAvgWaveVal = avgWaveVal/(waveLenCnt);
+ firstFullWave = waveStart;
+ fullWaveLen=waveLenCnt;
+ //if average wave value is > graph 0 then it is an up wave or a 1
+ if (lastAvgWaveVal > 123) curPhase^=1; //fudge graph 0 a little 123 vs 128
+ break;
+ }
+ waveStart = i+1;
+ avgWaveVal = 0;
+ }
+ avgWaveVal+=dest[i+2];
+ }
+ //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
+ lastClkBit = firstFullWave; //set start of wave as clock align
+ //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
+ waveStart = 0;
+ errCnt=0;
+ size_t numBits=0;
+ //set skipped bits
+ memset(dest+numBits,curPhase^1,firstFullWave / *clock);
+ numBits += (firstFullWave / *clock);
+ dest[numBits++] = curPhase; //set first read bit
+ 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;
+ lastClkBit += *clock;
+ } else if (i<lastClkBit+10+fc){
+ //noise after a phase shift - ignore
+ } else { //phase shift before supposed to based on clock
+ errCnt++;
+ dest[numBits++] = 77;
+ }
+ } else if (i+1 > lastClkBit + *clock + tol + fc){
+ lastClkBit += *clock; //no phase shift but clock bit
+ dest[numBits++] = curPhase;
+ }
+ avgWaveVal=0;
+ waveStart=i+1;
+ }
+ }
+ avgWaveVal+=dest[i+1];
+ }
+ *size = numBits;
+ return errCnt;
+}
projects / proxmark3-svn / blobdiff