]> cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
5a1007fb160e29cac7541a22ca5c2a413cc969fe
[proxmark3-svn] / common / lfdemod.c
1 //-----------------------------------------------------------------------------
2 // Copyright (C) 2014
3 //
4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
5 // at your option, any later version. See the LICENSE.txt file for the text of
6 // the license.
7 //-----------------------------------------------------------------------------
8 // Low frequency demod/decode commands - by marshmellow, holiman, iceman and
9 // many others who came before
10 //
11 // NOTES:
12 // LF Demod functions are placed here to allow the flexability to use client or
13 // device side. Most BUT NOT ALL of these functions are currenlty safe for
14 // device side use currently. (DetectST for example...)
15 //
16 // There are likely many improvements to the code that could be made, please
17 // make suggestions...
18 //
19 // we tried to include author comments so any questions could be directed to
20 // the source.
21 //
22 // There are 4 main sections of code below:
23 // Utilities Section:
24 // for general utilities used by multiple other functions
25 // Clock / Bitrate Detection Section:
26 // for clock detection functions for each modulation
27 // Modulation Demods &/or Decoding Section:
28 // for main general modulation demodulating and encoding decoding code.
29 // Tag format detection section:
30 // for detection of specific tag formats within demodulated data
31 //
32 // marshmellow
33 //-----------------------------------------------------------------------------
34
35 #include <string.h> // for memset, memcmp and size_t
36 #include <stdint.h> // for uint_32+
37 #include <stdbool.h> // for bool
38
39 //**********************************************************************************************
40 //---------------------------------Utilities Section--------------------------------------------
41 //**********************************************************************************************
42 #define LOWEST_DEFAULT_CLOCK 32
43 #define FSK_PSK_THRESHOLD 123
44
45 //to allow debug print calls when used not on device
46 void dummy(char *fmt, ...){}
47 #ifndef ON_DEVICE
48 #include "ui.h"
49 #include "cmdparser.h"
50 #include "cmddata.h"
51 #define prnt PrintAndLog
52 #else
53 uint8_t g_debugMode=0;
54 #define prnt dummy
55 #endif
56
57 uint8_t justNoise(uint8_t *BitStream, size_t size) {
58 //test samples are not just noise
59 uint8_t justNoise1 = 1;
60 for(size_t idx=0; idx < size && justNoise1 ;idx++){
61 justNoise1 = BitStream[idx] < FSK_PSK_THRESHOLD;
62 }
63 return justNoise1;
64 }
65
66 //by marshmellow
67 //get high and low values of a wave with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise
68 int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo) {
69 *high=0;
70 *low=255;
71 // get high and low thresholds
72 for (size_t i=0; i < size; i++){
73 if (BitStream[i] > *high) *high = BitStream[i];
74 if (BitStream[i] < *low) *low = BitStream[i];
75 }
76 if (*high < FSK_PSK_THRESHOLD) return -1; // just noise
77 *high = ((*high-128)*fuzzHi + 12800)/100;
78 *low = ((*low-128)*fuzzLo + 12800)/100;
79 return 1;
80 }
81
82 // by marshmellow
83 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
84 // returns 1 if passed
85 uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) {
86 uint8_t ans = 0;
87 for (uint8_t i = 0; i < bitLen; i++){
88 ans ^= ((bits >> i) & 1);
89 }
90 if (g_debugMode) prnt("DEBUG: ans: %d, ptype: %d, bits: %08X",ans,pType,bits);
91 return (ans == pType);
92 }
93
94 // by marshmellow
95 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
96 // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run)
97 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) {
98 uint32_t parityWd = 0;
99 size_t j = 0, bitCnt = 0;
100 for (int word = 0; word < (bLen); word+=pLen) {
101 for (int bit=0; bit < pLen; bit++) {
102 parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
103 BitStream[j++] = (BitStream[startIdx+word+bit]);
104 }
105 if (word+pLen > bLen) break;
106
107 j--; // overwrite parity with next data
108 // if parity fails then return 0
109 switch (pType) {
110 case 3: if (BitStream[j]==1) {return 0;} break; //should be 0 spacer bit
111 case 2: if (BitStream[j]==0) {return 0;} break; //should be 1 spacer bit
112 default: if (parityTest(parityWd, pLen, pType) == 0) {return 0;} break; //test parity
113 }
114 bitCnt+=(pLen-1);
115 parityWd = 0;
116 }
117 // if we got here then all the parities passed
118 //return ID start index and size
119 return bitCnt;
120 }
121
122 // by marshmellow
123 // takes a array of binary values, length of bits per parity (includes parity bit),
124 // Parity Type (1 for odd; 0 for even; 2 Always 1's; 3 Always 0's), and binary Length (length to run)
125 // Make sure *dest is long enough to store original sourceLen + #_of_parities_to_be_added
126 size_t addParity(uint8_t *BitSource, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, uint8_t pType) {
127 uint32_t parityWd = 0;
128 size_t j = 0, bitCnt = 0;
129 for (int word = 0; word < sourceLen; word+=pLen-1) {
130 for (int bit=0; bit < pLen-1; bit++){
131 parityWd = (parityWd << 1) | BitSource[word+bit];
132 dest[j++] = (BitSource[word+bit]);
133 }
134 // if parity fails then return 0
135 switch (pType) {
136 case 3: dest[j++]=0; break; // marker bit which should be a 0
137 case 2: dest[j++]=1; break; // marker bit which should be a 1
138 default:
139 dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1;
140 break;
141 }
142 bitCnt += pLen;
143 parityWd = 0;
144 }
145 // if we got here then all the parities passed
146 //return ID start index and size
147 return bitCnt;
148 }
149
150 uint32_t bytebits_to_byte(uint8_t *src, size_t numbits) {
151 uint32_t num = 0;
152 for(int i = 0 ; i < numbits ; i++)
153 {
154 num = (num << 1) | (*src);
155 src++;
156 }
157 return num;
158 }
159
160 //least significant bit first
161 uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits) {
162 uint32_t num = 0;
163 for(int i = 0 ; i < numbits ; i++)
164 {
165 num = (num << 1) | *(src + (numbits-(i+1)));
166 }
167 return num;
168 }
169
170 // search for given preamble in given BitStream and return success=1 or fail=0 and startIndex (where it was found) and length if not fineone
171 // fineone does not look for a repeating preamble for em4x05/4x69 sends preamble once, so look for it once in the first pLen bits
172 bool preambleSearchEx(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx, bool findone) {
173 // Sanity check. If preamble length is bigger than bitstream length.
174 if ( *size <= pLen ) return false;
175
176 uint8_t foundCnt = 0;
177 for (size_t idx = 0; idx < *size - pLen; idx++) {
178 if (memcmp(BitStream+idx, preamble, pLen) == 0) {
179 //first index found
180 foundCnt++;
181 if (foundCnt == 1) {
182 if (g_debugMode) prnt("DEBUG: preamble found at %u", idx);
183 *startIdx = idx;
184 if (findone) return true;
185 } else if (foundCnt == 2) {
186 *size = idx - *startIdx;
187 return true;
188 }
189 }
190 }
191 return false;
192 }
193
194 //by marshmellow
195 //search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
196 uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx) {
197 return (preambleSearchEx(BitStream, preamble, pLen, size, startIdx, false)) ? 1 : 0;
198 }
199
200 // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.
201 size_t findModStart(uint8_t dest[], size_t size, uint8_t expWaveSize) {
202 size_t i = 0;
203 size_t waveSizeCnt = 0;
204 uint8_t thresholdCnt = 0;
205 bool isAboveThreshold = dest[i++] >= FSK_PSK_THRESHOLD;
206 for (; i < size-20; i++ ) {
207 if(dest[i] < FSK_PSK_THRESHOLD && isAboveThreshold) {
208 thresholdCnt++;
209 if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
210 isAboveThreshold = false;
211 waveSizeCnt = 0;
212 } else if (dest[i] >= FSK_PSK_THRESHOLD && !isAboveThreshold) {
213 thresholdCnt++;
214 if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
215 isAboveThreshold = true;
216 waveSizeCnt = 0;
217 } else {
218 waveSizeCnt++;
219 }
220 if (thresholdCnt > 10) break;
221 }
222 if (g_debugMode == 2) prnt("DEBUG: threshold Count reached at %u, count: %u",i, thresholdCnt);
223 return i;
224 }
225
226 int getClosestClock(int testclk) {
227 uint8_t fndClk[] = {8,16,32,40,50,64,128};
228
229 for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++)
230 if (testclk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && testclk <= fndClk[clkCnt]+1)
231 return fndClk[clkCnt];
232
233 return 0;
234 }
235
236 void getNextLow(uint8_t samples[], size_t size, int low, size_t *i) {
237 while ((samples[*i] > low) && (*i < size))
238 *i+=1;
239 }
240
241 void getNextHigh(uint8_t samples[], size_t size, int high, size_t *i) {
242 while ((samples[*i] < high) && (*i < size))
243 *i+=1;
244 }
245
246 // load wave counters
247 bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) {
248 size_t i=0, firstLow, firstHigh;
249 size_t testsize = (size < 512) ? size : 512;
250
251 if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) {
252 if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
253 return false; //just noise
254 }
255
256 // get to first full low to prime loop and skip incomplete first pulse
257 getNextHigh(samples, size, *high, &i);
258 getNextLow(samples, size, *low, &i);
259 *skip = i;
260
261 // populate tmpbuff buffer with pulse lengths
262 while (i < size) {
263 // measure from low to low
264 firstLow = i;
265 //find first high point for this wave
266 getNextHigh(samples, size, *high, &i);
267 firstHigh = i;
268
269 getNextLow(samples, size, *low, &i);
270
271 if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK))
272 break;
273
274 highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low
275 lowToLowWaveLen[*waveCnt] = i - firstLow;
276 *waveCnt += 1;
277 if (i-firstLow < *minClk && i < size) {
278 *minClk = i - firstLow;
279 }
280 }
281 return true;
282 }
283
284 //by marshmellow
285 //amplify based on ask edge detection - not accurate enough to use all the time
286 void askAmp(uint8_t *BitStream, size_t size) {
287 uint8_t Last = 128;
288 for(size_t i = 1; i<size; i++){
289 if (BitStream[i]-BitStream[i-1]>=30) //large jump up
290 Last = 255;
291 else if(BitStream[i-1]-BitStream[i]>=20) //large jump down
292 Last = 0;
293
294 BitStream[i-1] = Last;
295 }
296 return;
297 }
298
299 uint32_t manchesterEncode2Bytes(uint16_t datain) {
300 uint32_t output = 0;
301 uint8_t curBit = 0;
302 for (uint8_t i=0; i<16; i++) {
303 curBit = (datain >> (15-i) & 1);
304 output |= (1<<(((15-i)*2)+curBit));
305 }
306 return output;
307 }
308
309 //by marshmellow
310 //encode binary data into binary manchester
311 //NOTE: BitStream must have double the size available in memory to do the swap
312 int ManchesterEncode(uint8_t *BitStream, size_t size) {
313 size_t modIdx=size, i=0;
314 if (size>modIdx) return -1;
315 for (size_t idx=0; idx < size; idx++){
316 BitStream[idx+modIdx++] = BitStream[idx];
317 BitStream[idx+modIdx++] = BitStream[idx]^1;
318 }
319 for (; i<(size*2); i++){
320 BitStream[i] = BitStream[i+size];
321 }
322 return i;
323 }
324
325 // by marshmellow
326 // to detect a wave that has heavily clipped (clean) samples
327 uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low) {
328 bool allArePeaks = true;
329 uint16_t cntPeaks=0;
330 size_t loopEnd = 512+160;
331 if (loopEnd > size) loopEnd = size;
332 for (size_t i=160; i<loopEnd; i++){
333 if (dest[i]>low && dest[i]<high)
334 allArePeaks = false;
335 else
336 cntPeaks++;
337 }
338 if (!allArePeaks){
339 if (cntPeaks > 300) return true;
340 }
341 return allArePeaks;
342 }
343
344 //**********************************************************************************************
345 //-------------------Clock / Bitrate Detection Section------------------------------------------
346 //**********************************************************************************************
347
348 // by marshmellow
349 // to help detect clocks on heavily clipped samples
350 // based on count of low to low
351 int DetectStrongAskClock(uint8_t dest[], size_t size, int high, int low, int *clock) {
352 size_t startwave;
353 size_t i = 100;
354 size_t minClk = 255;
355 int shortestWaveIdx = 0;
356 // get to first full low to prime loop and skip incomplete first pulse
357 getNextHigh(dest, size, high, &i);
358 getNextLow(dest, size, low, &i);
359
360 // loop through all samples
361 while (i < size) {
362 // measure from low to low
363 startwave = i;
364
365 getNextHigh(dest, size, high, &i);
366 getNextLow(dest, size, low, &i);
367 //get minimum measured distance
368 if (i-startwave < minClk && i < size) {
369 minClk = i - startwave;
370 shortestWaveIdx = startwave;
371 }
372 }
373 // set clock
374 if (g_debugMode==2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d",minClk);
375 *clock = getClosestClock(minClk);
376 if (*clock == 0)
377 return 0;
378
379 return shortestWaveIdx;
380 }
381
382 // by marshmellow
383 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
384 // maybe somehow adjust peak trimming value based on samples to fix?
385 // return start index of best starting position for that clock and return clock (by reference)
386 int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) {
387 size_t i=1;
388 uint8_t clk[] = {255,8,16,32,40,50,64,100,128,255};
389 uint8_t clkEnd = 9;
390 uint8_t loopCnt = 255; //don't need to loop through entire array...
391 if (size <= loopCnt+60) return -1; //not enough samples
392 size -= 60; //sometimes there is a strange end wave - filter out this....
393 //if we already have a valid clock
394 uint8_t clockFnd=0;
395 for (;i<clkEnd;++i)
396 if (clk[i] == *clock) clockFnd = i;
397 //clock found but continue to find best startpos
398
399 //get high and low peak
400 int peak, low;
401 if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return -1;
402
403 //test for large clean peaks
404 if (!clockFnd){
405 if (DetectCleanAskWave(dest, size, peak, low)==1){
406 int ans = DetectStrongAskClock(dest, size, peak, low, clock);
407 if (g_debugMode==2) prnt("DEBUG ASK: detectaskclk Clean Ask Wave Detected: clk %i, ShortestWave: %i",clock, ans);
408 if (ans > 0) {
409 return ans; //return shortest wave start position
410 }
411 }
412 }
413 uint8_t ii;
414 uint8_t clkCnt, tol = 0;
415 uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
416 uint8_t bestStart[]={0,0,0,0,0,0,0,0,0};
417 size_t errCnt = 0;
418 size_t arrLoc, loopEnd;
419
420 if (clockFnd>0) {
421 clkCnt = clockFnd;
422 clkEnd = clockFnd+1;
423 }
424 else clkCnt=1;
425
426 //test each valid clock from smallest to greatest to see which lines up
427 for(; clkCnt < clkEnd; clkCnt++){
428 if (clk[clkCnt] <= 32){
429 tol=1;
430 }else{
431 tol=0;
432 }
433 //if no errors allowed - keep start within the first clock
434 if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt]<128) loopCnt=clk[clkCnt]*2;
435 bestErr[clkCnt]=1000;
436 //try lining up the peaks by moving starting point (try first few clocks)
437 for (ii=0; ii < loopCnt; ii++){
438 if (dest[ii] < peak && dest[ii] > low) continue;
439
440 errCnt=0;
441 // now that we have the first one lined up test rest of wave array
442 loopEnd = ((size-ii-tol) / clk[clkCnt]) - 1;
443 for (i=0; i < loopEnd; ++i){
444 arrLoc = ii + (i * clk[clkCnt]);
445 if (dest[arrLoc] >= peak || dest[arrLoc] <= low){
446 }else if (dest[arrLoc-tol] >= peak || dest[arrLoc-tol] <= low){
447 }else if (dest[arrLoc+tol] >= peak || dest[arrLoc+tol] <= low){
448 }else{ //error no peak detected
449 errCnt++;
450 }
451 }
452 //if we found no errors then we can stop here and a low clock (common clocks)
453 // this is correct one - return this clock
454 if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d",clk[clkCnt],errCnt,ii,i);
455 if(errCnt==0 && clkCnt<7) {
456 if (!clockFnd) *clock = clk[clkCnt];
457 return ii;
458 }
459 //if we found errors see if it is lowest so far and save it as best run
460 if(errCnt<bestErr[clkCnt]){
461 bestErr[clkCnt]=errCnt;
462 bestStart[clkCnt]=ii;
463 }
464 }
465 }
466 uint8_t iii;
467 uint8_t best=0;
468 for (iii=1; iii<clkEnd; ++iii){
469 if (bestErr[iii] < bestErr[best]){
470 if (bestErr[iii] == 0) bestErr[iii]=1;
471 // current best bit to error ratio vs new bit to error ratio
472 if ( (size/clk[best])/bestErr[best] < (size/clk[iii])/bestErr[iii] ){
473 best = iii;
474 }
475 }
476 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]);
477 }
478 if (!clockFnd) *clock = clk[best];
479 return bestStart[best];
480 }
481
482 int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low){
483 //find shortest transition from high to low
484 size_t i = 0;
485 size_t transition1 = 0;
486 int lowestTransition = 255;
487 bool lastWasHigh = false;
488
489 //find first valid beginning of a high or low wave
490 while ((dest[i] >= peak || dest[i] <= low) && (i < size))
491 ++i;
492 while ((dest[i] < peak && dest[i] > low) && (i < size))
493 ++i;
494 lastWasHigh = (dest[i] >= peak);
495
496 if (i==size) return 0;
497 transition1 = i;
498
499 for (;i < size; i++) {
500 if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) {
501 lastWasHigh = (dest[i] >= peak);
502 if (i-transition1 < lowestTransition) lowestTransition = i-transition1;
503 transition1 = i;
504 }
505 }
506 if (lowestTransition == 255) lowestTransition = 0;
507 if (g_debugMode==2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d",lowestTransition);
508 return lowestTransition;
509 }
510
511 //by marshmellow
512 //detect nrz clock by reading #peaks vs no peaks(or errors)
513 int DetectNRZClock(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) {
514 size_t i=0;
515 uint8_t clk[]={8,16,32,40,50,64,100,128,255};
516 size_t loopCnt = 4096; //don't need to loop through entire array...
517 if (size == 0) return 0;
518 if (size<loopCnt) loopCnt = size-20;
519 //if we already have a valid clock quit
520 for (; i < 8; ++i)
521 if (clk[i] == clock) return clock;
522
523 //get high and low peak
524 int peak, low;
525 if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return 0;
526
527 int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low);
528 size_t ii;
529 uint8_t clkCnt;
530 uint8_t tol = 0;
531 uint16_t smplCnt = 0;
532 int16_t peakcnt = 0;
533 int16_t peaksdet[] = {0,0,0,0,0,0,0,0};
534 uint16_t maxPeak = 255;
535 bool firstpeak = false;
536 //test for large clipped waves
537 for (i=0; i<loopCnt; i++){
538 if (dest[i] >= peak || dest[i] <= low){
539 if (!firstpeak) continue;
540 smplCnt++;
541 } else {
542 firstpeak=true;
543 if (smplCnt > 6 ){
544 if (maxPeak > smplCnt){
545 maxPeak = smplCnt;
546 //prnt("maxPk: %d",maxPeak);
547 }
548 peakcnt++;
549 //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt);
550 smplCnt=0;
551 }
552 }
553 }
554 bool errBitHigh = 0;
555 bool bitHigh = 0;
556 uint8_t ignoreCnt = 0;
557 uint8_t ignoreWindow = 4;
558 bool lastPeakHigh = 0;
559 int lastBit = 0;
560 size_t bestStart[]={0,0,0,0,0,0,0,0,0};
561 peakcnt=0;
562 //test each valid clock from smallest to greatest to see which lines up
563 for(clkCnt=0; clkCnt < 8; ++clkCnt){
564 //ignore clocks smaller than smallest peak
565 if (clk[clkCnt] < maxPeak - (clk[clkCnt]/4)) continue;
566 //try lining up the peaks by moving starting point (try first 256)
567 for (ii=20; ii < loopCnt; ++ii){
568 if ((dest[ii] >= peak) || (dest[ii] <= low)){
569 peakcnt = 0;
570 bitHigh = false;
571 ignoreCnt = 0;
572 lastBit = ii-clk[clkCnt];
573 //loop through to see if this start location works
574 for (i = ii; i < size-20; ++i) {
575 //if we are at a clock bit
576 if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) {
577 //test high/low
578 if (dest[i] >= peak || dest[i] <= low) {
579 //if same peak don't count it
580 if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) {
581 peakcnt++;
582 }
583 lastPeakHigh = (dest[i] >= peak);
584 bitHigh = true;
585 errBitHigh = false;
586 ignoreCnt = ignoreWindow;
587 lastBit += clk[clkCnt];
588 } else if (i == lastBit + clk[clkCnt] + tol) {
589 lastBit += clk[clkCnt];
590 }
591 //else if not a clock bit and no peaks
592 } else if (dest[i] < peak && dest[i] > low){
593 if (ignoreCnt==0){
594 bitHigh=false;
595 if (errBitHigh==true) peakcnt--;
596 errBitHigh=false;
597 } else {
598 ignoreCnt--;
599 }
600 // else if not a clock bit but we have a peak
601 } else if ((dest[i]>=peak || dest[i]<=low) && (!bitHigh)) {
602 //error bar found no clock...
603 errBitHigh=true;
604 }
605 }
606 if(peakcnt>peaksdet[clkCnt]) {
607 bestStart[clkCnt]=ii;
608 peaksdet[clkCnt]=peakcnt;
609 }
610 }
611 }
612 }
613 int iii=7;
614 uint8_t best=0;
615 for (iii=7; iii > 0; iii--){
616 if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) {
617 if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) {
618 best = iii;
619 }
620 } else if (peaksdet[iii] > peaksdet[best]){
621 best = iii;
622 }
623 if (g_debugMode==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, maxPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],maxPeak, clk[best], lowestTransition);
624 }
625 *clockStartIdx = bestStart[best];
626 return clk[best];
627 }
628
629 //by marshmellow
630 //countFC is to detect the field clock lengths.
631 //counts and returns the 2 most common wave lengths
632 //mainly used for FSK field clock detection
633 uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) {
634 uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
635 uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
636 uint8_t fcLensFnd = 0;
637 uint8_t lastFCcnt = 0;
638 uint8_t fcCounter = 0;
639 size_t i;
640 if (size < 180) return 0;
641
642 // prime i to first up transition
643 for (i = 160; i < size-20; i++)
644 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
645 break;
646
647 for (; i < size-20; i++){
648 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
649 // new up transition
650 fcCounter++;
651 if (fskAdj){
652 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
653 if (lastFCcnt==5 && fcCounter==9) fcCounter--;
654 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
655 if ((fcCounter==9) || fcCounter==4) fcCounter++;
656 // save last field clock count (fc/xx)
657 lastFCcnt = fcCounter;
658 }
659 // find which fcLens to save it to:
660 for (int ii=0; ii<15; ii++){
661 if (fcLens[ii]==fcCounter){
662 fcCnts[ii]++;
663 fcCounter=0;
664 break;
665 }
666 }
667 if (fcCounter>0 && fcLensFnd<15){
668 //add new fc length
669 fcCnts[fcLensFnd]++;
670 fcLens[fcLensFnd++]=fcCounter;
671 }
672 fcCounter=0;
673 } else {
674 // count sample
675 fcCounter++;
676 }
677 }
678
679 uint8_t best1=14, best2=14, best3=14;
680 uint16_t maxCnt1=0;
681 // go through fclens and find which ones are bigest 2
682 for (i=0; i<15; i++){
683 // get the 3 best FC values
684 if (fcCnts[i]>maxCnt1) {
685 best3=best2;
686 best2=best1;
687 maxCnt1=fcCnts[i];
688 best1=i;
689 } else if(fcCnts[i]>fcCnts[best2]){
690 best3=best2;
691 best2=i;
692 } else if(fcCnts[i]>fcCnts[best3]){
693 best3=i;
694 }
695 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]);
696 }
697 if (fcLens[best1]==0) return 0;
698 uint8_t fcH=0, fcL=0;
699 if (fcLens[best1]>fcLens[best2]){
700 fcH=fcLens[best1];
701 fcL=fcLens[best2];
702 } else{
703 fcH=fcLens[best2];
704 fcL=fcLens[best1];
705 }
706 if ((size-180)/fcH/3 > fcCnts[best1]+fcCnts[best2]) {
707 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]);
708 return 0; //lots of waves not psk or fsk
709 }
710 // TODO: take top 3 answers and compare to known Field clocks to get top 2
711
712 uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
713 if (fskAdj) return fcs;
714 return fcLens[best1];
715 }
716
717 //by marshmellow
718 //detect psk clock by reading each phase shift
719 // a phase shift is determined by measuring the sample length of each wave
720 int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseShift) {
721 uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
722 uint16_t loopCnt = 4096; //don't need to loop through entire array...
723 if (size == 0) return 0;
724 if (size<loopCnt) loopCnt = size-20;
725
726 //if we already have a valid clock quit
727 size_t i=1;
728 for (; i < 8; ++i)
729 if (clk[i] == clock) return clock;
730
731 size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
732 uint8_t clkCnt, fc=0, fullWaveLen=0, tol=1;
733 uint16_t peakcnt=0, errCnt=0, waveLenCnt=0;
734 uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
735 uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0};
736 fc = countFC(dest, size, 0);
737 if (fc!=2 && fc!=4 && fc!=8) return -1;
738 if (g_debugMode==2) prnt("DEBUG PSK: FC: %d",fc);
739
740 //find first full wave
741 for (i=160; i<loopCnt; i++){
742 if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
743 if (waveStart == 0) {
744 waveStart = i+1;
745 //prnt("DEBUG: waveStart: %d",waveStart);
746 } else {
747 waveEnd = i+1;
748 //prnt("DEBUG: waveEnd: %d",waveEnd);
749 waveLenCnt = waveEnd-waveStart;
750 if (waveLenCnt > fc){
751 firstFullWave = waveStart;
752 fullWaveLen=waveLenCnt;
753 break;
754 }
755 waveStart=0;
756 }
757 }
758 }
759 *firstPhaseShift = firstFullWave;
760 if (g_debugMode ==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
761 //test each valid clock from greatest to smallest to see which lines up
762 for(clkCnt=7; clkCnt >= 1 ; clkCnt--){
763 lastClkBit = firstFullWave; //set end of wave as clock align
764 waveStart = 0;
765 errCnt=0;
766 peakcnt=0;
767 if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
768
769 for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){
770 //top edge of wave = start of new wave
771 if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
772 if (waveStart == 0) {
773 waveStart = i+1;
774 waveLenCnt=0;
775 } else { //waveEnd
776 waveEnd = i+1;
777 waveLenCnt = waveEnd-waveStart;
778 if (waveLenCnt > fc){
779 //if this wave is a phase shift
780 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);
781 if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
782 peakcnt++;
783 lastClkBit+=clk[clkCnt];
784 } else if (i<lastClkBit+8){
785 //noise after a phase shift - ignore
786 } else { //phase shift before supposed to based on clock
787 errCnt++;
788 }
789 } else if (i+1 > lastClkBit + clk[clkCnt] + tol + fc){
790 lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
791 }
792 waveStart=i+1;
793 }
794 }
795 }
796 if (errCnt == 0){
797 return clk[clkCnt];
798 }
799 if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt]=errCnt;
800 if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt]=peakcnt;
801 }
802 //all tested with errors
803 //return the highest clk with the most peaks found
804 uint8_t best=7;
805 for (i=7; i>=1; i--){
806 if (peaksdet[i] > peaksdet[best]) {
807 best = i;
808 }
809 if (g_debugMode == 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[i],peaksdet[i],bestErr[i],clk[best]);
810 }
811 return clk[best];
812 }
813
814 int DetectPSKClock(uint8_t dest[], size_t size, int clock) {
815 int firstPhaseShift = 0;
816 return DetectPSKClock_ext(dest, size, clock, &firstPhaseShift);
817 }
818
819 //by marshmellow
820 //detects the bit clock for FSK given the high and low Field Clocks
821 uint8_t detectFSKClk_ext(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow, int *firstClockEdge) {
822 uint8_t clk[] = {8,16,32,40,50,64,100,128,0};
823 uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
824 uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
825 uint8_t rfLensFnd = 0;
826 uint8_t lastFCcnt = 0;
827 uint16_t fcCounter = 0;
828 uint16_t rfCounter = 0;
829 uint8_t firstBitFnd = 0;
830 size_t i;
831 if (size == 0) return 0;
832
833 uint8_t fcTol = ((fcHigh*100 - fcLow*100)/2 + 50)/100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2);
834 rfLensFnd=0;
835 fcCounter=0;
836 rfCounter=0;
837 firstBitFnd=0;
838 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
839 // prime i to first peak / up transition
840 for (i = 160; i < size-20; i++)
841 if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1])
842 break;
843
844 for (; i < size-20; i++){
845 fcCounter++;
846 rfCounter++;
847
848 if (BitStream[i] <= BitStream[i-1] || BitStream[i] < BitStream[i+1])
849 continue;
850 // else new peak
851 // if we got less than the small fc + tolerance then set it to the small fc
852 // if it is inbetween set it to the last counter
853 if (fcCounter < fcHigh && fcCounter > fcLow)
854 fcCounter = lastFCcnt;
855 else if (fcCounter < fcLow+fcTol)
856 fcCounter = fcLow;
857 else //set it to the large fc
858 fcCounter = fcHigh;
859
860 //look for bit clock (rf/xx)
861 if ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)){
862 //not the same size as the last wave - start of new bit sequence
863 if (firstBitFnd > 1){ //skip first wave change - probably not a complete bit
864 for (int ii=0; ii<15; ii++){
865 if (rfLens[ii] >= (rfCounter-4) && rfLens[ii] <= (rfCounter+4)){
866 rfCnts[ii]++;
867 rfCounter = 0;
868 break;
869 }
870 }
871 if (rfCounter > 0 && rfLensFnd < 15){
872 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
873 rfCnts[rfLensFnd]++;
874 rfLens[rfLensFnd++] = rfCounter;
875 }
876 } else {
877 *firstClockEdge = i;
878 firstBitFnd++;
879 }
880 rfCounter=0;
881 lastFCcnt=fcCounter;
882 }
883 fcCounter=0;
884 }
885 uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15;
886
887 for (i=0; i<15; i++){
888 //get highest 2 RF values (might need to get more values to compare or compare all?)
889 if (rfCnts[i]>rfCnts[rfHighest]){
890 rfHighest3=rfHighest2;
891 rfHighest2=rfHighest;
892 rfHighest=i;
893 } else if(rfCnts[i]>rfCnts[rfHighest2]){
894 rfHighest3=rfHighest2;
895 rfHighest2=i;
896 } else if(rfCnts[i]>rfCnts[rfHighest3]){
897 rfHighest3=i;
898 }
899 if (g_debugMode==2) prnt("DEBUG FSK: RF %d, cnts %d",rfLens[i], rfCnts[i]);
900 }
901 // set allowed clock remainder tolerance to be 1 large field clock length+1
902 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
903 uint8_t tol1 = fcHigh+1;
904
905 if (g_debugMode==2) prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
906
907 // loop to find the highest clock that has a remainder less than the tolerance
908 // compare samples counted divided by
909 // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less)
910 int ii=7;
911 for (; ii>=2; ii--){
912 if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){
913 if (rfLens[rfHighest2] % clk[ii] < tol1 || rfLens[rfHighest2] % clk[ii] > clk[ii]-tol1){
914 if (rfLens[rfHighest3] % clk[ii] < tol1 || rfLens[rfHighest3] % clk[ii] > clk[ii]-tol1){
915 if (g_debugMode==2) prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance",clk[ii]);
916 break;
917 }
918 }
919 }
920 }
921
922 if (ii<2) return 0; // oops we went too far
923
924 return clk[ii];
925 }
926
927 uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow) {
928 int firstClockEdge = 0;
929 return detectFSKClk_ext(BitStream, size, fcHigh, fcLow, &firstClockEdge);
930 }
931
932 //**********************************************************************************************
933 //--------------------Modulation Demods &/or Decoding Section-----------------------------------
934 //**********************************************************************************************
935
936 // 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...
937 bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) {
938 for (; *i < buffSize - 4; *i+=1) {
939 *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...
940 if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior
941 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
942 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
943 if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
944 *stStopLoc = *i + 3;
945 return true;
946 }
947 }
948 }
949 }
950 }
951 return false;
952 }
953 //by marshmellow
954 //attempt to identify a Sequence Terminator in ASK modulated raw wave
955 bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststart, size_t *stend) {
956 size_t bufsize = *size;
957 //need to loop through all samples and identify our clock, look for the ST pattern
958 int clk = 0;
959 int tol = 0;
960 int j, high, low, skip, start, end, minClk=255;
961 size_t i = 0;
962 //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
963 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
964 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...
965 //size_t testsize = (bufsize < 512) ? bufsize : 512;
966 int phaseoff = 0;
967 high = low = 128;
968 memset(tmpbuff, 0, sizeof(tmpbuff));
969 memset(waveLen, 0, sizeof(waveLen));
970
971 if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false;
972 // set clock - might be able to get this externally and remove this work...
973 clk = getClosestClock(minClk);
974 // clock not found - ERROR
975 if (clk == 0) {
976 if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
977 return false;
978 }
979 *foundclock = clk;
980
981 tol = clk/8;
982 if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) {
983 // first ST not found - ERROR
984 if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
985 return false;
986 } else {
987 if (g_debugMode==2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j);
988 }
989 if (waveLen[i+2] > clk*1+tol)
990 phaseoff = 0;
991 else
992 phaseoff = clk/2;
993
994 // skip over the remainder of ST
995 skip += clk*7/2; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point
996
997 // now do it again to find the end
998 int dummy1 = 0;
999 end = skip;
1000 i+=3;
1001 if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) {
1002 //didn't find second ST - ERROR
1003 if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting");
1004 return false;
1005 }
1006 end -= phaseoff;
1007 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);
1008 //now begin to trim out ST so we can use normal demod cmds
1009 start = skip;
1010 size_t datalen = end - start;
1011 // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock
1012 if ( clk - (datalen % clk) <= clk/8) {
1013 // padd the amount off - could be problematic... but shouldn't happen often
1014 datalen += clk - (datalen % clk);
1015 } else if ( (datalen % clk) <= clk/8 ) {
1016 // padd the amount off - could be problematic... but shouldn't happen often
1017 datalen -= datalen % clk;
1018 } else {
1019 if (g_debugMode==2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk);
1020 return false;
1021 }
1022 // if datalen is less than one t55xx block - ERROR
1023 if (datalen/clk < 8*4) {
1024 if (g_debugMode==2) prnt("DEBUG STT: datalen is less than 1 full t55xx block - quitting");
1025 return false;
1026 }
1027 size_t dataloc = start;
1028 if (buffer[dataloc-(clk*4)-(clk/8)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {
1029 //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start
1030 for ( i=0; i <= (clk/8); ++i ) {
1031 if ( buffer[dataloc - (clk*4) - i] <= low ) {
1032 dataloc -= i;
1033 break;
1034 }
1035 }
1036 }
1037
1038 size_t newloc = 0;
1039 i=0;
1040 if (g_debugMode==2) prnt("DEBUG STT: Starting STT trim - start: %d, datalen: %d ",dataloc, datalen);
1041 bool firstrun = true;
1042 // warning - overwriting buffer given with raw wave data with ST removed...
1043 while ( dataloc < bufsize-(clk/2) ) {
1044 //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)
1045 if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+3]<high && buffer[dataloc+3]>low) {
1046 for(i=0; i < clk/2-tol; ++i) {
1047 buffer[dataloc+i] = high+5;
1048 }
1049 } //test for single sample outlier (high between two lows) in the case of very strong waves
1050 if (buffer[dataloc] >= high && buffer[dataloc+2] <= low) {
1051 buffer[dataloc] = buffer[dataloc+2];
1052 buffer[dataloc+1] = buffer[dataloc+2];
1053 }
1054 if (firstrun) {
1055 *stend = dataloc;
1056 *ststart = dataloc-(clk*4);
1057 firstrun=false;
1058 }
1059 for (i=0; i<datalen; ++i) {
1060 if (i+newloc < bufsize) {
1061 if (i+newloc < dataloc)
1062 buffer[i+newloc] = buffer[dataloc];
1063
1064 dataloc++;
1065 }
1066 }
1067 newloc += i;
1068 //skip next ST - we just assume it will be there from now on...
1069 if (g_debugMode==2) prnt("DEBUG STT: skipping STT at %d to %d", dataloc, dataloc+(clk*4));
1070 dataloc += clk*4;
1071 }
1072 *size = newloc;
1073 return true;
1074 }
1075 bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
1076 size_t ststart = 0, stend = 0;
1077 return DetectST_ext(buffer, size, foundclock, &ststart, &stend);
1078 }
1079
1080 //by marshmellow
1081 //take 11 10 01 11 00 and make 01100 ... miller decoding
1082 //check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
1083 //decodes miller encoded binary
1084 //NOTE askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!
1085 int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) {
1086 if (*size < 16) return -1;
1087 uint16_t MaxBits = 512, errCnt = 0;
1088 size_t i, bitCnt=0;
1089 uint8_t alignCnt = 0, curBit = BitStream[0], alignedIdx = 0;
1090 uint8_t halfClkErr = 0;
1091 //find alignment, needs 4 1s or 0s to properly align
1092 for (i=1; i < *size-1; i++) {
1093 alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0;
1094 curBit = BitStream[i];
1095 if (alignCnt == 4) break;
1096 }
1097 // for now error if alignment not found. later add option to run it with multiple offsets...
1098 if (alignCnt != 4) {
1099 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");
1100 return -1;
1101 }
1102 alignedIdx = (i-1) % 2;
1103 for (i=alignedIdx; i < *size-3; i+=2) {
1104 halfClkErr = (uint8_t)((halfClkErr << 1 | BitStream[i]) & 0xFF);
1105 if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) {
1106 errCnt++;
1107 BitStream[bitCnt++] = 7;
1108 continue;
1109 }
1110 BitStream[bitCnt++] = BitStream[i] ^ BitStream[i+1] ^ invert;
1111
1112 if (bitCnt > MaxBits) break;
1113 }
1114 *size = bitCnt;
1115 return errCnt;
1116 }
1117
1118 //by marshmellow
1119 //take 01 or 10 = 1 and 11 or 00 = 0
1120 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
1121 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
1122 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert) {
1123 uint16_t bitnum = 0;
1124 uint16_t errCnt = 0;
1125 size_t i = offset;
1126 uint16_t MaxBits=512;
1127 //if not enough samples - error
1128 if (*size < 51) return -1;
1129 //check for phase change faults - skip one sample if faulty
1130 uint8_t offsetA = 1, offsetB = 1;
1131 for (; i<48; i+=2){
1132 if (BitStream[i+1]==BitStream[i+2]) offsetA=0;
1133 if (BitStream[i+2]==BitStream[i+3]) offsetB=0;
1134 }
1135 if (!offsetA && offsetB) offset++;
1136 for (i=offset; i<*size-3; i+=2){
1137 //check for phase error
1138 if (BitStream[i+1]==BitStream[i+2]) {
1139 BitStream[bitnum++]=7;
1140 errCnt++;
1141 }
1142 if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
1143 BitStream[bitnum++]=1^invert;
1144 } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
1145 BitStream[bitnum++]=invert;
1146 } else {
1147 BitStream[bitnum++]=7;
1148 errCnt++;
1149 }
1150 if(bitnum>MaxBits) break;
1151 }
1152 *size=bitnum;
1153 return errCnt;
1154 }
1155
1156 //by marshmellow
1157 //take 10 and 01 and manchester decode
1158 //run through 2 times and take least errCnt
1159 int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert, uint8_t *alignPos) {
1160 uint16_t bitnum=0, MaxBits = 512, errCnt = 0;
1161 size_t i, ii;
1162 uint16_t bestErr = 1000, bestRun = 0;
1163 if (*size < 16) return -1;
1164 //find correct start position [alignment]
1165 for (ii=0;ii<2;++ii){
1166 for (i=ii; i<*size-3; i+=2)
1167 if (BitStream[i]==BitStream[i+1])
1168 errCnt++;
1169
1170 if (bestErr>errCnt){
1171 bestErr=errCnt;
1172 bestRun=ii;
1173 }
1174 errCnt=0;
1175 }
1176 *alignPos=bestRun;
1177 //decode
1178 for (i=bestRun; i < *size-3; i+=2){
1179 if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
1180 BitStream[bitnum++]=invert;
1181 } else if((BitStream[i] == 0) && BitStream[i+1] == 1){
1182 BitStream[bitnum++]=invert^1;
1183 } else {
1184 BitStream[bitnum++]=7;
1185 }
1186 if(bitnum>MaxBits) break;
1187 }
1188 *size=bitnum;
1189 return bestErr;
1190 }
1191
1192 //by marshmellow
1193 //demodulates strong heavily clipped samples
1194 int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low, int *startIdx)
1195 {
1196 *startIdx=0;
1197 size_t bitCnt=0, smplCnt=1, errCnt=0;
1198 bool waveHigh = (BinStream[0] >= high);
1199 for (size_t i=1; i < *size; i++){
1200 if (BinStream[i] >= high && waveHigh){
1201 smplCnt++;
1202 } else if (BinStream[i] <= low && !waveHigh){
1203 smplCnt++;
1204 } else { //transition
1205 if ((BinStream[i] >= high && !waveHigh) || (BinStream[i] <= low && waveHigh)){
1206 if (smplCnt > clk-(clk/4)-1) { //full clock
1207 if (smplCnt > clk + (clk/4)+1) { //too many samples
1208 errCnt++;
1209 if (g_debugMode==2) prnt("DEBUG ASK: Modulation Error at: %u", i);
1210 BinStream[bitCnt++] = 7;
1211 } else if (waveHigh) {
1212 BinStream[bitCnt++] = invert;
1213 BinStream[bitCnt++] = invert;
1214 } else if (!waveHigh) {
1215 BinStream[bitCnt++] = invert ^ 1;
1216 BinStream[bitCnt++] = invert ^ 1;
1217 }
1218 if (*startIdx==0) *startIdx = i-clk;
1219 waveHigh = !waveHigh;
1220 smplCnt = 0;
1221 } else if (smplCnt > (clk/2) - (clk/4)-1) { //half clock
1222 if (waveHigh) {
1223 BinStream[bitCnt++] = invert;
1224 } else if (!waveHigh) {
1225 BinStream[bitCnt++] = invert ^ 1;
1226 }
1227 if (*startIdx==0) *startIdx = i-(clk/2);
1228 waveHigh = !waveHigh;
1229 smplCnt = 0;
1230 } else {
1231 smplCnt++;
1232 //transition bit oops
1233 }
1234 } else { //haven't hit new high or new low yet
1235 smplCnt++;
1236 }
1237 }
1238 }
1239 *size = bitCnt;
1240 return errCnt;
1241 }
1242
1243 //by marshmellow
1244 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
1245 int askdemod_ext(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType, int *startIdx) {
1246 if (*size==0) return -1;
1247 int start = DetectASKClock(BinStream, *size, clk, maxErr); //clock default
1248 if (*clk==0 || start < 0) return -3;
1249 if (*invert != 1) *invert = 0;
1250 if (amp==1) askAmp(BinStream, *size);
1251 if (g_debugMode==2) prnt("DEBUG ASK: clk %d, beststart %d, amp %d", *clk, start, amp);
1252
1253 //start pos from detect ask clock is 1/2 clock offset
1254 // NOTE: can be negative (demod assumes rest of wave was there)
1255 *startIdx = start - (*clk/2);
1256 uint8_t initLoopMax = 255;
1257 if (initLoopMax > *size) initLoopMax = *size;
1258 // Detect high and lows
1259 //25% clip in case highs and lows aren't clipped [marshmellow]
1260 int high, low;
1261 if (getHiLo(BinStream, initLoopMax, &high, &low, 75, 75) < 1)
1262 return -2; //just noise
1263
1264 size_t errCnt = 0;
1265 // if clean clipped waves detected run alternate demod
1266 if (DetectCleanAskWave(BinStream, *size, high, low)) {
1267 if (g_debugMode==2) prnt("DEBUG ASK: Clean Wave Detected - using clean wave demod");
1268 errCnt = cleanAskRawDemod(BinStream, size, *clk, *invert, high, low, startIdx);
1269 if (askType) { //askman
1270 uint8_t alignPos = 0;
1271 errCnt = manrawdecode(BinStream, size, 0, &alignPos);
1272 *startIdx += *clk/2 * alignPos;
1273 if (g_debugMode) prnt("DEBUG ASK CLEAN: startIdx %i, alignPos %u", *startIdx, alignPos);
1274 return errCnt;
1275 } else { //askraw
1276 return errCnt;
1277 }
1278 }
1279 if (g_debugMode) prnt("DEBUG ASK WEAK: startIdx %i", *startIdx);
1280 if (g_debugMode==2) prnt("DEBUG ASK: Weak Wave Detected - using weak wave demod");
1281
1282 int lastBit; //set first clock check - can go negative
1283 size_t i, bitnum = 0; //output counter
1284 uint8_t midBit = 0;
1285 uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
1286 if (*clk <= 32) tol = 1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely
1287 size_t MaxBits = 3072; //max bits to collect
1288 lastBit = start - *clk;
1289
1290 for (i = start; i < *size; ++i) {
1291 if (i-lastBit >= *clk-tol){
1292 if (BinStream[i] >= high) {
1293 BinStream[bitnum++] = *invert;
1294 } else if (BinStream[i] <= low) {
1295 BinStream[bitnum++] = *invert ^ 1;
1296 } else if (i-lastBit >= *clk+tol) {
1297 if (bitnum > 0) {
1298 if (g_debugMode==2) prnt("DEBUG ASK: Modulation Error at: %u", i);
1299 BinStream[bitnum++]=7;
1300 errCnt++;
1301 }
1302 } else { //in tolerance - looking for peak
1303 continue;
1304 }
1305 midBit = 0;
1306 lastBit += *clk;
1307 } else if (i-lastBit >= (*clk/2-tol) && !midBit && !askType){
1308 if (BinStream[i] >= high) {
1309 BinStream[bitnum++] = *invert;
1310 } else if (BinStream[i] <= low) {
1311 BinStream[bitnum++] = *invert ^ 1;
1312 } else if (i-lastBit >= *clk/2+tol) {
1313 BinStream[bitnum] = BinStream[bitnum-1];
1314 bitnum++;
1315 } else { //in tolerance - looking for peak
1316 continue;
1317 }
1318 midBit = 1;
1319 }
1320 if (bitnum >= MaxBits) break;
1321 }
1322 *size = bitnum;
1323 return errCnt;
1324 }
1325
1326 int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType) {
1327 int start = 0;
1328 return askdemod_ext(BinStream, size, clk, invert, maxErr, amp, askType, &start);
1329 }
1330
1331 // by marshmellow - demodulate NRZ wave - requires a read with strong signal
1332 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1333 int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) {
1334 if (justNoise(dest, *size)) return -1;
1335 size_t clkStartIdx = 0;
1336 *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx);
1337 if (*clk==0) return -2;
1338 size_t i, gLen = 4096;
1339 if (gLen>*size) gLen = *size-20;
1340 int high, low;
1341 if (getHiLo(dest, gLen, &high, &low, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low
1342
1343 uint8_t bit=0;
1344 //convert wave samples to 1's and 0's
1345 for(i=20; i < *size-20; i++){
1346 if (dest[i] >= high) bit = 1;
1347 if (dest[i] <= low) bit = 0;
1348 dest[i] = bit;
1349 }
1350 //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit)
1351 size_t lastBit = 0;
1352 size_t numBits = 0;
1353 for(i=21; i < *size-20; i++) {
1354 //if transition detected or large number of same bits - store the passed bits
1355 if (dest[i] != dest[i-1] || (i-lastBit) == (10 * *clk)) {
1356 memset(dest+numBits, dest[i-1] ^ *invert, (i - lastBit + (*clk/4)) / *clk);
1357 numBits += (i - lastBit + (*clk/4)) / *clk;
1358 if (lastBit == 0) {
1359 *startIdx = i - (numBits * *clk);
1360 if (g_debugMode==2) prnt("DEBUG NRZ: startIdx %i", *startIdx);
1361 }
1362 lastBit = i-1;
1363 }
1364 }
1365 *size = numBits;
1366 return 0;
1367 }
1368
1369 //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq])
1370 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) {
1371 size_t last_transition = 0;
1372 size_t idx = 1;
1373 if (fchigh==0) fchigh=10;
1374 if (fclow==0) fclow=8;
1375 //set the threshold close to 0 (graph) or 128 std to avoid static
1376 size_t preLastSample = 0;
1377 size_t LastSample = 0;
1378 size_t currSample = 0;
1379 if ( size < 1024 ) return 0; // not enough samples
1380
1381 //find start of modulating data in trace
1382 idx = findModStart(dest, size, fchigh);
1383 // Need to threshold first sample
1384 if(dest[idx] < FSK_PSK_THRESHOLD) dest[0] = 0;
1385 else dest[0] = 1;
1386
1387 last_transition = idx;
1388 idx++;
1389 size_t numBits = 0;
1390 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
1391 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with anywhere
1392 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
1393 // (could also be fc/5 && fc/7 for fsk1 = 4-9)
1394 for(; idx < size; idx++) {
1395 // threshold current value
1396 if (dest[idx] < FSK_PSK_THRESHOLD) dest[idx] = 0;
1397 else dest[idx] = 1;
1398
1399 // Check for 0->1 transition
1400 if (dest[idx-1] < dest[idx]) {
1401 preLastSample = LastSample;
1402 LastSample = currSample;
1403 currSample = idx-last_transition;
1404 if (currSample < (fclow-2)) { //0-5 = garbage noise (or 0-3)
1405 //do nothing with extra garbage
1406 } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves (or 3-6 = 5)
1407 //correct previous 9 wave surrounded by 8 waves (or 6 surrounded by 5)
1408 if (numBits > 1 && LastSample > (fchigh-2) && (preLastSample < (fchigh-1))){
1409 dest[numBits-1]=1;
1410 }
1411 dest[numBits++]=1;
1412 if (numBits > 0 && *startIdx==0) *startIdx = idx - fclow;
1413 } else if (currSample > (fchigh+1) && numBits < 3) { //12 + and first two bit = unusable garbage
1414 //do nothing with beginning garbage and reset.. should be rare..
1415 numBits = 0;
1416 } else if (currSample == (fclow+1) && LastSample == (fclow-1)) { // had a 7 then a 9 should be two 8's (or 4 then a 6 should be two 5's)
1417 dest[numBits++]=1;
1418 if (numBits > 0 && *startIdx==0) *startIdx = idx - fclow;
1419 } else { //9+ = 10 sample waves (or 6+ = 7)
1420 dest[numBits++]=0;
1421 if (numBits > 0 && *startIdx==0) *startIdx = idx - fchigh;
1422 }
1423 last_transition = idx;
1424 }
1425 }
1426 return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
1427 }
1428
1429 //translate 11111100000 to 10
1430 //rfLen = clock, fchigh = larger field clock, fclow = smaller field clock
1431 size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow, int *startIdx) {
1432 uint8_t lastval=dest[0];
1433 size_t idx=0;
1434 size_t numBits=0;
1435 uint32_t n=1;
1436 for( idx=1; idx < size; idx++) {
1437 n++;
1438 if (dest[idx]==lastval) continue; //skip until we hit a transition
1439
1440 //find out how many bits (n) we collected (use 1/2 clk tolerance)
1441 //if lastval was 1, we have a 1->0 crossing
1442 if (dest[idx-1]==1) {
1443 n = (n * fclow + rfLen/2) / rfLen;
1444 } else {// 0->1 crossing
1445 n = (n * fchigh + rfLen/2) / rfLen;
1446 }
1447 if (n == 0) n = 1;
1448
1449 //first transition - save startidx
1450 if (numBits == 0) {
1451 if (lastval == 1) { //high to low
1452 *startIdx += (fclow * idx) - (n*rfLen);
1453 if (g_debugMode==2) prnt("DEBUG FSK: startIdx %i, fclow*idx %i, n*rflen %u", *startIdx, fclow*(idx), n*rfLen);
1454 } else {
1455 *startIdx += (fchigh * idx) - (n*rfLen);
1456 if (g_debugMode==2) prnt("DEBUG FSK: startIdx %i, fchigh*idx %i, n*rflen %u", *startIdx, fchigh*(idx), n*rfLen);
1457 }
1458 }
1459
1460 //add to our destination the bits we collected
1461 memset(dest+numBits, dest[idx-1]^invert , n);
1462 numBits += n;
1463 n=0;
1464 lastval=dest[idx];
1465 }//end for
1466 // if valid extra bits at the end were all the same frequency - add them in
1467 if (n > rfLen/fchigh) {
1468 if (dest[idx-2]==1) {
1469 n = (n * fclow + rfLen/2) / rfLen;
1470 } else {
1471 n = (n * fchigh + rfLen/2) / rfLen;
1472 }
1473 memset(dest+numBits, dest[idx-1]^invert , n);
1474 numBits += n;
1475 }
1476 return numBits;
1477 }
1478
1479 //by marshmellow (from holiman's base)
1480 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
1481 int fskdemod_ext(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow, int *startIdx) {
1482 // FSK demodulator
1483 size = fsk_wave_demod(dest, size, fchigh, fclow, startIdx);
1484 size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow, startIdx);
1485 return size;
1486 }
1487
1488 int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow) {
1489 int startIdx=0;
1490 return fskdemod_ext(dest, size, rfLen, invert, fchigh, fclow, &startIdx);
1491 }
1492
1493 // by marshmellow
1494 // convert psk1 demod to psk2 demod
1495 // only transition waves are 1s
1496 void psk1TOpsk2(uint8_t *BitStream, size_t size) {
1497 size_t i=1;
1498 uint8_t lastBit=BitStream[0];
1499 for (; i<size; i++){
1500 if (BitStream[i]==7){
1501 //ignore errors
1502 } else if (lastBit!=BitStream[i]){
1503 lastBit=BitStream[i];
1504 BitStream[i]=1;
1505 } else {
1506 BitStream[i]=0;
1507 }
1508 }
1509 return;
1510 }
1511
1512 // by marshmellow
1513 // convert psk2 demod to psk1 demod
1514 // from only transition waves are 1s to phase shifts change bit
1515 void psk2TOpsk1(uint8_t *BitStream, size_t size) {
1516 uint8_t phase=0;
1517 for (size_t i=0; i<size; i++){
1518 if (BitStream[i]==1){
1519 phase ^=1;
1520 }
1521 BitStream[i]=phase;
1522 }
1523 return;
1524 }
1525
1526 size_t pskFindFirstPhaseShift(uint8_t samples[], size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) {
1527 uint16_t loopCnt = (size+3 < 4096) ? size : 4096; //don't need to loop through entire array...
1528
1529 uint16_t avgWaveVal=0, lastAvgWaveVal=0;
1530 size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave;
1531 for (; i<loopCnt; i++) {
1532 // find peak
1533 if (samples[i]+fc < samples[i+1] && samples[i+1] >= samples[i+2]){
1534 waveEnd = i+1;
1535 if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart);
1536 waveLenCnt = waveEnd-waveStart;
1537 if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack
1538 lastAvgWaveVal = avgWaveVal/(waveLenCnt);
1539 firstFullWave = waveStart;
1540 *fullWaveLen = waveLenCnt;
1541 //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
1542 if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1;
1543 return firstFullWave;
1544 }
1545 waveStart = i+1;
1546 avgWaveVal = 0;
1547 }
1548 avgWaveVal += samples[i+2];
1549 }
1550 return 0;
1551 }
1552
1553 //by marshmellow - demodulate PSK1 wave
1554 //uses wave lengths (# Samples)
1555 int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *startIdx) {
1556 if (*size < 170) return -1;
1557
1558 uint8_t curPhase = *invert;
1559 size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
1560 uint16_t fc=0, fullWaveLen=0, waveLenCnt=0, avgWaveVal, tol=1;
1561 uint16_t errCnt=0, errCnt2=0;
1562
1563 fc = countFC(dest, *size, 1);
1564 if ((fc >> 8) == 10) return -1; //fsk found - quit
1565 fc = fc & 0xFF;
1566 if (fc!=2 && fc!=4 && fc!=8) return -1;
1567 *clock = DetectPSKClock(dest, *size, *clock);
1568 if (*clock == 0) return -1;
1569
1570 //find start of modulating data in trace
1571 i = findModStart(dest, *size, fc);
1572
1573 //find first phase shift
1574 firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen);
1575 if (firstFullWave == 0) {
1576 // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
1577 // so skip a little to ensure we are past any Start Signal
1578 firstFullWave = 160;
1579 memset(dest, curPhase, firstFullWave / *clock);
1580 } else {
1581 memset(dest, curPhase^1, firstFullWave / *clock);
1582 }
1583 //advance bits
1584 numBits += (firstFullWave / *clock);
1585 *startIdx = firstFullWave - (*clock * numBits)+2;
1586 //set start of wave as clock align
1587 lastClkBit = firstFullWave;
1588 if (g_debugMode==2) prnt("DEBUG PSK: firstFullWave: %u, waveLen: %u, startIdx %i",firstFullWave,fullWaveLen, *startIdx);
1589 if (g_debugMode==2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit,(unsigned int) fc);
1590 waveStart = 0;
1591 dest[numBits++] = curPhase; //set first read bit
1592 for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){
1593 //top edge of wave = start of new wave
1594 if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
1595 if (waveStart == 0) {
1596 waveStart = i+1;
1597 waveLenCnt = 0;
1598 avgWaveVal = dest[i+1];
1599 } else { //waveEnd
1600 waveEnd = i+1;
1601 waveLenCnt = waveEnd-waveStart;
1602 if (waveLenCnt > fc){
1603 //this wave is a phase shift
1604 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1605 if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
1606 curPhase ^= 1;
1607 dest[numBits++] = curPhase;
1608 lastClkBit += *clock;
1609 } else if (i < lastClkBit+10+fc){
1610 //noise after a phase shift - ignore
1611 } else { //phase shift before supposed to based on clock
1612 errCnt++;
1613 dest[numBits++] = 7;
1614 }
1615 } else if (i+1 > lastClkBit + *clock + tol + fc){
1616 lastClkBit += *clock; //no phase shift but clock bit
1617 dest[numBits++] = curPhase;
1618 } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often)
1619 errCnt2++;
1620 if(errCnt2 > 101) return errCnt2;
1621 }
1622 avgWaveVal = 0;
1623 waveStart = i+1;
1624 }
1625 }
1626 avgWaveVal += dest[i+1];
1627 }
1628 *size = numBits;
1629 return errCnt;
1630 }
1631
1632 int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) {
1633 int startIdx = 0;
1634 return pskRawDemod_ext(dest, size, clock, invert, &startIdx);
1635 }
1636
1637 //**********************************************************************************************
1638 //-----------------Tag format detection section-------------------------------------------------
1639 //**********************************************************************************************
1640
1641 // by marshmellow
1642 // FSK Demod then try to locate an AWID ID
1643 int AWIDdemodFSK(uint8_t *dest, size_t *size) {
1644 //make sure buffer has enough data
1645 if (*size < 96*50) return -1;
1646
1647 if (justNoise(dest, *size)) return -2;
1648
1649 // FSK demodulator
1650 *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
1651 if (*size < 96) return -3; //did we get a good demod?
1652
1653 uint8_t preamble[] = {0,0,0,0,0,0,0,1};
1654 size_t startIdx = 0;
1655 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1656 if (errChk == 0) return -4; //preamble not found
1657 if (*size != 96) return -5;
1658 return (int)startIdx;
1659 }
1660
1661 //by marshmellow
1662 //takes 1s and 0s and searches for EM410x format - output EM ID
1663 uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo)
1664 {
1665 //sanity checks
1666 if (*size < 64) return 0;
1667 if (BitStream[1]>1) return 0; //allow only 1s and 0s
1668
1669 // 111111111 bit pattern represent start of frame
1670 // include 0 in front to help get start pos
1671 uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1};
1672 uint8_t errChk = 0;
1673 uint8_t FmtLen = 10; // sets of 4 bits = end data
1674 *startIdx = 0;
1675 errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, startIdx);
1676 if ( errChk == 0 || (*size != 64 && *size != 128) ) return 0;
1677 if (*size == 128) FmtLen = 22; // 22 sets of 4 bits
1678
1679 //skip last 4bit parity row for simplicity
1680 *size = removeParity(BitStream, *startIdx + sizeof(preamble), 5, 0, FmtLen * 5);
1681 if (*size == 40) { // std em410x format
1682 *hi = 0;
1683 *lo = ((uint64_t)(bytebits_to_byte(BitStream, 8)) << 32) | (bytebits_to_byte(BitStream + 8, 32));
1684 } else if (*size == 88) { // long em format
1685 *hi = (bytebits_to_byte(BitStream, 24));
1686 *lo = ((uint64_t)(bytebits_to_byte(BitStream + 24, 32)) << 32) | (bytebits_to_byte(BitStream + 24 + 32, 32));
1687 } else {
1688 return 0;
1689 }
1690 return 1;
1691 }
1692
1693 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
1694 // BitStream must contain previously askrawdemod and biphasedemoded data
1695 int FDXBdemodBI(uint8_t *dest, size_t *size) {
1696 //make sure buffer has enough data
1697 if (*size < 128) return -1;
1698
1699 size_t startIdx = 0;
1700 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,1};
1701
1702 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1703 if (errChk == 0) return -2; //preamble not found
1704 return (int)startIdx;
1705 }
1706
1707 // by marshmellow
1708 // demod gProxIIDemod
1709 // error returns as -x
1710 // success returns start position in BitStream
1711 // BitStream must contain previously askrawdemod and biphasedemoded data
1712 int gProxII_Demod(uint8_t BitStream[], size_t *size) {
1713 size_t startIdx=0;
1714 uint8_t preamble[] = {1,1,1,1,1,0};
1715
1716 uint8_t errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, &startIdx);
1717 if (errChk == 0) return -3; //preamble not found
1718 if (*size != 96) return -2; //should have found 96 bits
1719 //check first 6 spacer bits to verify format
1720 if (!BitStream[startIdx+5] && !BitStream[startIdx+10] && !BitStream[startIdx+15] && !BitStream[startIdx+20] && !BitStream[startIdx+25] && !BitStream[startIdx+30]){
1721 //confirmed proper separator bits found
1722 //return start position
1723 return (int) startIdx;
1724 }
1725 return -5; //spacer bits not found - not a valid gproxII
1726 }
1727
1728 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
1729 int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo) {
1730 if (justNoise(dest, *size)) return -1;
1731
1732 size_t numStart=0, size2=*size, startIdx=0;
1733 // FSK demodulator
1734 *size = fskdemod(dest, size2,50,1,10,8); //fsk2a
1735 if (*size < 96*2) return -2;
1736 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
1737 uint8_t preamble[] = {0,0,0,1,1,1,0,1};
1738 // find bitstring in array
1739 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1740 if (errChk == 0) return -3; //preamble not found
1741
1742 numStart = startIdx + sizeof(preamble);
1743 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
1744 for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
1745 if (dest[idx] == dest[idx+1]){
1746 return -4; //not manchester data
1747 }
1748 *hi2 = (*hi2<<1)|(*hi>>31);
1749 *hi = (*hi<<1)|(*lo>>31);
1750 //Then, shift in a 0 or one into low
1751 if (dest[idx] && !dest[idx+1]) // 1 0
1752 *lo=(*lo<<1)|1;
1753 else // 0 1
1754 *lo=(*lo<<1)|0;
1755 }
1756 return (int)startIdx;
1757 }
1758
1759 int IOdemodFSK(uint8_t *dest, size_t size) {
1760 if (justNoise(dest, size)) return -1;
1761 //make sure buffer has data
1762 if (size < 66*64) return -2;
1763 // FSK demodulator
1764 size = fskdemod(dest, size, 64, 1, 10, 8); // FSK2a RF/64
1765 if (size < 65) return -3; //did we get a good demod?
1766 //Index map
1767 //0 10 20 30 40 50 60
1768 //| | | | | | |
1769 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
1770 //-----------------------------------------------------------------------------
1771 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
1772 //
1773 //XSF(version)facility:codeone+codetwo
1774 //Handle the data
1775 size_t startIdx = 0;
1776 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,1};
1777 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), &size, &startIdx);
1778 if (errChk == 0) return -4; //preamble not found
1779
1780 if (!dest[startIdx+8] && dest[startIdx+17]==1 && dest[startIdx+26]==1 && dest[startIdx+35]==1 && dest[startIdx+44]==1 && dest[startIdx+53]==1){
1781 //confirmed proper separator bits found
1782 //return start position
1783 return (int) startIdx;
1784 }
1785 return -5;
1786 }
1787
1788 // redesigned by marshmellow adjusted from existing decode functions
1789 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1790 int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert) {
1791 //26 bit 40134 format (don't know other formats)
1792 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
1793 uint8_t preamble_i[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0};
1794 size_t startidx = 0;
1795 if (!preambleSearch(bitStream, preamble, sizeof(preamble), size, &startidx)){
1796 // if didn't find preamble try again inverting
1797 if (!preambleSearch(bitStream, preamble_i, sizeof(preamble_i), size, &startidx)) return -1;
1798 *invert ^= 1;
1799 }
1800 if (*size != 64 && *size != 224) return -2;
1801 if (*invert==1)
1802 for (size_t i = startidx; i < *size; i++)
1803 bitStream[i] ^= 1;
1804
1805 return (int) startidx;
1806 }
1807
1808 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
1809 int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo) {
1810 if (justNoise(dest, *size)) return -1;
1811
1812 size_t numStart=0, size2=*size, startIdx=0;
1813 // FSK demodulator
1814 *size = fskdemod(dest, size2,50,1,10,8); //fsk2a
1815 if (*size < 96) return -2;
1816
1817 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
1818 uint8_t preamble[] = {0,0,0,0,1,1,1,1};
1819
1820 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1821 if (errChk == 0) return -3; //preamble not found
1822
1823 numStart = startIdx + sizeof(preamble);
1824 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
1825 for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
1826 if (dest[idx] == dest[idx+1])
1827 return -4; //not manchester data
1828 *hi2 = (*hi2<<1)|(*hi>>31);
1829 *hi = (*hi<<1)|(*lo>>31);
1830 //Then, shift in a 0 or one into low
1831 if (dest[idx] && !dest[idx+1]) // 1 0
1832 *lo=(*lo<<1)|1;
1833 else // 0 1
1834 *lo=(*lo<<1)|0;
1835 }
1836 return (int)startIdx;
1837 }
1838
1839 // find presco preamble 0x10D in already demoded data
1840 int PrescoDemod(uint8_t *dest, size_t *size) {
1841 //make sure buffer has data
1842 if (*size < 64*2) return -2;
1843
1844 size_t startIdx = 0;
1845 uint8_t preamble[] = {1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0};
1846 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1847 if (errChk == 0) return -4; //preamble not found
1848 //return start position
1849 return (int) startIdx;
1850 }
1851
1852 // by marshmellow
1853 // FSK Demod then try to locate a Farpointe Data (pyramid) ID
1854 int PyramiddemodFSK(uint8_t *dest, size_t *size) {
1855 //make sure buffer has data
1856 if (*size < 128*50) return -5;
1857
1858 //test samples are not just noise
1859 if (justNoise(dest, *size)) return -1;
1860
1861 // FSK demodulator
1862 *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
1863 if (*size < 128) return -2; //did we get a good demod?
1864
1865 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
1866 size_t startIdx = 0;
1867 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1868 if (errChk == 0) return -4; //preamble not found
1869 if (*size != 128) return -3;
1870 return (int)startIdx;
1871 }
1872
1873 // by marshmellow
1874 // find viking preamble 0xF200 in already demoded data
1875 int VikingDemod_AM(uint8_t *dest, size_t *size) {
1876 //make sure buffer has data
1877 if (*size < 64*2) return -2;
1878
1879 size_t startIdx = 0;
1880 uint8_t preamble[] = {1,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1881 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
1882 if (errChk == 0) return -4; //preamble not found
1883 uint32_t checkCalc = bytebits_to_byte(dest+startIdx,8) ^ bytebits_to_byte(dest+startIdx+8,8) ^ bytebits_to_byte(dest+startIdx+16,8)
1884 ^ bytebits_to_byte(dest+startIdx+24,8) ^ bytebits_to_byte(dest+startIdx+32,8) ^ bytebits_to_byte(dest+startIdx+40,8)
1885 ^ bytebits_to_byte(dest+startIdx+48,8) ^ bytebits_to_byte(dest+startIdx+56,8);
1886 if ( checkCalc != 0xA8 ) return -5;
1887 if (*size != 64) return -6;
1888 //return start position
1889 return (int) startIdx;
1890 }
1891
1892 // by iceman
1893 // find Visa2000 preamble in already demoded data
1894 int Visa2kDemod_AM(uint8_t *dest, size_t *size) {
1895 if (*size < 96) return -1; //make sure buffer has data
1896 size_t startIdx = 0;
1897 uint8_t preamble[] = {0,1,0,1,0,1,1,0,0,1,0,0,1,0,0,1,0,1,0,1,0,0,1,1,0,0,1,1,0,0,1,0};
1898 if (preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx) == 0)
1899 return -2; //preamble not found
1900 if (*size != 96) return -3; //wrong demoded size
1901 //return start position
1902 return (int)startIdx;
1903 }
Impressum, Datenschutz