]> git.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
8f1a376422e9e13bb4782a95439f8873c3d57e1d
[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
9 //-----------------------------------------------------------------------------
10
11 #include <stdlib.h>
12 #include <string.h>
13 #include "lfdemod.h"
14
15 //by marshmellow
16 //get high and low with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise
17 int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo)
18 {
19 *high=0;
20 *low=255;
21 // get high and low thresholds
22 for (int i=0; i < size; i++){
23 if (BitStream[i] > *high) *high = BitStream[i];
24 if (BitStream[i] < *low) *low = BitStream[i];
25 }
26 if (*high < 123) return -1; // just noise
27 *high = (int)(((*high-128)*(((float)fuzzHi)/100))+128);
28 *low = (int)(((*low-128)*(((float)fuzzLo)/100))+128);
29 return 1;
30 }
31
32 //by marshmellow
33 //takes 1s and 0s and searches for EM410x format - output EM ID
34 uint64_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx)
35 {
36 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
37 // otherwise could be a void with no arguments
38 //set defaults
39 uint64_t lo=0;
40 uint32_t i = 0;
41 if (BitStream[10]>1){ //allow only 1s and 0s
42 // PrintAndLog("no data found");
43 return 0;
44 }
45 uint8_t parityTest=0;
46 // 111111111 bit pattern represent start of frame
47 uint8_t frame_marker_mask[] = {1,1,1,1,1,1,1,1,1};
48 uint32_t idx = 0;
49 uint32_t ii=0;
50 uint8_t resetCnt = 0;
51 while( (idx + 64) < *size) {
52 restart:
53 // search for a start of frame marker
54 if ( memcmp(BitStream+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
55 { // frame marker found
56 *startIdx=idx;
57 idx+=9;
58 for (i=0; i<10;i++){
59 for(ii=0; ii<5; ++ii){
60 parityTest ^= BitStream[(i*5)+ii+idx];
61 }
62 if (!parityTest){ //even parity
63 parityTest=0;
64 for (ii=0; ii<4;++ii){
65 lo=(lo<<1LL)|(BitStream[(i*5)+ii+idx]);
66 }
67 //PrintAndLog("DEBUG: EM parity passed parity val: %d, i:%d, ii:%d,idx:%d, Buffer: %d%d%d%d%d,lo: %d",parityTest,i,ii,idx,BitStream[idx+ii+(i*5)-5],BitStream[idx+ii+(i*5)-4],BitStream[idx+ii+(i*5)-3],BitStream[idx+ii+(i*5)-2],BitStream[idx+ii+(i*5)-1],lo);
68 }else {//parity failed
69 //PrintAndLog("DEBUG: EM parity failed parity val: %d, i:%d, ii:%d,idx:%d, Buffer: %d%d%d%d%d",parityTest,i,ii,idx,BitStream[idx+ii+(i*5)-5],BitStream[idx+ii+(i*5)-4],BitStream[idx+ii+(i*5)-3],BitStream[idx+ii+(i*5)-2],BitStream[idx+ii+(i*5)-1]);
70 parityTest=0;
71 idx-=8;
72 if (resetCnt>5)return 0; //try 5 times
73 resetCnt++;
74 goto restart;//continue;
75 }
76 }
77 //skip last 5 bit parity test for simplicity.
78 *size = 64;
79 return lo;
80 }else{
81 idx++;
82 }
83 }
84 return 0;
85 }
86
87 //by marshmellow
88 //takes 2 arguments - clock and invert both as integers
89 //attempts to demodulate ask while decoding manchester
90 //prints binary found and saves in graphbuffer for further commands
91 int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert)
92 {
93 int i;
94 int clk2=*clk;
95 *clk=DetectASKClock(BinStream, *size, *clk); //clock default
96
97 // if autodetected too low then adjust //MAY NEED ADJUSTMENT
98 if (clk2==0 && *clk<8) *clk =64;
99 if (clk2==0 && *clk<32) *clk=32;
100 if (*invert != 0 && *invert != 1) *invert=0;
101 uint32_t initLoopMax = 200;
102 if (initLoopMax > *size) initLoopMax=*size;
103 // Detect high and lows
104 // 25% fuzz in case highs and lows aren't clipped [marshmellow]
105 int high, low, ans;
106 ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75);
107 if (ans<1) return -2; //just noise
108
109 // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
110 int lastBit = 0; //set first clock check
111 uint32_t bitnum = 0; //output counter
112 int tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
113 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
114 int iii = 0;
115 uint32_t gLen = *size;
116 if (gLen > 3000) gLen=3000;
117 uint8_t errCnt =0;
118 uint32_t bestStart = *size;
119 uint32_t bestErrCnt = (*size/1000);
120 uint32_t maxErr = (*size/1000);
121 // PrintAndLog("DEBUG - lastbit - %d",lastBit);
122 // loop to find first wave that works
123 for (iii=0; iii < gLen; ++iii){
124 if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){
125 lastBit=iii-*clk;
126 errCnt=0;
127 // loop through to see if this start location works
128 for (i = iii; i < *size; ++i) {
129 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
130 lastBit+=*clk;
131 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
132 //low found and we are expecting a bar
133 lastBit+=*clk;
134 } else {
135 //mid value found or no bar supposed to be here
136 if ((i-lastBit)>(*clk+tol)){
137 //should have hit a high or low based on clock!!
138
139 //debug
140 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
141
142 errCnt++;
143 lastBit+=*clk;//skip over until hit too many errors
144 if (errCnt>(maxErr)) break; //allow 1 error for every 1000 samples else start over
145 }
146 }
147 if ((i-iii) >(400 * *clk)) break; //got plenty of bits
148 }
149 //we got more than 64 good bits and not all errors
150 if ((((i-iii)/ *clk) > (64+errCnt)) && (errCnt<maxErr)) {
151 //possible good read
152 if (errCnt==0){
153 bestStart=iii;
154 bestErrCnt=errCnt;
155 break; //great read - finish
156 }
157 if (errCnt<bestErrCnt){ //set this as new best run
158 bestErrCnt=errCnt;
159 bestStart = iii;
160 }
161 }
162 }
163 }
164 if (bestErrCnt<maxErr){
165 //best run is good enough set to best run and set overwrite BinStream
166 iii=bestStart;
167 lastBit = bestStart - *clk;
168 bitnum=0;
169 for (i = iii; i < *size; ++i) {
170 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
171 lastBit += *clk;
172 BinStream[bitnum] = *invert;
173 bitnum++;
174 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
175 //low found and we are expecting a bar
176 lastBit+=*clk;
177 BinStream[bitnum] = 1-*invert;
178 bitnum++;
179 } else {
180 //mid value found or no bar supposed to be here
181 if ((i-lastBit)>(*clk+tol)){
182 //should have hit a high or low based on clock!!
183
184 //debug
185 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
186 if (bitnum > 0){
187 BinStream[bitnum]=77;
188 bitnum++;
189 }
190
191 lastBit+=*clk;//skip over error
192 }
193 }
194 if (bitnum >=400) break;
195 }
196 *size=bitnum;
197 } else{
198 *invert=bestStart;
199 *clk=iii;
200 return -1;
201 }
202 return bestErrCnt;
203 }
204
205 //by marshmellow
206 //encode binary data into binary manchester
207 int ManchesterEncode(uint8_t *BitStream, size_t size)
208 {
209 size_t modIdx=20000, i=0;
210 if (size>modIdx) return -1;
211 for (size_t idx=0; idx < size; idx++){
212 BitStream[idx+modIdx++] = BitStream[idx];
213 BitStream[idx+modIdx++] = BitStream[idx]^1;
214 }
215 for (; i<(size*2); i++){
216 BitStream[i] = BitStream[i+20000];
217 }
218 return i;
219 }
220
221 //by marshmellow
222 //take 10 and 01 and manchester decode
223 //run through 2 times and take least errCnt
224 int manrawdecode(uint8_t * BitStream, size_t *size)
225 {
226 int bitnum=0;
227 int errCnt =0;
228 int i=1;
229 int bestErr = 1000;
230 int bestRun = 0;
231 int ii=1;
232 for (ii=1;ii<3;++ii){
233 i=1;
234 for (i=i+ii;i<*size-2;i+=2){
235 if(BitStream[i]==1 && (BitStream[i+1]==0)){
236 } else if((BitStream[i]==0)&& BitStream[i+1]==1){
237 } else {
238 errCnt++;
239 }
240 if(bitnum>300) break;
241 }
242 if (bestErr>errCnt){
243 bestErr=errCnt;
244 bestRun=ii;
245 }
246 errCnt=0;
247 }
248 errCnt=bestErr;
249 if (errCnt<20){
250 ii=bestRun;
251 i=1;
252 for (i=i+ii;i < *size-2;i+=2){
253 if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
254 BitStream[bitnum++]=0;
255 } else if((BitStream[i] == 0) && BitStream[i+1] == 1){
256 BitStream[bitnum++]=1;
257 } else {
258 BitStream[bitnum++]=77;
259 //errCnt++;
260 }
261 if(bitnum>300) break;
262 }
263 *size=bitnum;
264 }
265 return errCnt;
266 }
267
268 //by marshmellow
269 //take 01 or 10 = 0 and 11 or 00 = 1
270 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert)
271 {
272 uint8_t bitnum=0;
273 uint32_t errCnt =0;
274 uint32_t i;
275 i=offset;
276 for (;i<*size-2; i+=2){
277 if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
278 BitStream[bitnum++]=1^invert;
279 } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
280 BitStream[bitnum++]=invert;
281 } else {
282 BitStream[bitnum++]=77;
283 errCnt++;
284 }
285 if(bitnum>250) break;
286 }
287 *size=bitnum;
288 return errCnt;
289 }
290
291 //by marshmellow
292 //takes 2 arguments - clock and invert both as integers
293 //attempts to demodulate ask only
294 //prints binary found and saves in graphbuffer for further commands
295 int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert)
296 {
297 uint32_t i;
298 // int invert=0; //invert default
299 int clk2 = *clk;
300 *clk=DetectASKClock(BinStream, *size, *clk); //clock default
301 //uint8_t BitStream[502] = {0};
302
303 //HACK: if clock not detected correctly - default
304 if (clk2==0 && *clk<8) *clk =64;
305 if (clk2==0 && *clk<32 && clk2==0) *clk=32;
306 if (*invert != 0 && *invert != 1) *invert =0;
307 uint32_t initLoopMax = 200;
308 if (initLoopMax > *size) initLoopMax=*size;
309 // Detect high and lows
310 //25% fuzz in case highs and lows aren't clipped [marshmellow]
311 int high, low, ans;
312 ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75);
313 if (ans<1) return -2; //just noise
314
315 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
316 int lastBit = 0; //set first clock check
317 uint32_t bitnum = 0; //output counter
318 uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock
319 // if they fall + or - this value + clock from last valid wave
320 if (*clk == 32) tol=1; //clock tolerance may not be needed anymore currently set to
321 // + or - 1 but could be increased for poor waves or removed entirely
322 uint32_t iii = 0;
323 uint32_t gLen = *size;
324 if (gLen > 500) gLen=500;
325 uint8_t errCnt =0;
326 uint32_t bestStart = *size;
327 uint32_t bestErrCnt = (*size/1000);
328 uint32_t maxErr = bestErrCnt;
329 uint8_t midBit=0;
330 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
331 //loop to find first wave that works
332 for (iii=0; iii < gLen; ++iii){
333 if ((BinStream[iii]>=high) || (BinStream[iii]<=low)){
334 lastBit=iii-*clk;
335 //loop through to see if this start location works
336 for (i = iii; i < *size; ++i) {
337 if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){
338 lastBit+=*clk;
339 //BitStream[bitnum] = *invert;
340 //bitnum++;
341 midBit=0;
342 } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){
343 //low found and we are expecting a bar
344 lastBit+=*clk;
345 //BitStream[bitnum] = 1- *invert;
346 //bitnum++;
347 midBit=0;
348 } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
349 //mid bar?
350 midBit=1;
351 //BitStream[bitnum]= 1- *invert;
352 //bitnum++;
353 } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
354 //mid bar?
355 midBit=1;
356 //BitStream[bitnum]= *invert;
357 //bitnum++;
358 } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){
359 //no mid bar found
360 midBit=1;
361 //BitStream[bitnum]= BitStream[bitnum-1];
362 //bitnum++;
363 } else {
364 //mid value found or no bar supposed to be here
365
366 if ((i-lastBit)>(*clk+tol)){
367 //should have hit a high or low based on clock!!
368 //debug
369 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
370 //if (bitnum > 0){
371 // BitStream[bitnum]=77;
372 // bitnum++;
373 //}
374
375 errCnt++;
376 lastBit+=*clk;//skip over until hit too many errors
377 if (errCnt > ((*size/1000))){ //allow 1 error for every 1000 samples else start over
378 errCnt=0;
379 // bitnum=0;//start over
380 break;
381 }
382 }
383 }
384 if ((i-iii)>(500 * *clk)) break; //got enough bits
385 }
386 //we got more than 64 good bits and not all errors
387 if ((((i-iii)/ *clk) > (64+errCnt)) && (errCnt<(*size/1000))) {
388 //possible good read
389 if (errCnt==0){
390 bestStart=iii;
391 bestErrCnt=errCnt;
392 break; //great read - finish
393 }
394 if (errCnt<bestErrCnt){ //set this as new best run
395 bestErrCnt=errCnt;
396 bestStart = iii;
397 }
398 }
399 }
400 }
401 if (bestErrCnt<maxErr){
402 //best run is good enough - set to best run and overwrite BinStream
403 iii=bestStart;
404 lastBit = bestStart - *clk;
405 bitnum=0;
406 for (i = iii; i < *size; ++i) {
407 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
408 lastBit += *clk;
409 BinStream[bitnum] = *invert;
410 bitnum++;
411 midBit=0;
412 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
413 //low found and we are expecting a bar
414 lastBit+=*clk;
415 BinStream[bitnum] = 1-*invert;
416 bitnum++;
417 midBit=0;
418 } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
419 //mid bar?
420 midBit=1;
421 BinStream[bitnum] = 1 - *invert;
422 bitnum++;
423 } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
424 //mid bar?
425 midBit=1;
426 BinStream[bitnum] = *invert;
427 bitnum++;
428 } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){
429 //no mid bar found
430 midBit=1;
431 if (bitnum!=0) BinStream[bitnum] = BinStream[bitnum-1];
432 bitnum++;
433
434 } else {
435 //mid value found or no bar supposed to be here
436 if ((i-lastBit)>(*clk+tol)){
437 //should have hit a high or low based on clock!!
438
439 //debug
440 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
441 if (bitnum > 0){
442 BinStream[bitnum]=77;
443 bitnum++;
444 }
445
446 lastBit+=*clk;//skip over error
447 }
448 }
449 if (bitnum >=400) break;
450 }
451 *size=bitnum;
452 } else{
453 *invert=bestStart;
454 *clk=iii;
455 return -1;
456 }
457 return bestErrCnt;
458 }
459 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
460 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow)
461 {
462 uint32_t last_transition = 0;
463 uint32_t idx = 1;
464 //uint32_t maxVal=0;
465 if (fchigh==0) fchigh=10;
466 if (fclow==0) fclow=8;
467 //set the threshold close to 0 (graph) or 128 std to avoid static
468 uint8_t threshold_value = 123;
469
470 // sync to first lo-hi transition, and threshold
471
472 // Need to threshold first sample
473
474 if(dest[0] < threshold_value) dest[0] = 0;
475 else dest[0] = 1;
476
477 size_t numBits = 0;
478 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
479 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
480 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
481 for(idx = 1; idx < size; idx++) {
482 // threshold current value
483
484 if (dest[idx] < threshold_value) dest[idx] = 0;
485 else dest[idx] = 1;
486
487 // Check for 0->1 transition
488 if (dest[idx-1] < dest[idx]) { // 0 -> 1 transition
489 if ((idx-last_transition)<(fclow-2)){ //0-5 = garbage noise
490 //do nothing with extra garbage
491 } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves
492 dest[numBits]=1;
493 } else { //9+ = 10 waves
494 dest[numBits]=0;
495 }
496 last_transition = idx;
497 numBits++;
498 }
499 }
500 return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
501 }
502
503 uint32_t myround2(float f)
504 {
505 if (f >= 2000) return 2000;//something bad happened
506 return (uint32_t) (f + (float)0.5);
507 }
508
509 //translate 11111100000 to 10
510 size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxConsequtiveBits,
511 uint8_t invert, uint8_t fchigh, uint8_t fclow)
512 {
513 uint8_t lastval=dest[0];
514 uint32_t idx=0;
515 size_t numBits=0;
516 uint32_t n=1;
517
518 for( idx=1; idx < size; idx++) {
519
520 if (dest[idx]==lastval) {
521 n++;
522 continue;
523 }
524 //if lastval was 1, we have a 1->0 crossing
525 if ( dest[idx-1]==1 ) {
526 n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow));
527 } else {// 0->1 crossing
528 n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh)); //-1 for fudge factor
529 }
530 if (n == 0) n = 1;
531
532 if(n < maxConsequtiveBits) //Consecutive
533 {
534 if(invert==0){ //invert bits
535 memset(dest+numBits, dest[idx-1] , n);
536 }else{
537 memset(dest+numBits, dest[idx-1]^1 , n);
538 }
539 numBits += n;
540 }
541 n=0;
542 lastval=dest[idx];
543 }//end for
544 return numBits;
545 }
546 //by marshmellow (from holiman's base)
547 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
548 int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow)
549 {
550 // FSK demodulator
551 size = fsk_wave_demod(dest, size, fchigh, fclow);
552 size = aggregate_bits(dest, size, rfLen, 192, invert, fchigh, fclow);
553 return size;
554 }
555 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
556 int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
557 {
558
559 size_t idx=0, size2=*size, startIdx=0;
560 // FSK demodulator
561
562 *size = fskdemod(dest, size2,50,0,10,8);
563
564 // final loop, go over previously decoded manchester data and decode into usable tag ID
565 // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
566 uint8_t frame_marker_mask[] = {1,1,1,0,0,0};
567 int numshifts = 0;
568 idx = 0;
569 //one scan
570 while( idx + sizeof(frame_marker_mask) < *size) {
571 // search for a start of frame marker
572 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
573 { // frame marker found
574 startIdx=idx;
575 idx+=sizeof(frame_marker_mask);
576 while(dest[idx] != dest[idx+1] && idx < *size-2)
577 {
578 // Keep going until next frame marker (or error)
579 // Shift in a bit. Start by shifting high registers
580 *hi2 = (*hi2<<1)|(*hi>>31);
581 *hi = (*hi<<1)|(*lo>>31);
582 //Then, shift in a 0 or one into low
583 if (dest[idx] && !dest[idx+1]) // 1 0
584 *lo=(*lo<<1)|0;
585 else // 0 1
586 *lo=(*lo<<1)|1;
587 numshifts++;
588 idx += 2;
589 }
590 // Hopefully, we read a tag and hit upon the next frame marker
591 if(idx + sizeof(frame_marker_mask) < *size)
592 {
593 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
594 {
595 //good return
596 *size=idx-startIdx;
597 return startIdx;
598 }
599 }
600 // reset
601 *hi2 = *hi = *lo = 0;
602 numshifts = 0;
603 }else {
604 idx++;
605 }
606 }
607 return -1;
608 }
609
610 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
611 size_t ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
612 {
613
614 size_t idx=0, size2=*size;
615 // FSK demodulator
616
617 *size = fskdemod(dest, size2,50,1,10,8);
618
619 // final loop, go over previously decoded manchester data and decode into usable tag ID
620 // 00001111 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
621 uint8_t frame_marker_mask[] = {0,0,0,0,1,1,1,1};
622 uint16_t numshifts = 0;
623 idx = 0;
624 //one scan
625 while( idx + sizeof(frame_marker_mask) < *size) {
626 // search for a start of frame marker
627 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
628 { // frame marker found
629 size2=idx;
630 idx+=sizeof(frame_marker_mask);
631 while(dest[idx] != dest[idx+1] && idx < *size-2)
632 {
633 // Keep going until next frame marker (or error)
634 // Shift in a bit. Start by shifting high registers
635 *hi2 = (*hi2<<1)|(*hi>>31);
636 *hi = (*hi<<1)|(*lo>>31);
637 //Then, shift in a 0 or one into low
638 if (dest[idx] && !dest[idx+1]) // 1 0
639 *lo=(*lo<<1)|1;
640 else // 0 1
641 *lo=(*lo<<1)|0;
642 numshifts++;
643 idx += 2;
644 }
645 // Hopefully, we read a tag and hit upon the next frame marker and got enough bits
646 if(idx + sizeof(frame_marker_mask) < *size && numshifts > 40)
647 {
648 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
649 {
650 //good return - return start grid position and bits found
651 *size = ((numshifts*2)+8);
652 return size2;
653 }
654 }
655 // reset
656 *hi2 = *hi = *lo = 0;
657 numshifts = 0;
658 }else {
659 idx++;
660 }
661 }
662 return 0;
663 }
664
665 uint32_t bytebits_to_byte(uint8_t* src, size_t numbits)
666 {
667 uint32_t num = 0;
668 for(int i = 0 ; i < numbits ; i++)
669 {
670 num = (num << 1) | (*src);
671 src++;
672 }
673 return num;
674 }
675
676 int IOdemodFSK(uint8_t *dest, size_t size)
677 {
678 static const uint8_t THRESHOLD = 129;
679 uint32_t idx=0;
680 //make sure buffer has data
681 if (size < 66) return -1;
682 //test samples are not just noise
683 uint8_t justNoise = 1;
684 for(idx=0;idx< size && justNoise ;idx++){
685 justNoise = dest[idx] < THRESHOLD;
686 }
687 if(justNoise) return 0;
688
689 // FSK demodulator
690 size = fskdemod(dest, size, 64, 1, 10, 8); // RF/64 and invert
691 if (size < 65) return -1; //did we get a good demod?
692 //Index map
693 //0 10 20 30 40 50 60
694 //| | | | | | |
695 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
696 //-----------------------------------------------------------------------------
697 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
698 //
699 //XSF(version)facility:codeone+codetwo
700 //Handle the data
701 uint8_t mask[] = {0,0,0,0,0,0,0,0,0,1};
702 for( idx=0; idx < (size - 65); idx++) {
703 if ( memcmp(dest + idx, mask, sizeof(mask))==0) {
704 //frame marker found
705 if (!dest[idx+8] && dest[idx+17]==1 && dest[idx+26]==1 && dest[idx+35]==1 && dest[idx+44]==1 && dest[idx+53]==1){
706 //confirmed proper separator bits found
707 //return start position
708 return (int) idx;
709 }
710 }
711 }
712 return 0;
713 }
714
715 // by marshmellow
716 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
717 // returns 1 if passed
718 uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType)
719 {
720 uint8_t ans = 0;
721 for (uint8_t i = 0; i < bitLen; i++){
722 ans ^= ((bits >> i) & 1);
723 }
724 //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType);
725 return (ans == pType);
726 }
727
728 // by marshmellow
729 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
730 // Parity Type (1 for odd 0 for even), and binary Length (length to run)
731 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen)
732 {
733 uint32_t parityWd = 0;
734 size_t j = 0, bitCnt = 0;
735 for (int word = 0; word < (bLen); word+=pLen){
736 for (int bit=0; bit < pLen; bit++){
737 parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
738 BitStream[j++] = (BitStream[startIdx+word+bit]);
739 }
740 j--;
741 // if parity fails then return 0
742 if (parityTest(parityWd, pLen, pType) == 0) return -1;
743 bitCnt+=(pLen-1);
744 parityWd = 0;
745 }
746 // if we got here then all the parities passed
747 //return ID start index and size
748 return bitCnt;
749 }
750
751 // by marshmellow
752 // FSK Demod then try to locate an AWID ID
753 int AWIDdemodFSK(uint8_t *dest, size_t size)
754 {
755 static const uint8_t THRESHOLD = 123;
756 uint32_t idx=0, idx2=0;
757 //make sure buffer has data
758 if (size < 96*50) return -1;
759 //test samples are not just noise
760 uint8_t justNoise = 1;
761 for(idx=0; idx < size && justNoise ;idx++){
762 justNoise = dest[idx] < THRESHOLD;
763 }
764 if(justNoise) return -2;
765
766 // FSK demodulator
767 size = fskdemod(dest, size, 50, 1, 10, 8); // RF/64 and invert
768 if (size < 96) return -3; //did we get a good demod?
769
770 uint8_t mask[] = {0,0,0,0,0,0,0,1};
771 for( idx=0; idx < (size - 96); idx++) {
772 if ( memcmp(dest + idx, mask, sizeof(mask))==0) {
773 // frame marker found
774 //return ID start index
775 if (idx2 == 0) idx2=idx;
776 else if(idx-idx2==96) return idx2;
777 else return -5;
778
779 // should always get 96 bits if it is awid
780 }
781 }
782 //never found mask
783 return -4;
784 }
785
786 // by marshmellow
787 // FSK Demod then try to locate an Farpointe Data (pyramid) ID
788 int PyramiddemodFSK(uint8_t *dest, size_t size)
789 {
790 static const uint8_t THRESHOLD = 123;
791 uint32_t idx=0, idx2=0;
792 // size_t size2 = size;
793 //make sure buffer has data
794 if (size < 128*50) return -5;
795 //test samples are not just noise
796 uint8_t justNoise = 1;
797 for(idx=0; idx < size && justNoise ;idx++){
798 justNoise = dest[idx] < THRESHOLD;
799 }
800 if(justNoise) return -1;
801
802 // FSK demodulator
803 size = fskdemod(dest, size, 50, 1, 10, 8); // RF/64 and invert
804 if (size < 128) return -2; //did we get a good demod?
805
806 uint8_t mask[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
807 for( idx=0; idx < (size - 128); idx++) {
808 if ( memcmp(dest + idx, mask, sizeof(mask))==0) {
809 // frame marker found
810 if (idx2==0) idx2=idx;
811 else if (idx-idx2==128) return idx2;
812 else return -3;
813 }
814 }
815 //never found mask
816 return -4;
817 }
818
819
820 // by marshmellow
821 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
822 // maybe somehow adjust peak trimming value based on samples to fix?
823 int DetectASKClock(uint8_t dest[], size_t size, int clock)
824 {
825 int i=0;
826 int clk[]={8,16,32,40,50,64,100,128,256};
827 int loopCnt = 256; //don't need to loop through entire array...
828 if (size<loopCnt) loopCnt = size;
829
830 //if we already have a valid clock quit
831
832 for (;i<8;++i)
833 if (clk[i] == clock) return clock;
834
835 //get high and low peak
836 int peak, low;
837 getHiLo(dest, loopCnt, &peak, &low, 75, 75);
838
839 int ii;
840 int clkCnt;
841 int tol = 0;
842 int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
843 int errCnt=0;
844 //test each valid clock from smallest to greatest to see which lines up
845 for(clkCnt=0; clkCnt < 8; ++clkCnt){
846 if (clk[clkCnt] == 32){
847 tol=1;
848 }else{
849 tol=0;
850 }
851 bestErr[clkCnt]=1000;
852 //try lining up the peaks by moving starting point (try first 256)
853 for (ii=0; ii < loopCnt; ++ii){
854 if ((dest[ii] >= peak) || (dest[ii] <= low)){
855 errCnt=0;
856 // now that we have the first one lined up test rest of wave array
857 for (i=0; i<((int)((size-ii-tol)/clk[clkCnt])-1); ++i){
858 if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
859 }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){
860 }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){
861 }else{ //error no peak detected
862 errCnt++;
863 }
864 }
865 //if we found no errors then we can stop here
866 // this is correct one - return this clock
867 //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
868 if(errCnt==0 && clkCnt<6) return clk[clkCnt];
869 //if we found errors see if it is lowest so far and save it as best run
870 if(errCnt<bestErr[clkCnt]) bestErr[clkCnt]=errCnt;
871 }
872 }
873 }
874 uint8_t iii=0;
875 uint8_t best=0;
876 for (iii=0; iii<8; ++iii){
877 if (bestErr[iii]<bestErr[best]){
878 if (bestErr[iii]==0) bestErr[iii]=1;
879 // current best bit to error ratio vs new bit to error ratio
880 if (((size/clk[best])/bestErr[best] < (size/clk[iii])/bestErr[iii]) ){
881 best = iii;
882 }
883 }
884 }
885 return clk[best];
886 }
887
888
889 //by marshmellow
890 //detect psk clock by reading #peaks vs no peaks(or errors)
891 int DetectpskNRZClock(uint8_t dest[], size_t size, int clock)
892 {
893 int i=0;
894 int clk[]={16,32,40,50,64,100,128,256};
895 int loopCnt = 2048; //don't need to loop through entire array...
896 if (size<loopCnt) loopCnt = size;
897
898 //if we already have a valid clock quit
899 for (; i < 7; ++i)
900 if (clk[i] == clock) return clock;
901
902 //get high and low peak
903 int peak, low;
904 getHiLo(dest, loopCnt, &peak, &low, 75, 75);
905
906 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
907 int ii;
908 uint8_t clkCnt;
909 uint8_t tol = 0;
910 int peakcnt=0;
911 int errCnt=0;
912 int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000};
913 int peaksdet[]={0,0,0,0,0,0,0,0};
914 //test each valid clock from smallest to greatest to see which lines up
915 for(clkCnt=0; clkCnt < 7; ++clkCnt){
916 if (clk[clkCnt] <= 32){
917 tol=1;
918 }else{
919 tol=0;
920 }
921 //try lining up the peaks by moving starting point (try first 256)
922 for (ii=0; ii< loopCnt; ++ii){
923 if ((dest[ii] >= peak) || (dest[ii] <= low)){
924 errCnt=0;
925 peakcnt=0;
926 // now that we have the first one lined up test rest of wave array
927 for (i=0; i < ((int)((size-ii-tol)/clk[clkCnt])-1); ++i){
928 if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
929 peakcnt++;
930 }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){
931 peakcnt++;
932 }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){
933 peakcnt++;
934 }else{ //error no peak detected
935 errCnt++;
936 }
937 }
938 if(peakcnt>peaksdet[clkCnt]) {
939 peaksdet[clkCnt]=peakcnt;
940 bestErr[clkCnt]=errCnt;
941 }
942 }
943 }
944 }
945 int iii=0;
946 int best=0;
947 //int ratio2; //debug
948 int ratio;
949 //int bits;
950 for (iii=0; iii < 7; ++iii){
951 ratio=1000;
952 //ratio2=1000; //debug
953 //bits=size/clk[iii]; //debug
954 if (peaksdet[iii] > 0){
955 ratio=bestErr[iii]/peaksdet[iii];
956 if (((bestErr[best]/peaksdet[best]) > (ratio)+1)){
957 best = iii;
958 }
959 //ratio2=bits/peaksdet[iii]; //debug
960 }
961 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d, ratio: %d, bits: %d, peakbitr: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best],ratio, bits,ratio2);
962 }
963 return clk[best];
964 }
965
966 //by marshmellow (attempt to get rid of high immediately after a low)
967 void pskCleanWave(uint8_t *BitStream, size_t size)
968 {
969 int i;
970 int gap = 4;
971 int newLow=0;
972 int newHigh=0;
973 int high, low;
974 getHiLo(BitStream, size, &high, &low, 80, 90);
975
976 for (i=0; i < size; ++i){
977 if (newLow == 1){
978 if (BitStream[i]>low){
979 BitStream[i]=low+8;
980 gap--;
981 }
982 if (gap == 0){
983 newLow=0;
984 gap=4;
985 }
986 }else if (newHigh == 1){
987 if (BitStream[i]<high){
988 BitStream[i]=high-8;
989 gap--;
990 }
991 if (gap == 0){
992 newHigh=0;
993 gap=4;
994 }
995 }
996 if (BitStream[i] <= low) newLow=1;
997 if (BitStream[i] >= high) newHigh=1;
998 }
999 return;
1000 }
1001
1002
1003 //redesigned by marshmellow adjusted from existing decode functions
1004 //indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1005 int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert)
1006 {
1007 //26 bit 40134 format (don't know other formats)
1008 int i;
1009 int long_wait=29;//29 leading zeros in format
1010 int start;
1011 int first = 0;
1012 int first2 = 0;
1013 int bitCnt = 0;
1014 int ii;
1015 // Finding the start of a UID
1016 for (start = 0; start <= *size - 250; start++) {
1017 first = bitStream[start];
1018 for (i = start; i < start + long_wait; i++) {
1019 if (bitStream[i] != first) {
1020 break;
1021 }
1022 }
1023 if (i == (start + long_wait)) {
1024 break;
1025 }
1026 }
1027 if (start == *size - 250 + 1) {
1028 // did not find start sequence
1029 return -1;
1030 }
1031 // Inverting signal if needed
1032 if (first == 1) {
1033 for (i = start; i < *size; i++) {
1034 bitStream[i] = !bitStream[i];
1035 }
1036 *invert = 1;
1037 }else *invert=0;
1038
1039 int iii;
1040 //found start once now test length by finding next one
1041 for (ii=start+29; ii <= *size - 250; ii++) {
1042 first2 = bitStream[ii];
1043 for (iii = ii; iii < ii + long_wait; iii++) {
1044 if (bitStream[iii] != first2) {
1045 break;
1046 }
1047 }
1048 if (iii == (ii + long_wait)) {
1049 break;
1050 }
1051 }
1052 if (ii== *size - 250 + 1){
1053 // did not find second start sequence
1054 return -2;
1055 }
1056 bitCnt=ii-start;
1057
1058 // Dumping UID
1059 i = start;
1060 for (ii = 0; ii < bitCnt; ii++) {
1061 bitStream[ii] = bitStream[i++];
1062 }
1063 *size=bitCnt;
1064 return 1;
1065 }
1066
1067
1068 //by marshmellow - demodulate PSK1 wave or NRZ wave (both similar enough)
1069 //peaks switch bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1070 int pskNRZrawDemod(uint8_t *dest, size_t *size, int *clk, int *invert)
1071 {
1072 pskCleanWave(dest,*size);
1073 int clk2 = DetectpskNRZClock(dest, *size, *clk);
1074 *clk=clk2;
1075 uint32_t i;
1076 int high, low, ans;
1077 ans = getHiLo(dest, 1260, &high, &low, 75, 80); //25% fuzz on high 20% fuzz on low
1078 if (ans<1) return -2; //just noise
1079 uint32_t gLen = *size;
1080 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
1081 int lastBit = 0; //set first clock check
1082 uint32_t bitnum = 0; //output counter
1083 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
1084 if (*clk==32) tol = 2; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely
1085 uint32_t iii = 0;
1086 uint8_t errCnt =0;
1087 uint32_t bestStart = *size;
1088 uint32_t maxErr = (*size/1000);
1089 uint32_t bestErrCnt = maxErr;
1090 //uint8_t midBit=0;
1091 uint8_t curBit=0;
1092 uint8_t bitHigh=0;
1093 uint8_t ignorewin=*clk/8;
1094 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
1095 //loop to find first wave that works - align to clock
1096 for (iii=0; iii < gLen; ++iii){
1097 if ((dest[iii]>=high) || (dest[iii]<=low)){
1098 lastBit=iii-*clk;
1099 //loop through to see if this start location works
1100 for (i = iii; i < *size; ++i) {
1101 //if we found a high bar and we are at a clock bit
1102 if ((dest[i]>=high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1103 bitHigh=1;
1104 lastBit+=*clk;
1105 ignorewin=*clk/8;
1106 bitnum++;
1107 //else if low bar found and we are at a clock point
1108 }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1109 bitHigh=1;
1110 lastBit+=*clk;
1111 ignorewin=*clk/8;
1112 bitnum++;
1113 //else if no bars found
1114 }else if(dest[i] < high && dest[i] > low) {
1115 if (ignorewin==0){
1116 bitHigh=0;
1117 }else ignorewin--;
1118 //if we are past a clock point
1119 if (i >= lastBit+*clk+tol){ //clock val
1120 lastBit+=*clk;
1121 bitnum++;
1122 }
1123 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1124 }else if ((dest[i]>=high || dest[i]<=low) && (i<lastBit+*clk-tol || i>lastBit+*clk+tol) && (bitHigh==0)){
1125 //error bar found no clock...
1126 errCnt++;
1127 }
1128 if (bitnum>=1000) break;
1129 }
1130 //we got more than 64 good bits and not all errors
1131 if ((bitnum > (64+errCnt)) && (errCnt < (maxErr))) {
1132 //possible good read
1133 if (errCnt == 0){
1134 bestStart = iii;
1135 bestErrCnt = errCnt;
1136 break; //great read - finish
1137 }
1138 if (errCnt < bestErrCnt){ //set this as new best run
1139 bestErrCnt = errCnt;
1140 bestStart = iii;
1141 }
1142 }
1143 }
1144 }
1145 if (bestErrCnt < maxErr){
1146 //best run is good enough set to best run and set overwrite BinStream
1147 iii=bestStart;
1148 lastBit=bestStart-*clk;
1149 bitnum=0;
1150 for (i = iii; i < *size; ++i) {
1151 //if we found a high bar and we are at a clock bit
1152 if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1153 bitHigh=1;
1154 lastBit+=*clk;
1155 curBit=1-*invert;
1156 dest[bitnum]=curBit;
1157 ignorewin=*clk/8;
1158 bitnum++;
1159 //else if low bar found and we are at a clock point
1160 }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1161 bitHigh=1;
1162 lastBit+=*clk;
1163 curBit=*invert;
1164 dest[bitnum]=curBit;
1165 ignorewin=*clk/8;
1166 bitnum++;
1167 //else if no bars found
1168 }else if(dest[i]<high && dest[i]>low) {
1169 if (ignorewin==0){
1170 bitHigh=0;
1171 }else ignorewin--;
1172 //if we are past a clock point
1173 if (i>=lastBit+*clk+tol){ //clock val
1174 lastBit+=*clk;
1175 dest[bitnum]=curBit;
1176 bitnum++;
1177 }
1178 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1179 }else if ((dest[i]>=high || dest[i]<=low) && ((i<lastBit+*clk-tol) || (i>lastBit+*clk+tol)) && (bitHigh==0)){
1180 //error bar found no clock...
1181 bitHigh=1;
1182 dest[bitnum]=77;
1183 bitnum++;
1184 errCnt++;
1185 }
1186 if (bitnum >=1000) break;
1187 }
1188 *size=bitnum;
1189 } else{
1190 *size=bitnum;
1191 *clk=bestStart;
1192 return -1;
1193 }
1194
1195 if (bitnum>16){
1196 *size=bitnum;
1197 } else return -1;
1198 return errCnt;
1199 }
1200
1201
1202 //by marshmellow
1203 //countFC is to detect the field clock and bit clock rates.
1204 //for fsk or ask not psk or nrz
1205 uint32_t countFC(uint8_t *BitStream, size_t size)
1206 {
1207 // get high/low thresholds
1208 int high, low;
1209 getHiLo(BitStream,10, &high, &low, 100, 100);
1210 // get zero crossing
1211 uint8_t zeroC = (high-low)/2+low;
1212 uint8_t clk[]={8,16,32,40,50,64,100,128};
1213 uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0};
1214 uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0};
1215 uint8_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0};
1216 // uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0};
1217 uint8_t fcLensFnd = 0;
1218 uint8_t rfLensFnd = 0;
1219 uint8_t lastBit=0;
1220 uint8_t curBit=0;
1221 uint8_t lastFCcnt=0;
1222 uint32_t errCnt=0;
1223 uint32_t fcCounter = 0;
1224 uint32_t rfCounter = 0;
1225 uint8_t firstBitFnd = 0;
1226 int i;
1227
1228 // prime i to first up transition
1229 for (i = 1; i < size; i++)
1230 if (BitStream[i]>=zeroC && BitStream[i-1]<zeroC)
1231 break;
1232
1233 for (; i < size; i++){
1234 curBit = BitStream[i];
1235 lastBit = BitStream[i-1];
1236 if (lastBit<zeroC && curBit >= zeroC){
1237 // new up transition
1238 fcCounter++;
1239 rfCounter++;
1240 if (fcCounter > 3 && fcCounter < 256){
1241 //we've counted enough that it could be a valid field clock
1242
1243 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1244 if (lastFCcnt==5 && fcCounter==9) fcCounter--;
1245 //if odd and not rc/5 add one (for when we get a fc 9 instead of 10)
1246 if ((fcCounter==9 && fcCounter & 1) || fcCounter==4) fcCounter++;
1247
1248 //look for bit clock (rf/xx)
1249 if ((fcCounter<lastFCcnt || fcCounter>lastFCcnt)){
1250 //not the same size as the last wave - start of new bit sequence
1251
1252 if (firstBitFnd>1){ //skip first wave change - probably not a complete bit
1253 for (int ii=0; ii<10; ii++){
1254 if (rfLens[ii]==rfCounter){
1255 //rfCnts[ii]++;
1256 rfCounter=0;
1257 break;
1258 }
1259 }
1260 if (rfCounter>0 && rfLensFnd<10){
1261 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1262 //rfCnts[rfLensFnd]++;
1263 rfLens[rfLensFnd++]=rfCounter;
1264 }
1265 } else {
1266 //PrintAndLog("DEBUG i: %d",i);
1267 firstBitFnd++;
1268 }
1269 rfCounter=0;
1270 lastFCcnt=fcCounter;
1271 }
1272
1273 // save last field clock count (fc/xx)
1274 // find which fcLens to save it to:
1275 for (int ii=0; ii<10; ii++){
1276 if (fcLens[ii]==fcCounter){
1277 fcCnts[ii]++;
1278 fcCounter=0;
1279 break;
1280 }
1281 }
1282 if (fcCounter>0 && fcLensFnd<10){
1283 //add new fc length
1284 //PrintAndLog("FCCntr %d",fcCounter);
1285 fcCnts[fcLensFnd]++;
1286 fcLens[fcLensFnd++]=fcCounter;
1287 }
1288 } else{
1289 // hmmm this should not happen often - count them
1290 errCnt++;
1291 }
1292 // reset counter
1293 fcCounter=0;
1294 } else {
1295 // count sample
1296 fcCounter++;
1297 rfCounter++;
1298 }
1299 }
1300 // if too many errors return errors as negative number (IS THIS NEEDED?)
1301 if (errCnt>100) return -1*errCnt;
1302
1303 uint8_t maxCnt1=0, best1=9, best2=9, best3=9, rfHighest=10, rfHighest2=10, rfHighest3=10;
1304
1305 // go through fclens and find which ones are bigest 2
1306 for (i=0; i<10; i++){
1307 // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d, RF %d",fcLens[i],fcCnts[i],errCnt,rfLens[i]);
1308
1309 // get the 3 best FC values
1310 if (fcCnts[i]>maxCnt1) {
1311 best3=best2;
1312 best2=best1;
1313 maxCnt1=fcCnts[i];
1314 best1=i;
1315 } else if(fcCnts[i]>fcCnts[best2]){
1316 best3=best2;
1317 best2=i;
1318 } else if(fcCnts[i]>fcCnts[best3]){
1319 best3=i;
1320 }
1321 //get highest 2 RF values (might need to get more values to compare or compare all?)
1322 if (rfLens[i]>rfLens[rfHighest]){
1323 rfHighest3=rfHighest2;
1324 rfHighest2=rfHighest;
1325 rfHighest=i;
1326 } else if(rfLens[i]>rfLens[rfHighest2]){
1327 rfHighest3=rfHighest2;
1328 rfHighest2=i;
1329 } else if(rfLens[i]>rfLens[rfHighest3]){
1330 rfHighest3=i;
1331 }
1332 }
1333
1334 // set allowed clock remainder tolerance to be 1 large field clock length
1335 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1336 int tol1 = (fcLens[best1]>fcLens[best2]) ? fcLens[best1] : fcLens[best2];
1337
1338 // loop to find the highest clock that has a remainder less than the tolerance
1339 // compare samples counted divided by
1340 int ii=7;
1341 for (; ii>=0; ii--){
1342 if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){
1343 if (rfLens[rfHighest2] % clk[ii] < tol1 || rfLens[rfHighest2] % clk[ii] > clk[ii]-tol1){
1344 if (rfLens[rfHighest3] % clk[ii] < tol1 || rfLens[rfHighest3] % clk[ii] > clk[ii]-tol1){
1345 break;
1346 }
1347 }
1348 }
1349 }
1350
1351 if (ii<0) ii=7; // oops we went too far
1352
1353 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1354
1355 uint32_t fcs=0;
1356 // PrintAndLog("DEBUG: Best %d best2 %d best3 %d, clk %d, clk2 %d",fcLens[best1],fcLens[best2],fcLens[best3],clk[i],clk[ii]);
1357 //
1358
1359 if (fcLens[best1]>fcLens[best2]){
1360 fcs = (((uint32_t)clk[ii])<<16) | (((uint32_t)fcLens[best1])<<8) | ((fcLens[best2]));
1361 } else {
1362 fcs = (((uint32_t)clk[ii])<<16) | (((uint32_t)fcLens[best2])<<8) | ((fcLens[best1]));
1363 }
1364
1365 return fcs;
1366 }
Impressum, Datenschutz