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