]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
47e63ef6ec64515b7ff190bbc9cce0a79c99f074
1 //-----------------------------------------------------------------------------
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
7 //-----------------------------------------------------------------------------
8 // Low frequency demod/decode commands
9 //-----------------------------------------------------------------------------
16 uint8_t justNoise(uint8_t *BitStream
, size_t size
)
18 static const uint8_t THRESHOLD
= 123;
19 //test samples are not just noise
20 uint8_t justNoise1
= 1;
21 for(size_t idx
=0; idx
< size
&& justNoise1
;idx
++){
22 justNoise1
= BitStream
[idx
] < THRESHOLD
;
28 //get high and low with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise
29 int getHiLo(uint8_t *BitStream
, size_t size
, int *high
, int *low
, uint8_t fuzzHi
, uint8_t fuzzLo
)
33 // get high and low thresholds
34 for (int i
=0; i
< size
; i
++){
35 if (BitStream
[i
] > *high
) *high
= BitStream
[i
];
36 if (BitStream
[i
] < *low
) *low
= BitStream
[i
];
38 if (*high
< 123) return -1; // just noise
39 *high
= (int)(((*high
-128)*(((float)fuzzHi
)/100))+128);
40 *low
= (int)(((*low
-128)*(((float)fuzzLo
)/100))+128);
45 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
46 // returns 1 if passed
47 uint8_t parityTest(uint32_t bits
, uint8_t bitLen
, uint8_t pType
)
50 for (uint8_t i
= 0; i
< bitLen
; i
++){
51 ans
^= ((bits
>> i
) & 1);
53 //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType);
54 return (ans
== pType
);
58 //search for given preamble in given BitStream and return startIndex and length
59 uint8_t preambleSearch(uint8_t *BitStream
, uint8_t *preamble
, size_t pLen
, size_t *size
, size_t *startIdx
)
62 for (int idx
=0; idx
< *size
- pLen
; idx
++){
63 if (memcmp(BitStream
+idx
, preamble
, pLen
) == 0){
70 *size
= idx
- *startIdx
;
80 //takes 1s and 0s and searches for EM410x format - output EM ID
81 uint64_t Em410xDecode(uint8_t *BitStream
, size_t *size
, size_t *startIdx
)
83 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
84 // otherwise could be a void with no arguments
88 if (BitStream
[1]>1){ //allow only 1s and 0s
89 // PrintAndLog("no data found");
92 // 111111111 bit pattern represent start of frame
93 uint8_t preamble
[] = {1,1,1,1,1,1,1,1,1};
95 uint32_t parityBits
= 0;
98 for (uint8_t extraBitChk
=0; extraBitChk
<5; extraBitChk
++){
99 errChk
= preambleSearch(BitStream
+extraBitChk
+*startIdx
, preamble
, sizeof(preamble
), size
, startIdx
);
100 if (errChk
== 0) return 0;
102 for (i
=0; i
<10;i
++){ //loop through 10 sets of 5 bits (50-10p = 40 bits)
103 parityBits
= bytebits_to_byte(BitStream
+(i
*5)+idx
,5);
105 if (parityTest(parityBits
, 5, 0) == 0){
106 //parity failed try next bit (in the case of 1111111111) but last 9 = preamble
111 for (uint8_t ii
=0; ii
<4; ii
++){
112 lo
= (lo
<< 1LL) | (BitStream
[(i
*5)+ii
+idx
]);
115 if (errChk
!= 0) return lo
;
116 //skip last 5 bit parity test for simplicity.
123 //takes 3 arguments - clock, invert, maxErr as integers
124 //attempts to demodulate ask while decoding manchester
125 //prints binary found and saves in graphbuffer for further commands
126 int askmandemod(uint8_t *BinStream
, size_t *size
, int *clk
, int *invert
, int maxErr
)
130 int start
= DetectASKClock(BinStream
, *size
, clk
, 20); //clock default
131 if (*clk
==0) return -3;
132 if (start
< 0) return -3;
133 // if autodetected too low then adjust //MAY NEED ADJUSTMENT
134 //if (clk2==0 && *clk<8) *clk =64;
135 //if (clk2==0 && *clk<32) *clk=32;
136 if (*invert
!= 0 && *invert
!= 1) *invert
=0;
137 uint32_t initLoopMax
= 200;
138 if (initLoopMax
> *size
) initLoopMax
=*size
;
139 // Detect high and lows
140 // 25% fuzz in case highs and lows aren't clipped [marshmellow]
142 ans
= getHiLo(BinStream
, initLoopMax
, &high
, &low
, 75, 75);
143 if (ans
<1) return -2; //just noise
145 // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
146 int lastBit
= 0; //set first clock check
147 uint32_t bitnum
= 0; //output counter
148 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
149 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
151 uint32_t gLen
= *size
;
152 if (gLen
> 3000) gLen
=3000;
154 uint16_t MaxBits
= 500;
155 uint32_t bestStart
= *size
;
156 int bestErrCnt
= maxErr
+1;
157 // PrintAndLog("DEBUG - lastbit - %d",lastBit);
158 // loop to find first wave that works
159 for (iii
=0; iii
< gLen
; ++iii
){
160 if ((BinStream
[iii
] >= high
) || (BinStream
[iii
] <= low
)){
163 // loop through to see if this start location works
164 for (i
= iii
; i
< *size
; ++i
) {
165 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
167 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
168 //low found and we are expecting a bar
171 //mid value found or no bar supposed to be here
172 if ((i
-lastBit
)>(*clk
+tol
)){
173 //should have hit a high or low based on clock!!
176 //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);
179 lastBit
+=*clk
;//skip over until hit too many errors
180 if (errCnt
>(maxErr
)) break; //allow 1 error for every 1000 samples else start over
183 if ((i
-iii
) >(MaxBits
* *clk
)) break; //got plenty of bits
185 //we got more than 64 good bits and not all errors
186 if ((((i
-iii
)/ *clk
) > (64)) && (errCnt
<=maxErr
)) {
191 break; //great read - finish
193 if (errCnt
<bestErrCnt
){ //set this as new best run
200 if (bestErrCnt
<=maxErr
){
201 //best run is good enough set to best run and set overwrite BinStream
203 lastBit
= bestStart
- *clk
;
205 for (i
= iii
; i
< *size
; ++i
) {
206 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
208 BinStream
[bitnum
] = *invert
;
210 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
211 //low found and we are expecting a bar
213 BinStream
[bitnum
] = 1-*invert
;
216 //mid value found or no bar supposed to be here
217 if ((i
-lastBit
)>(*clk
+tol
)){
218 //should have hit a high or low based on clock!!
221 //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);
223 BinStream
[bitnum
]=77;
227 lastBit
+=*clk
;//skip over error
230 if (bitnum
>=MaxBits
) break;
242 //encode binary data into binary manchester
243 int ManchesterEncode(uint8_t *BitStream
, size_t size
)
245 size_t modIdx
=20000, i
=0;
246 if (size
>modIdx
) return -1;
247 for (size_t idx
=0; idx
< size
; idx
++){
248 BitStream
[idx
+modIdx
++] = BitStream
[idx
];
249 BitStream
[idx
+modIdx
++] = BitStream
[idx
]^1;
251 for (; i
<(size
*2); i
++){
252 BitStream
[i
] = BitStream
[i
+20000];
258 //take 10 and 01 and manchester decode
259 //run through 2 times and take least errCnt
260 int manrawdecode(uint8_t * BitStream
, size_t *size
)
263 uint16_t MaxBits
= 500;
266 uint16_t bestErr
= 1000;
267 uint16_t bestRun
= 0;
269 if (size
== 0) return -1;
270 for (ii
=1;ii
<3;++ii
){
272 for (i
=i
+ii
;i
<*size
-2;i
+=2){
273 if(BitStream
[i
]==1 && (BitStream
[i
+1]==0)){
274 } else if((BitStream
[i
]==0)&& BitStream
[i
+1]==1){
278 if(bitnum
>MaxBits
) break;
290 for (i
=i
+ii
; i
< *size
-2; i
+=2){
291 if(BitStream
[i
] == 1 && (BitStream
[i
+1] == 0)){
292 BitStream
[bitnum
++]=0;
293 } else if((BitStream
[i
] == 0) && BitStream
[i
+1] == 1){
294 BitStream
[bitnum
++]=1;
296 BitStream
[bitnum
++]=77;
299 if(bitnum
>MaxBits
) break;
307 //take 01 or 10 = 0 and 11 or 00 = 1
308 int BiphaseRawDecode(uint8_t *BitStream
, size_t *size
, int offset
, int invert
)
313 uint16_t MaxBits
=500;
315 if (size
== 0) return -1;
316 for (;i
<*size
-2; i
+=2){
317 if((BitStream
[i
]==1 && BitStream
[i
+1]==0) || (BitStream
[i
]==0 && BitStream
[i
+1]==1)){
318 BitStream
[bitnum
++]=1^invert
;
319 } else if((BitStream
[i
]==0 && BitStream
[i
+1]==0) || (BitStream
[i
]==1 && BitStream
[i
+1]==1)){
320 BitStream
[bitnum
++]=invert
;
322 BitStream
[bitnum
++]=77;
325 if(bitnum
>MaxBits
) break;
332 void askAmp(uint8_t *BitStream
, size_t size
)
336 for(int i
= 1; i
<size
; i
++){
337 if (BitStream
[i
]-BitStream
[i
-1]>=30) //large jump up
339 else if(BitStream
[i
]-BitStream
[i
-1]<=-20) //large jump down
342 shiftedVal
=BitStream
[i
]+shift
;
346 else if (shiftedVal
<0)
348 BitStream
[i
-1] = shiftedVal
;
354 //takes 3 arguments - clock, invert and maxErr as integers
355 //attempts to demodulate ask only
356 //prints binary found and saves in graphbuffer for further commands
357 int askrawdemod(uint8_t *BinStream
, size_t *size
, int *clk
, int *invert
, int maxErr
, uint8_t amp
)
360 if (*size
==0) return -1;
361 int start
= DetectASKClock(BinStream
, *size
, clk
, 20); //clock default
362 if (*clk
==0) return -1;
363 if (start
<0) return -1;
364 if (*invert
!= 0 && *invert
!= 1) *invert
=0;
365 uint32_t initLoopMax
= 200;
366 if (initLoopMax
> *size
) initLoopMax
=*size
;
367 // Detect high and lows
368 //25% fuzz in case highs and lows aren't clipped [marshmellow]
370 if (amp
==1) askAmp(BinStream
, *size
);
371 ans
= getHiLo(BinStream
, initLoopMax
, &high
, &low
, 75, 75);
372 if (ans
<1) return -1; //just noise
374 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
375 int lastBit
= 0; //set first clock check
376 uint32_t bitnum
= 0; //output counter
377 uint8_t tol
= 0; //clock tolerance adjust - waves will be accepted as within the clock
378 // if they fall + or - this value + clock from last valid wave
379 if (*clk
== 32) tol
=0; //clock tolerance may not be needed anymore currently set to
380 // + or - 1 but could be increased for poor waves or removed entirely
382 uint32_t gLen
= *size
;
383 if (gLen
> 500) gLen
=500;
385 uint32_t bestStart
= *size
;
386 uint32_t bestErrCnt
= maxErr
; //(*size/1000);
388 uint16_t MaxBits
=1000;
389 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
390 //loop to find first wave that works
391 for (iii
=start
; iii
< gLen
; ++iii
){
392 if ((BinStream
[iii
]>=high
) || (BinStream
[iii
]<=low
)){
395 //loop through to see if this start location works
396 for (i
= iii
; i
< *size
; ++i
) {
397 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
400 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
401 //low found and we are expecting a bar
404 } else if ((BinStream
[i
]<=low
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
407 } else if ((BinStream
[i
]>=high
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
410 } else if ((i
-lastBit
)>((*clk
/2)+tol
) && (midBit
==0)){
414 //mid value found or no bar supposed to be here
416 if ((i
-lastBit
)>(*clk
+tol
)){
417 //should have hit a high or low based on clock!!
419 //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 lastBit
+=*clk
;//skip over until hit too many errors
423 if (errCnt
> maxErr
){
429 if ((i
-iii
)>(MaxBits
* *clk
)) break; //got enough bits
431 //we got more than 64 good bits and not all errors
432 if ((((i
-iii
)/ *clk
) > (64)) && (errCnt
<=maxErr
)) {
437 break; //great read - finish
439 if (errCnt
<bestErrCnt
){ //set this as new best run
446 if (bestErrCnt
<=maxErr
){
447 //best run is good enough - set to best run and overwrite BinStream
449 lastBit
= bestStart
- *clk
;
451 for (i
= iii
; i
< *size
; ++i
) {
452 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
454 BinStream
[bitnum
] = *invert
;
457 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
458 //low found and we are expecting a bar
460 BinStream
[bitnum
] = 1 - *invert
;
463 } else if ((BinStream
[i
]<=low
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
466 BinStream
[bitnum
] = 1 - *invert
;
468 } else if ((BinStream
[i
]>=high
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
471 BinStream
[bitnum
] = *invert
;
473 } else if ((i
-lastBit
)>((*clk
/2)+tol
) && (midBit
==0)){
476 if (bitnum
!=0) BinStream
[bitnum
] = BinStream
[bitnum
-1];
480 //mid value found or no bar supposed to be here
481 if ((i
-lastBit
)>(*clk
+tol
)){
482 //should have hit a high or low based on clock!!
485 //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);
487 BinStream
[bitnum
]=77;
490 lastBit
+=*clk
;//skip over error
493 if (bitnum
>= MaxBits
) break;
503 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
504 size_t fsk_wave_demod(uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
506 uint32_t last_transition
= 0;
509 if (fchigh
==0) fchigh
=10;
510 if (fclow
==0) fclow
=8;
511 //set the threshold close to 0 (graph) or 128 std to avoid static
512 uint8_t threshold_value
= 123;
514 // sync to first lo-hi transition, and threshold
516 // Need to threshold first sample
518 if(dest
[0] < threshold_value
) dest
[0] = 0;
522 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
523 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
524 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
525 for(idx
= 1; idx
< size
; idx
++) {
526 // threshold current value
528 if (dest
[idx
] < threshold_value
) dest
[idx
] = 0;
531 // Check for 0->1 transition
532 if (dest
[idx
-1] < dest
[idx
]) { // 0 -> 1 transition
533 if ((idx
-last_transition
)<(fclow
-2)){ //0-5 = garbage noise
534 //do nothing with extra garbage
535 } else if ((idx
-last_transition
) < (fchigh
-1)) { //6-8 = 8 waves
537 } else { //9+ = 10 waves
540 last_transition
= idx
;
544 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
547 uint32_t myround2(float f
)
549 if (f
>= 2000) return 2000;//something bad happened
550 return (uint32_t) (f
+ (float)0.5);
553 //translate 11111100000 to 10
554 size_t aggregate_bits(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t maxConsequtiveBits
,
555 uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
557 uint8_t lastval
=dest
[0];
562 for( idx
=1; idx
< size
; idx
++) {
564 if (dest
[idx
]==lastval
) {
568 //if lastval was 1, we have a 1->0 crossing
569 if ( dest
[idx
-1]==1 ) {
570 n
=myround2((float)(n
+1)/((float)(rfLen
)/(float)fclow
));
571 } else {// 0->1 crossing
572 n
=myround2((float)(n
+1)/((float)(rfLen
-1)/(float)fchigh
)); //-1 for fudge factor
576 if(n
< maxConsequtiveBits
) //Consecutive
578 if(invert
==0){ //invert bits
579 memset(dest
+numBits
, dest
[idx
-1] , n
);
581 memset(dest
+numBits
, dest
[idx
-1]^1 , n
);
590 //by marshmellow (from holiman's base)
591 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
592 int fskdemod(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
595 size
= fsk_wave_demod(dest
, size
, fchigh
, fclow
);
596 size
= aggregate_bits(dest
, size
, rfLen
, 192, invert
, fchigh
, fclow
);
600 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
601 int HIDdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
603 if (justNoise(dest
, *size
)) return -1;
605 size_t numStart
=0, size2
=*size
, startIdx
=0;
607 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
608 if (*size
< 96) return -2;
609 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
610 uint8_t preamble
[] = {0,0,0,1,1,1,0,1};
611 // find bitstring in array
612 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
613 if (errChk
== 0) return -3; //preamble not found
615 numStart
= startIdx
+ sizeof(preamble
);
616 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
617 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
618 if (dest
[idx
] == dest
[idx
+1]){
619 return -4; //not manchester data
621 *hi2
= (*hi2
<<1)|(*hi
>>31);
622 *hi
= (*hi
<<1)|(*lo
>>31);
623 //Then, shift in a 0 or one into low
624 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
629 return (int)startIdx
;
632 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
633 int ParadoxdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
635 if (justNoise(dest
, *size
)) return -1;
637 size_t numStart
=0, size2
=*size
, startIdx
=0;
639 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
640 if (*size
< 96) return -2;
642 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
643 uint8_t preamble
[] = {0,0,0,0,1,1,1,1};
645 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
646 if (errChk
== 0) return -3; //preamble not found
648 numStart
= startIdx
+ sizeof(preamble
);
649 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
650 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
651 if (dest
[idx
] == dest
[idx
+1])
652 return -4; //not manchester data
653 *hi2
= (*hi2
<<1)|(*hi
>>31);
654 *hi
= (*hi
<<1)|(*lo
>>31);
655 //Then, shift in a 0 or one into low
656 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
661 return (int)startIdx
;
664 uint32_t bytebits_to_byte(uint8_t* src
, size_t numbits
)
667 for(int i
= 0 ; i
< numbits
; i
++)
669 num
= (num
<< 1) | (*src
);
675 int IOdemodFSK(uint8_t *dest
, size_t size
)
677 if (justNoise(dest
, size
)) return -1;
678 //make sure buffer has data
679 if (size
< 66*64) return -2;
681 size
= fskdemod(dest
, size
, 64, 1, 10, 8); // FSK2a RF/64
682 if (size
< 65) return -3; //did we get a good demod?
684 //0 10 20 30 40 50 60
686 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
687 //-----------------------------------------------------------------------------
688 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
690 //XSF(version)facility:codeone+codetwo
693 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,1};
694 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), &size
, &startIdx
);
695 if (errChk
== 0) return -4; //preamble not found
697 if (!dest
[startIdx
+8] && dest
[startIdx
+17]==1 && dest
[startIdx
+26]==1 && dest
[startIdx
+35]==1 && dest
[startIdx
+44]==1 && dest
[startIdx
+53]==1){
698 //confirmed proper separator bits found
699 //return start position
700 return (int) startIdx
;
706 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
707 // Parity Type (1 for odd 0 for even), and binary Length (length to run)
708 size_t removeParity(uint8_t *BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
)
710 uint32_t parityWd
= 0;
711 size_t j
= 0, bitCnt
= 0;
712 for (int word
= 0; word
< (bLen
); word
+=pLen
){
713 for (int bit
=0; bit
< pLen
; bit
++){
714 parityWd
= (parityWd
<< 1) | BitStream
[startIdx
+word
+bit
];
715 BitStream
[j
++] = (BitStream
[startIdx
+word
+bit
]);
718 // if parity fails then return 0
719 if (parityTest(parityWd
, pLen
, pType
) == 0) return -1;
723 // if we got here then all the parities passed
724 //return ID start index and size
729 // FSK Demod then try to locate an AWID ID
730 int AWIDdemodFSK(uint8_t *dest
, size_t *size
)
732 //make sure buffer has enough data
733 if (*size
< 96*50) return -1;
735 if (justNoise(dest
, *size
)) return -2;
738 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
739 if (*size
< 96) return -3; //did we get a good demod?
741 uint8_t preamble
[] = {0,0,0,0,0,0,0,1};
743 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
744 if (errChk
== 0) return -4; //preamble not found
745 if (*size
!= 96) return -5;
746 return (int)startIdx
;
750 // FSK Demod then try to locate an Farpointe Data (pyramid) ID
751 int PyramiddemodFSK(uint8_t *dest
, size_t *size
)
753 //make sure buffer has data
754 if (*size
< 128*50) return -5;
756 //test samples are not just noise
757 if (justNoise(dest
, *size
)) return -1;
760 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
761 if (*size
< 128) return -2; //did we get a good demod?
763 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
765 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
766 if (errChk
== 0) return -4; //preamble not found
767 if (*size
!= 128) return -3;
768 return (int)startIdx
;
772 uint8_t DetectCleanAskWave(uint8_t dest
[], size_t size
, int high
, int low
)
776 for (size_t i
=20; i
<255; i
++){
777 if (dest
[i
]>low
&& dest
[i
]<high
)
783 if (cntPeaks
>190) return 1;
789 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
790 // maybe somehow adjust peak trimming value based on samples to fix?
791 // return start index of best starting position for that clock and return clock (by reference)
792 int DetectASKClock(uint8_t dest
[], size_t size
, int *clock
, int maxErr
)
795 int clk
[]={8,16,32,40,50,64,100,128,256};
796 int loopCnt
= 256; //don't need to loop through entire array...
797 if (size
== 0) return -1;
798 if (size
<loopCnt
) loopCnt
= size
;
799 //if we already have a valid clock quit
802 if (clk
[i
] == *clock
) return 0;
804 //get high and low peak
806 getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75);
808 //test for large clean peaks
809 if (DetectCleanAskWave(dest
, size
, peak
, low
)==1){
812 fcTest
=countFC(dest
, size
, &mostFC
);
813 uint8_t fc1
= fcTest
>> 8;
814 uint8_t fc2
= fcTest
& 0xFF;
831 int bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
832 int bestStart
[]={0,0,0,0,0,0,0,0,0};
834 //test each valid clock from smallest to greatest to see which lines up
835 for(clkCnt
=0; clkCnt
< 8; clkCnt
++){
836 if (clk
[clkCnt
] == 32){
841 bestErr
[clkCnt
]=1000;
842 //try lining up the peaks by moving starting point (try first 256)
843 for (ii
=0; ii
< loopCnt
; ii
++){
844 if ((dest
[ii
] >= peak
) || (dest
[ii
] <= low
)){
846 // now that we have the first one lined up test rest of wave array
847 for (i
=0; i
<((int)((size
-ii
-tol
)/clk
[clkCnt
])-1); ++i
){
848 if (dest
[ii
+(i
*clk
[clkCnt
])]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])]<=low
){
849 }else if(dest
[ii
+(i
*clk
[clkCnt
])-tol
]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])-tol
]<=low
){
850 }else if(dest
[ii
+(i
*clk
[clkCnt
])+tol
]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])+tol
]<=low
){
851 }else{ //error no peak detected
855 //if we found no errors then we can stop here
856 // this is correct one - return this clock
857 //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
858 if(errCnt
==0 && clkCnt
<6) {
859 *clock
= clk
[clkCnt
];
862 //if we found errors see if it is lowest so far and save it as best run
863 if(errCnt
<bestErr
[clkCnt
]){
864 bestErr
[clkCnt
]=errCnt
;
865 bestStart
[clkCnt
]=ii
;
872 for (iii
=0; iii
<8; ++iii
){
873 if (bestErr
[iii
]<bestErr
[best
]){
874 if (bestErr
[iii
]==0) bestErr
[iii
]=1;
875 // current best bit to error ratio vs new bit to error ratio
876 if (((size
/clk
[best
])/bestErr
[best
] < (size
/clk
[iii
])/bestErr
[iii
]) ){
881 if (bestErr
[best
]>maxErr
) return -1;
883 return bestStart
[best
];
887 //detect psk clock by reading each phase shift
888 // a phase shift is determined by measuring the sample length of each wave
889 int DetectPSKClock(uint8_t dest
[], size_t size
, int clock
)
891 uint8_t clk
[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
892 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
893 if (size
== 0) return 0;
894 if (size
<loopCnt
) loopCnt
= size
;
896 //if we already have a valid clock quit
899 if (clk
[i
] == clock
) return clock
;
901 size_t waveStart
=0, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
902 uint8_t clkCnt
, fc
=0, fullWaveLen
=0, tol
=1;
903 uint16_t peakcnt
=0, errCnt
=0, waveLenCnt
=0;
904 uint16_t bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
905 uint16_t peaksdet
[]={0,0,0,0,0,0,0,0,0};
906 countFC(dest
, size
, &fc
);
907 //PrintAndLog("DEBUG: FC: %d",fc);
909 //find first full wave
910 for (i
=0; i
<loopCnt
; i
++){
911 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
912 if (waveStart
== 0) {
914 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
917 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
918 waveLenCnt
= waveEnd
-waveStart
;
919 if (waveLenCnt
> fc
){
920 firstFullWave
= waveStart
;
921 fullWaveLen
=waveLenCnt
;
928 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
930 //test each valid clock from greatest to smallest to see which lines up
931 for(clkCnt
=7; clkCnt
>= 1 ; clkCnt
--){
932 lastClkBit
= firstFullWave
; //set end of wave as clock align
936 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
938 for (i
= firstFullWave
+fullWaveLen
-1; i
< loopCnt
-2; i
++){
939 //top edge of wave = start of new wave
940 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
941 if (waveStart
== 0) {
946 waveLenCnt
= waveEnd
-waveStart
;
947 if (waveLenCnt
> fc
){
948 //if this wave is a phase shift
949 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc);
950 if (i
+1 >= lastClkBit
+ clk
[clkCnt
] - tol
){ //should be a clock bit
952 lastClkBit
+=clk
[clkCnt
];
953 } else if (i
<lastClkBit
+8){
954 //noise after a phase shift - ignore
955 } else { //phase shift before supposed to based on clock
958 } else if (i
+1 > lastClkBit
+ clk
[clkCnt
] + tol
+ fc
){
959 lastClkBit
+=clk
[clkCnt
]; //no phase shift but clock bit
968 if (errCnt
<= bestErr
[clkCnt
]) bestErr
[clkCnt
]=errCnt
;
969 if (peakcnt
> peaksdet
[clkCnt
]) peaksdet
[clkCnt
]=peakcnt
;
971 //all tested with errors
972 //return the highest clk with the most peaks found
974 for (i
=7; i
>=1; i
--){
975 if (peaksdet
[i
] > peaksdet
[best
]) {
978 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
984 //detect nrz clock by reading #peaks vs no peaks(or errors)
985 int DetectNRZClock(uint8_t dest
[], size_t size
, int clock
)
988 int clk
[]={8,16,32,40,50,64,100,128,256};
989 int loopCnt
= 4096; //don't need to loop through entire array...
990 if (size
== 0) return 0;
991 if (size
<loopCnt
) loopCnt
= size
;
993 //if we already have a valid clock quit
995 if (clk
[i
] == clock
) return clock
;
997 //get high and low peak
999 getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75);
1001 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
1006 int peaksdet
[]={0,0,0,0,0,0,0,0};
1008 //test for large clipped waves
1009 for (i
=0; i
<loopCnt
; i
++){
1010 if (dest
[i
] >= peak
|| dest
[i
] <= low
){
1013 if (peakcnt
>0 && maxPeak
< peakcnt
){
1020 //test each valid clock from smallest to greatest to see which lines up
1021 for(clkCnt
=0; clkCnt
< 8; ++clkCnt
){
1022 //ignore clocks smaller than largest peak
1023 if (clk
[clkCnt
]<maxPeak
) continue;
1025 //try lining up the peaks by moving starting point (try first 256)
1026 for (ii
=0; ii
< loopCnt
; ++ii
){
1027 if ((dest
[ii
] >= peak
) || (dest
[ii
] <= low
)){
1029 // now that we have the first one lined up test rest of wave array
1030 for (i
=0; i
< ((int)((size
-ii
-tol
)/clk
[clkCnt
])-1); ++i
){
1031 if (dest
[ii
+(i
*clk
[clkCnt
])]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])]<=low
){
1035 if(peakcnt
>peaksdet
[clkCnt
]) {
1036 peaksdet
[clkCnt
]=peakcnt
;
1043 for (iii
=7; iii
> 0; iii
--){
1044 if (peaksdet
[iii
] > peaksdet
[best
]){
1047 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
1053 // convert psk1 demod to psk2 demod
1054 // only transition waves are 1s
1055 void psk1TOpsk2(uint8_t *BitStream
, size_t size
)
1058 uint8_t lastBit
=BitStream
[0];
1059 for (; i
<size
; i
++){
1060 if (lastBit
!=BitStream
[i
]){
1061 lastBit
=BitStream
[i
];
1070 // redesigned by marshmellow adjusted from existing decode functions
1071 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1072 int indala26decode(uint8_t *bitStream
, size_t *size
, uint8_t *invert
)
1074 //26 bit 40134 format (don't know other formats)
1076 int long_wait
=29;//29 leading zeros in format
1082 // Finding the start of a UID
1083 for (start
= 0; start
<= *size
- 250; start
++) {
1084 first
= bitStream
[start
];
1085 for (i
= start
; i
< start
+ long_wait
; i
++) {
1086 if (bitStream
[i
] != first
) {
1090 if (i
== (start
+ long_wait
)) {
1094 if (start
== *size
- 250 + 1) {
1095 // did not find start sequence
1098 // Inverting signal if needed
1100 for (i
= start
; i
< *size
; i
++) {
1101 bitStream
[i
] = !bitStream
[i
];
1107 //found start once now test length by finding next one
1108 for (ii
=start
+29; ii
<= *size
- 250; ii
++) {
1109 first2
= bitStream
[ii
];
1110 for (iii
= ii
; iii
< ii
+ long_wait
; iii
++) {
1111 if (bitStream
[iii
] != first2
) {
1115 if (iii
== (ii
+ long_wait
)) {
1119 if (ii
== *size
- 250 + 1){
1120 // did not find second start sequence
1127 for (ii
= 0; ii
< bitCnt
; ii
++) {
1128 bitStream
[ii
] = bitStream
[i
++];
1134 // by marshmellow - demodulate NRZ wave (both similar enough)
1135 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1136 // there probably is a much simpler way to do this....
1137 int nrzRawDemod(uint8_t *dest
, size_t *size
, int *clk
, int *invert
, int maxErr
)
1139 if (justNoise(dest
, *size
)) return -1;
1140 *clk
= DetectNRZClock(dest
, *size
, *clk
);
1141 if (*clk
==0) return -2;
1144 ans
= getHiLo(dest
, 1260, &high
, &low
, 75, 75); //25% fuzz on high 25% fuzz on low
1145 if (ans
<1) return -2; //just noise
1146 uint32_t gLen
= 256;
1147 if (gLen
>*size
) gLen
= *size
;
1148 int lastBit
= 0; //set first clock check
1149 uint32_t bitnum
= 0; //output counter
1150 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
1153 uint16_t MaxBits
= 1000;
1154 uint32_t bestErrCnt
= maxErr
+1;
1155 uint32_t bestPeakCnt
= 0;
1156 uint32_t bestPeakStart
=0;
1159 uint8_t errBitHigh
=0;
1161 uint8_t ignoreWindow
=4;
1162 uint8_t ignoreCnt
=ignoreWindow
; //in case of noice near peak
1163 //loop to find first wave that works - align to clock
1164 for (iii
=0; iii
< gLen
; ++iii
){
1165 if ((dest
[iii
]>=high
) || (dest
[iii
]<=low
)){
1170 //loop through to see if this start location works
1171 for (i
= iii
; i
< *size
; ++i
) {
1172 //if we found a high bar and we are at a clock bit
1173 if ((dest
[i
]>=high
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1179 ignoreCnt
=ignoreWindow
;
1180 //else if low bar found and we are at a clock point
1181 }else if ((dest
[i
]<=low
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1187 ignoreCnt
=ignoreWindow
;
1188 //else if no bars found
1189 }else if(dest
[i
] < high
&& dest
[i
] > low
) {
1199 //if we are past a clock point
1200 if (i
>= lastBit
+*clk
+tol
){ //clock val
1204 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1205 }else if ((dest
[i
]>=high
|| dest
[i
]<=low
) && (i
<lastBit
+*clk
-tol
|| i
>lastBit
+*clk
+tol
) && (bitHigh
==0)){
1206 //error bar found no clock...
1209 if (bitnum
>=MaxBits
) break;
1211 //we got more than 64 good bits and not all errors
1212 if (bitnum
> (64) && (errCnt
<= (maxErr
))) {
1213 //possible good read
1216 bestErrCnt
= errCnt
;
1217 bestPeakCnt
= peakCnt
;
1218 bestPeakStart
= iii
;
1219 break; //great read - finish
1221 if (errCnt
< bestErrCnt
){ //set this as new best run
1222 bestErrCnt
= errCnt
;
1225 if (peakCnt
> bestPeakCnt
){
1226 bestPeakCnt
=peakCnt
;
1232 //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart);
1233 if (bestErrCnt
<= maxErr
){
1234 //best run is good enough set to best run and set overwrite BinStream
1236 lastBit
=bestPeakStart
-*clk
;
1238 for (i
= iii
; i
< *size
; ++i
) {
1239 //if we found a high bar and we are at a clock bit
1240 if ((dest
[i
] >= high
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1244 dest
[bitnum
]=curBit
;
1247 ignoreCnt
=ignoreWindow
;
1248 //else if low bar found and we are at a clock point
1249 }else if ((dest
[i
]<=low
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1253 dest
[bitnum
]=curBit
;
1256 ignoreCnt
=ignoreWindow
;
1257 //else if no bars found
1258 }else if(dest
[i
]<high
&& dest
[i
]>low
) {
1261 //if peak is done was it an error peak?
1271 //if we are past a clock point
1272 if (i
>=lastBit
+*clk
+tol
){ //clock val
1274 dest
[bitnum
]=curBit
;
1277 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1278 }else if ((dest
[i
]>=high
|| dest
[i
]<=low
) && ((i
<lastBit
+*clk
-tol
) || (i
>lastBit
+*clk
+tol
)) && (bitHigh
==0)){
1279 //error bar found no clock...
1282 if (bitnum
>= MaxBits
) break;
1297 //detects the bit clock for FSK given the high and low Field Clocks
1298 uint8_t detectFSKClk(uint8_t *BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
)
1300 uint8_t clk
[] = {8,16,32,40,50,64,100,128,0};
1301 uint16_t rfLens
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1302 uint8_t rfCnts
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1303 uint8_t rfLensFnd
= 0;
1304 uint8_t lastFCcnt
=0;
1305 uint32_t fcCounter
= 0;
1306 uint16_t rfCounter
= 0;
1307 uint8_t firstBitFnd
= 0;
1309 if (size
== 0) return 0;
1311 uint8_t fcTol
= (uint8_t)(0.5+(float)(fcHigh
-fcLow
)/2);
1316 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1317 // prime i to first up transition
1318 for (i
= 1; i
< size
-1; i
++)
1319 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
]>=BitStream
[i
+1])
1322 for (; i
< size
-1; i
++){
1323 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
]>=BitStream
[i
+1]){
1327 // if we got less than the small fc + tolerance then set it to the small fc
1328 if (fcCounter
< fcLow
+fcTol
)
1330 else //set it to the large fc
1333 //look for bit clock (rf/xx)
1334 if ((fcCounter
<lastFCcnt
|| fcCounter
>lastFCcnt
)){
1335 //not the same size as the last wave - start of new bit sequence
1337 if (firstBitFnd
>1){ //skip first wave change - probably not a complete bit
1338 for (int ii
=0; ii
<15; ii
++){
1339 if (rfLens
[ii
]==rfCounter
){
1345 if (rfCounter
>0 && rfLensFnd
<15){
1346 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1347 rfCnts
[rfLensFnd
]++;
1348 rfLens
[rfLensFnd
++]=rfCounter
;
1354 lastFCcnt
=fcCounter
;
1363 uint8_t rfHighest
=15, rfHighest2
=15, rfHighest3
=15;
1365 for (i
=0; i
<15; i
++){
1366 //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1367 //get highest 2 RF values (might need to get more values to compare or compare all?)
1368 if (rfCnts
[i
]>rfCnts
[rfHighest
]){
1369 rfHighest3
=rfHighest2
;
1370 rfHighest2
=rfHighest
;
1372 } else if(rfCnts
[i
]>rfCnts
[rfHighest2
]){
1373 rfHighest3
=rfHighest2
;
1375 } else if(rfCnts
[i
]>rfCnts
[rfHighest3
]){
1379 // set allowed clock remainder tolerance to be 1 large field clock length+1
1380 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1381 uint8_t tol1
= fcHigh
+1;
1383 //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1385 // loop to find the highest clock that has a remainder less than the tolerance
1386 // compare samples counted divided by
1388 for (; ii
>=0; ii
--){
1389 if (rfLens
[rfHighest
] % clk
[ii
] < tol1
|| rfLens
[rfHighest
] % clk
[ii
] > clk
[ii
]-tol1
){
1390 if (rfLens
[rfHighest2
] % clk
[ii
] < tol1
|| rfLens
[rfHighest2
] % clk
[ii
] > clk
[ii
]-tol1
){
1391 if (rfLens
[rfHighest3
] % clk
[ii
] < tol1
|| rfLens
[rfHighest3
] % clk
[ii
] > clk
[ii
]-tol1
){
1398 if (ii
<0) return 0; // oops we went too far
1404 //countFC is to detect the field clock lengths.
1405 //counts and returns the 2 most common wave lengths
1406 //mainly used for FSK field clock detection
1407 uint16_t countFC(uint8_t *BitStream
, size_t size
, uint8_t *mostFC
)
1409 uint8_t fcLens
[] = {0,0,0,0,0,0,0,0,0,0};
1410 uint16_t fcCnts
[] = {0,0,0,0,0,0,0,0,0,0};
1411 uint8_t fcLensFnd
= 0;
1412 uint8_t lastFCcnt
=0;
1413 uint32_t fcCounter
= 0;
1415 if (size
== 0) return 0;
1417 // prime i to first up transition
1418 for (i
= 1; i
< size
-1; i
++)
1419 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1])
1422 for (; i
< size
-1; i
++){
1423 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1]){
1424 // new up transition
1427 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1428 if (lastFCcnt
==5 && fcCounter
==9) fcCounter
--;
1429 //if odd and not rc/5 add one (for when we get a fc 9 instead of 10)
1430 if ((fcCounter
==9 && fcCounter
& 1) || fcCounter
==4) fcCounter
++;
1432 // save last field clock count (fc/xx)
1433 // find which fcLens to save it to:
1434 for (int ii
=0; ii
<10; ii
++){
1435 if (fcLens
[ii
]==fcCounter
){
1441 if (fcCounter
>0 && fcLensFnd
<10){
1443 fcCnts
[fcLensFnd
]++;
1444 fcLens
[fcLensFnd
++]=fcCounter
;
1453 uint8_t best1
=9, best2
=9, best3
=9;
1455 // go through fclens and find which ones are bigest 2
1456 for (i
=0; i
<10; i
++){
1457 // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt);
1458 // get the 3 best FC values
1459 if (fcCnts
[i
]>maxCnt1
) {
1464 } else if(fcCnts
[i
]>fcCnts
[best2
]){
1467 } else if(fcCnts
[i
]>fcCnts
[best3
]){
1471 uint8_t fcH
=0, fcL
=0;
1472 if (fcLens
[best1
]>fcLens
[best2
]){
1480 *mostFC
=fcLens
[best1
];
1481 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1483 uint16_t fcs
= (((uint16_t)fcH
)<<8) | fcL
;
1484 // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
1490 //countPSK_FC is to detect the psk carrier clock length.
1491 //counts and returns the 1 most common wave length
1492 uint8_t countPSK_FC(uint8_t *BitStream
, size_t size
)
1494 uint8_t fcLens
[] = {0,0,0,0,0,0,0,0,0,0};
1495 uint16_t fcCnts
[] = {0,0,0,0,0,0,0,0,0,0};
1496 uint8_t fcLensFnd
= 0;
1497 uint32_t fcCounter
= 0;
1499 if (size
== 0) return 0;
1501 // prime i to first up transition
1502 for (i
= 1; i
< size
-1; i
++)
1503 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1])
1506 for (; i
< size
-1; i
++){
1507 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1]){
1508 // new up transition
1511 // save last field clock count (fc/xx)
1512 // find which fcLens to save it to:
1513 for (int ii
=0; ii
<10; ii
++){
1514 if (fcLens
[ii
]==fcCounter
){
1520 if (fcCounter
>0 && fcLensFnd
<10){
1522 fcCnts
[fcLensFnd
]++;
1523 fcLens
[fcLensFnd
++]=fcCounter
;
1534 // go through fclens and find which ones are bigest
1535 for (i
=0; i
<10; i
++){
1536 //PrintAndLog("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]);
1537 // get the best FC value
1538 if (fcCnts
[i
]>maxCnt1
) {
1543 return fcLens
[best1
];
1546 //by marshmellow - demodulate PSK1 wave
1547 //uses wave lengths (# Samples)
1548 int pskRawDemod(uint8_t dest
[], size_t *size
, int *clock
, int *invert
)
1550 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
1551 if (size
== 0) return -1;
1552 if (*size
<loopCnt
) loopCnt
= *size
;
1554 uint8_t curPhase
= *invert
;
1555 size_t i
, waveStart
=0, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
1556 uint8_t fc
=0, fullWaveLen
=0, tol
=1;
1557 uint16_t errCnt
=0, waveLenCnt
=0;
1558 fc
= countPSK_FC(dest
, *size
);
1559 if (fc
!=2 && fc
!=4 && fc
!=8) return -1;
1560 //PrintAndLog("DEBUG: FC: %d",fc);
1561 *clock
= DetectPSKClock(dest
, *size
, *clock
);
1562 if (*clock
==0) return -1;
1563 int avgWaveVal
=0, lastAvgWaveVal
=0;
1564 //find first full wave
1565 for (i
=0; i
<loopCnt
; i
++){
1566 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1567 if (waveStart
== 0) {
1569 avgWaveVal
=dest
[i
+1];
1570 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
1573 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1574 waveLenCnt
= waveEnd
-waveStart
;
1575 lastAvgWaveVal
= avgWaveVal
/waveLenCnt
;
1576 if (waveLenCnt
> fc
){
1577 firstFullWave
= waveStart
;
1578 fullWaveLen
=waveLenCnt
;
1579 //if average wave value is > graph 0 then it is an up wave or a 1
1580 if (lastAvgWaveVal
> 128) curPhase
^=1;
1587 avgWaveVal
+=dest
[i
+1];
1589 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1590 lastClkBit
= firstFullWave
; //set start of wave as clock align
1594 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1596 for (i
= firstFullWave
+fullWaveLen
-1; i
< *size
-3; i
++){
1597 //top edge of wave = start of new wave
1598 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1599 if (waveStart
== 0) {
1602 avgWaveVal
= dest
[i
+1];
1605 waveLenCnt
= waveEnd
-waveStart
;
1606 lastAvgWaveVal
= avgWaveVal
/waveLenCnt
;
1607 if (waveLenCnt
> fc
){
1608 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1609 //if this wave is a phase shift
1610 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1611 if (i
+1 >= lastClkBit
+ *clock
- tol
){ //should be a clock bit
1613 dest
[numBits
] = curPhase
;
1615 lastClkBit
+= *clock
;
1616 } else if (i
<lastClkBit
+10){
1617 //noise after a phase shift - ignore
1618 } else { //phase shift before supposed to based on clock
1623 } else if (i
+1 > lastClkBit
+ *clock
+ tol
+ fc
){
1624 lastClkBit
+= *clock
; //no phase shift but clock bit
1625 dest
[numBits
] = curPhase
;
1632 avgWaveVal
+=dest
[i
+1];