+ if (justNoise(dest, size)) return -1;
+ //make sure buffer has data
+ if (size < 66*64) return -2;
+ // FSK demodulator
+ size = fskdemod(dest, size, 64, 1, 10, 8); // FSK2a RF/64
+ if (size < 65) return -3; //did we get a good demod?
+ //Index map
+ //0 10 20 30 40 50 60
+ //| | | | | | |
+ //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
+ //-----------------------------------------------------------------------------
+ //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
+ //
+ //XSF(version)facility:codeone+codetwo
+ //Handle the data
+ size_t startIdx = 0;
+ uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,1};
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), &size, &startIdx);
+ if (errChk == 0) return -4; //preamble not found
+
+ if (!dest[startIdx+8] && dest[startIdx+17]==1 && dest[startIdx+26]==1 && dest[startIdx+35]==1 && dest[startIdx+44]==1 && dest[startIdx+53]==1){
+ //confirmed proper separator bits found
+ //return start position
+ return (int) startIdx;
+ }
+ return -5;
+}
+
+// by marshmellow
+// takes a array of binary values, start position, length of bits per parity (includes parity bit),
+// Parity Type (1 for odd 0 for even), and binary Length (length to run)
+size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen)
+{
+ uint32_t parityWd = 0;
+ size_t j = 0, bitCnt = 0;
+ for (int word = 0; word < (bLen); word+=pLen){
+ for (int bit=0; bit < pLen; bit++){
+ parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
+ BitStream[j++] = (BitStream[startIdx+word+bit]);
+ }
+ j--;
+ // if parity fails then return 0
+ if (parityTest(parityWd, pLen, pType) == 0) return -1;
+ bitCnt+=(pLen-1);
+ parityWd = 0;
+ }
+ // if we got here then all the parities passed
+ //return ID start index and size
+ return bitCnt;
+}
+
+// by marshmellow
+// FSK Demod then try to locate an AWID ID
+int AWIDdemodFSK(uint8_t *dest, size_t *size)
+{
+ //make sure buffer has enough data
+ if (*size < 96*50) return -1;
+
+ if (justNoise(dest, *size)) return -2;
+
+ // FSK demodulator
+ *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
+ if (*size < 96) return -3; //did we get a good demod?
+
+ uint8_t preamble[] = {0,0,0,0,0,0,0,1};
+ size_t startIdx = 0;
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -4; //preamble not found
+ if (*size != 96) return -5;
+ return (int)startIdx;
+}
+
+// by marshmellow
+// FSK Demod then try to locate an Farpointe Data (pyramid) ID
+int PyramiddemodFSK(uint8_t *dest, size_t *size)
+{
+ //make sure buffer has data
+ if (*size < 128*50) return -5;
+
+ //test samples are not just noise
+ if (justNoise(dest, *size)) return -1;
+
+ // FSK demodulator
+ *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
+ if (*size < 128) return -2; //did we get a good demod?
+
+ uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
+ size_t startIdx = 0;
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -4; //preamble not found
+ if (*size != 128) return -3;
+ return (int)startIdx;
+}
+
+
+uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, int high, int low)
+{
+ uint16_t allPeaks=1;
+ uint16_t cntPeaks=0;
+ size_t loopEnd = 572;
+ if (loopEnd > size) loopEnd = size;
+ for (size_t i=60; i<loopEnd; i++){
+ if (dest[i]>low && dest[i]<high)
+ allPeaks=0;
+ else
+ cntPeaks++;
+ }
+ if (allPeaks == 0){
+ if (cntPeaks > 300) return 1;
+ }
+ return allPeaks;
+}
+
+int DetectStrongAskClock(uint8_t dest[], size_t size)
+{
+ int clk[]={0,8,16,32,40,50,64,100,128,256};
+ size_t idx = 40;
+ uint8_t high=0;
+ size_t cnt = 0;
+ size_t highCnt = 0;
+ size_t highCnt2 = 0;
+ for (;idx < size; idx++){
+ if (dest[idx]>128) {
+ if (!high){
+ high=1;
+ if (cnt > highCnt){
+ if (highCnt != 0) highCnt2 = highCnt;
+ highCnt = cnt;
+ } else if (cnt > highCnt2) {
+ highCnt2 = cnt;
+ }
+ cnt=1;
+ } else {
+ cnt++;
+ }
+ } else if (dest[idx] <= 128){
+ if (high) {
+ high=0;
+ if (cnt > highCnt) {
+ if (highCnt != 0) highCnt2 = highCnt;
+ highCnt = cnt;
+ } else if (cnt > highCnt2) {
+ highCnt2 = cnt;
+ }
+ cnt=1;
+ } else {
+ cnt++;
+ }
+ }
+ }
+ uint8_t tol;
+ for (idx=8; idx>0; idx--){
+ tol = clk[idx]/8;
+ if (clk[idx] >= highCnt - tol && clk[idx] <= highCnt + tol)
+ return clk[idx];
+ if (clk[idx] >= highCnt2 - tol && clk[idx] <= highCnt2 + tol)
+ return clk[idx];
+ }
+ return -1;
+}
+
+// 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)
+{
+ int i=0;
+ int clk[]={8,16,32,40,50,64,100,128,256};
+ int loopCnt = 256; //don't need to loop through entire array...
+ if (size == 0) return -1;
+ if (size<loopCnt) loopCnt = size;
+ //if we already have a valid clock quit
+
+ for (;i<8;++i)
+ if (clk[i] == *clock) return 0;
+
+ //get high and low peak
+ int peak, low;
+ getHiLo(dest, loopCnt, &peak, &low, 75, 75);
+
+ //test for large clean peaks
+ if (DetectCleanAskWave(dest, size, peak, low)==1){
+ int ans = DetectStrongAskClock(dest, size);
+ for (i=7; i>0; i--){
+ if (clk[i] == ans) {
+ *clock=ans;
+ return 0;
+ }
+ }
+ }
+ int ii;
+ int clkCnt;
+ int tol = 0;
+ int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
+ int bestStart[]={0,0,0,0,0,0,0,0,0};
+ int errCnt=0;
+ //test each valid clock from smallest to greatest to see which lines up
+ for(clkCnt=0; clkCnt < 8; clkCnt++){
+ if (clk[clkCnt] == 32){
+ tol=1;
+ }else{
+ tol=0;
+ }
+ if (!maxErr) loopCnt=clk[clkCnt]*2;
+ bestErr[clkCnt]=1000;
+ //try lining up the peaks by moving starting point (try first 256)
+ for (ii=0; ii < loopCnt; ii++){
+ if ((dest[ii] >= peak) || (dest[ii] <= low)){
+ errCnt=0;
+ // now that we have the first one lined up test rest of wave array
+ for (i=0; i<((int)((size-ii-tol)/clk[clkCnt])-1); ++i){
+ if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
+ }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){
+ }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){
+ }else{ //error no peak detected
+ errCnt++;
+ }
+ }
+ //if we found no errors then we can stop here
+ // this is correct one - return this clock
+ //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
+ if(errCnt==0 && clkCnt<6) {
+ *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=0;
+ uint8_t best=0;
+ for (iii=0; iii<8; ++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 (bestErr[best]>maxErr) return -1;
+ *clock=clk[best];
+ return bestStart[best];
+}
+
+//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)
+{
+ 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<loopCnt) loopCnt = size;
+
+ //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, fc=0, fullWaveLen=0, tol=1;
+ uint16_t peakcnt=0, errCnt=0, waveLenCnt=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};
+ countFC(dest, size, &fc);
+ //PrintAndLog("DEBUG: FC: %d",fc);
+
+ //find first full wave
+ for (i=0; i<loopCnt; i++){
+ if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
+ if (waveStart == 0) {
+ waveStart = i+1;
+ //PrintAndLog("DEBUG: waveStart: %d",waveStart);
+ } else {
+ waveEnd = i+1;
+ //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
+ waveLenCnt = waveEnd-waveStart;
+ if (waveLenCnt > fc){
+ firstFullWave = waveStart;
+ fullWaveLen=waveLenCnt;
+ break;
+ }
+ waveStart=0;
+ }
projects / proxmark3-svn / blobdiff