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 //un_comment to allow debug print calls when used not on device
17 void dummy(char *fmt
, ...){}
21 #include "cmdparser.h"
23 #define prnt PrintAndLog
25 uint8_t g_debugMode
=0;
29 uint8_t justNoise(uint8_t *BitStream
, size_t size
)
31 static const uint8_t THRESHOLD
= 123;
32 //test samples are not just noise
33 uint8_t justNoise1
= 1;
34 for(size_t idx
=0; idx
< size
&& justNoise1
;idx
++){
35 justNoise1
= BitStream
[idx
] < THRESHOLD
;
41 //get high and low values of a wave with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise
42 int getHiLo(uint8_t *BitStream
, size_t size
, int *high
, int *low
, uint8_t fuzzHi
, uint8_t fuzzLo
)
46 // get high and low thresholds
47 for (size_t i
=0; i
< size
; i
++){
48 if (BitStream
[i
] > *high
) *high
= BitStream
[i
];
49 if (BitStream
[i
] < *low
) *low
= BitStream
[i
];
51 if (*high
< 123) return -1; // just noise
52 *high
= ((*high
-128)*fuzzHi
+ 12800)/100;
53 *low
= ((*low
-128)*fuzzLo
+ 12800)/100;
58 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
59 // returns 1 if passed
60 uint8_t parityTest(uint32_t bits
, uint8_t bitLen
, uint8_t pType
)
63 for (uint8_t i
= 0; i
< bitLen
; i
++){
64 ans
^= ((bits
>> i
) & 1);
66 //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType);
67 return (ans
== pType
);
71 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
72 // Parity Type (1 for odd; 0 for even; 2 Always 1's), and binary Length (length to run)
73 size_t removeParity(uint8_t *BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
)
75 uint32_t parityWd
= 0;
76 size_t j
= 0, bitCnt
= 0;
77 for (int word
= 0; word
< (bLen
); word
+=pLen
){
78 for (int bit
=0; bit
< pLen
; bit
++){
79 parityWd
= (parityWd
<< 1) | BitStream
[startIdx
+word
+bit
];
80 BitStream
[j
++] = (BitStream
[startIdx
+word
+bit
]);
82 j
--; // overwrite parity with next data
83 // if parity fails then return 0
84 if (pType
== 2) { // then marker bit which should be a 1
85 if (!BitStream
[j
]) return 0;
87 if (parityTest(parityWd
, pLen
, pType
) == 0) return 0;
92 // if we got here then all the parities passed
93 //return ID start index and size
98 // takes a array of binary values, length of bits per parity (includes parity bit),
99 // Parity Type (1 for odd; 0 for even; 2 Always 1's), and binary Length (length to run)
100 size_t addParity(uint8_t *BitSource
, uint8_t *dest
, uint8_t sourceLen
, uint8_t pLen
, uint8_t pType
)
102 uint32_t parityWd
= 0;
103 size_t j
= 0, bitCnt
= 0;
104 for (int word
= 0; word
< sourceLen
; word
+=pLen
-1) {
105 for (int bit
=0; bit
< pLen
-1; bit
++){
106 parityWd
= (parityWd
<< 1) | BitSource
[word
+bit
];
107 dest
[j
++] = (BitSource
[word
+bit
]);
109 // if parity fails then return 0
110 if (pType
== 2) { // then marker bit which should be a 1
113 dest
[j
++] = parityTest(parityWd
, pLen
-1, pType
) ^ 1;
118 // if we got here then all the parities passed
119 //return ID start index and size
123 uint32_t bytebits_to_byte(uint8_t *src
, size_t numbits
)
126 for(int i
= 0 ; i
< numbits
; i
++)
128 num
= (num
<< 1) | (*src
);
134 //least significant bit first
135 uint32_t bytebits_to_byteLSBF(uint8_t *src
, size_t numbits
)
138 for(int i
= 0 ; i
< numbits
; i
++) {
139 num
= (num
<< 1) | *(src
+ (numbits
-(i
+1)));
145 //search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
146 uint8_t preambleSearch(uint8_t *BitStream
, uint8_t *preamble
, size_t pLen
, size_t *size
, size_t *startIdx
)
149 for (int idx
=0; idx
< *size
- pLen
; idx
++){
150 if (memcmp(BitStream
+idx
, preamble
, pLen
) == 0){
157 *size
= idx
- *startIdx
;
166 //takes 1s and 0s and searches for EM410x format - output EM ID
167 uint8_t Em410xDecode(uint8_t *BitStream
, size_t *size
, size_t *startIdx
, uint32_t *hi
, uint64_t *lo
)
169 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
170 // otherwise could be a void with no arguments
173 if (BitStream
[1]>1) return 0; //allow only 1s and 0s
175 // 111111111 bit pattern represent start of frame
176 // include 0 in front to help get start pos
177 uint8_t preamble
[] = {0,1,1,1,1,1,1,1,1,1};
179 uint32_t parityBits
= 0;
183 errChk
= preambleSearch(BitStream
, preamble
, sizeof(preamble
), size
, startIdx
);
184 if (errChk
== 0 || *size
< 64) return 0;
185 if (*size
> 64) FmtLen
= 22;
186 *startIdx
+= 1; //get rid of 0 from preamble
188 for (i
=0; i
<FmtLen
; i
++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
189 parityBits
= bytebits_to_byte(BitStream
+(i
*5)+idx
,5);
190 //check even parity - quit if failed
191 if (parityTest(parityBits
, 5, 0) == 0) return 0;
192 //set uint64 with ID from BitStream
193 for (uint8_t ii
=0; ii
<4; ii
++){
194 *hi
= (*hi
<< 1) | (*lo
>> 63);
195 *lo
= (*lo
<< 1) | (BitStream
[(i
*5)+ii
+idx
]);
198 if (errChk
!= 0) return 1;
199 //skip last 5 bit parity test for simplicity.
205 //demodulates strong heavily clipped samples
206 int cleanAskRawDemod(uint8_t *BinStream
, size_t *size
, int clk
, int invert
, int high
, int low
)
208 size_t bitCnt
=0, smplCnt
=0, errCnt
=0;
209 uint8_t waveHigh
= 0;
210 for (size_t i
=0; i
< *size
; i
++){
211 if (BinStream
[i
] >= high
&& waveHigh
){
213 } else if (BinStream
[i
] <= low
&& !waveHigh
){
215 } else { //transition
216 if ((BinStream
[i
] >= high
&& !waveHigh
) || (BinStream
[i
] <= low
&& waveHigh
)){
217 if (smplCnt
> clk
-(clk
/4)-1) { //full clock
218 if (smplCnt
> clk
+ (clk
/4)+1) { //too many samples
220 BinStream
[bitCnt
++]=7;
221 } else if (waveHigh
) {
222 BinStream
[bitCnt
++] = invert
;
223 BinStream
[bitCnt
++] = invert
;
224 } else if (!waveHigh
) {
225 BinStream
[bitCnt
++] = invert
^ 1;
226 BinStream
[bitCnt
++] = invert
^ 1;
230 } else if (smplCnt
> (clk
/2) - (clk
/4)-1) {
232 BinStream
[bitCnt
++] = invert
;
233 } else if (!waveHigh
) {
234 BinStream
[bitCnt
++] = invert
^ 1;
238 } else if (!bitCnt
) {
240 waveHigh
= (BinStream
[i
] >= high
);
244 //transition bit oops
246 } else { //haven't hit new high or new low yet
256 void askAmp(uint8_t *BitStream
, size_t size
)
258 for(size_t i
= 1; i
<size
; i
++){
259 if (BitStream
[i
]-BitStream
[i
-1]>=30) //large jump up
261 else if(BitStream
[i
]-BitStream
[i
-1]<=-20) //large jump down
268 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
269 int askdemod(uint8_t *BinStream
, size_t *size
, int *clk
, int *invert
, int maxErr
, uint8_t amp
, uint8_t askType
)
271 if (*size
==0) return -1;
272 int start
= DetectASKClock(BinStream
, *size
, clk
, maxErr
); //clock default
273 if (*clk
==0 || start
< 0) return -3;
274 if (*invert
!= 1) *invert
= 0;
275 if (amp
==1) askAmp(BinStream
, *size
);
276 if (g_debugMode
==2) prnt("DEBUG: clk %d, beststart %d", *clk
, start
);
278 uint8_t initLoopMax
= 255;
279 if (initLoopMax
> *size
) initLoopMax
= *size
;
280 // Detect high and lows
281 //25% clip in case highs and lows aren't clipped [marshmellow]
283 if (getHiLo(BinStream
, initLoopMax
, &high
, &low
, 75, 75) < 1)
284 return -2; //just noise
287 // if clean clipped waves detected run alternate demod
288 if (DetectCleanAskWave(BinStream
, *size
, high
, low
)) {
289 if (g_debugMode
==2) prnt("DEBUG: Clean Wave Detected");
290 errCnt
= cleanAskRawDemod(BinStream
, size
, *clk
, *invert
, high
, low
);
291 if (askType
) //askman
292 return manrawdecode(BinStream
, size
, 0);
297 int lastBit
; //set first clock check - can go negative
298 size_t i
, bitnum
= 0; //output counter
300 uint8_t tol
= 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
301 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
302 size_t MaxBits
= 3072;
303 lastBit
= start
- *clk
;
305 for (i
= start
; i
< *size
; ++i
) {
306 if (i
-lastBit
>= *clk
-tol
){
307 if (BinStream
[i
] >= high
) {
308 BinStream
[bitnum
++] = *invert
;
309 } else if (BinStream
[i
] <= low
) {
310 BinStream
[bitnum
++] = *invert
^ 1;
311 } else if (i
-lastBit
>= *clk
+tol
) {
313 BinStream
[bitnum
++]=7;
316 } else { //in tolerance - looking for peak
321 } else if (i
-lastBit
>= (*clk
/2-tol
) && !midBit
&& !askType
){
322 if (BinStream
[i
] >= high
) {
323 BinStream
[bitnum
++] = *invert
;
324 } else if (BinStream
[i
] <= low
) {
325 BinStream
[bitnum
++] = *invert
^ 1;
326 } else if (i
-lastBit
>= *clk
/2+tol
) {
327 BinStream
[bitnum
] = BinStream
[bitnum
-1];
329 } else { //in tolerance - looking for peak
334 if (bitnum
>= MaxBits
) break;
341 //take 10 and 01 and manchester decode
342 //run through 2 times and take least errCnt
343 int manrawdecode(uint8_t * BitStream
, size_t *size
, uint8_t invert
)
345 uint16_t bitnum
=0, MaxBits
= 512, errCnt
= 0;
347 uint16_t bestErr
= 1000, bestRun
= 0;
348 if (*size
< 16) return -1;
349 //find correct start position [alignment]
350 for (ii
=0;ii
<2;++ii
){
351 for (i
=ii
; i
<*size
-3; i
+=2)
352 if (BitStream
[i
]==BitStream
[i
+1])
362 for (i
=bestRun
; i
< *size
-3; i
+=2){
363 if(BitStream
[i
] == 1 && (BitStream
[i
+1] == 0)){
364 BitStream
[bitnum
++]=invert
;
365 } else if((BitStream
[i
] == 0) && BitStream
[i
+1] == 1){
366 BitStream
[bitnum
++]=invert
^1;
368 BitStream
[bitnum
++]=7;
370 if(bitnum
>MaxBits
) break;
376 uint32_t manchesterEncode2Bytes(uint16_t datain
) {
379 for (uint8_t i
=0; i
<16; i
++) {
380 curBit
= (datain
>> (15-i
) & 1);
381 output
|= (1<<(((15-i
)*2)+curBit
));
387 //encode binary data into binary manchester
388 int ManchesterEncode(uint8_t *BitStream
, size_t size
)
390 size_t modIdx
=20000, i
=0;
391 if (size
>modIdx
) return -1;
392 for (size_t idx
=0; idx
< size
; idx
++){
393 BitStream
[idx
+modIdx
++] = BitStream
[idx
];
394 BitStream
[idx
+modIdx
++] = BitStream
[idx
]^1;
396 for (; i
<(size
*2); i
++){
397 BitStream
[i
] = BitStream
[i
+20000];
403 //take 01 or 10 = 1 and 11 or 00 = 0
404 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
405 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
406 int BiphaseRawDecode(uint8_t *BitStream
, size_t *size
, int offset
, int invert
)
411 uint16_t MaxBits
=512;
412 //if not enough samples - error
413 if (*size
< 51) return -1;
414 //check for phase change faults - skip one sample if faulty
415 uint8_t offsetA
= 1, offsetB
= 1;
417 if (BitStream
[i
+1]==BitStream
[i
+2]) offsetA
=0;
418 if (BitStream
[i
+2]==BitStream
[i
+3]) offsetB
=0;
420 if (!offsetA
&& offsetB
) offset
++;
421 for (i
=offset
; i
<*size
-3; i
+=2){
422 //check for phase error
423 if (BitStream
[i
+1]==BitStream
[i
+2]) {
424 BitStream
[bitnum
++]=7;
427 if((BitStream
[i
]==1 && BitStream
[i
+1]==0) || (BitStream
[i
]==0 && BitStream
[i
+1]==1)){
428 BitStream
[bitnum
++]=1^invert
;
429 } else if((BitStream
[i
]==0 && BitStream
[i
+1]==0) || (BitStream
[i
]==1 && BitStream
[i
+1]==1)){
430 BitStream
[bitnum
++]=invert
;
432 BitStream
[bitnum
++]=7;
435 if(bitnum
>MaxBits
) break;
442 // demod gProxIIDemod
443 // error returns as -x
444 // success returns start position in BitStream
445 // BitStream must contain previously askrawdemod and biphasedemoded data
446 int gProxII_Demod(uint8_t BitStream
[], size_t *size
)
449 uint8_t preamble
[] = {1,1,1,1,1,0};
451 uint8_t errChk
= preambleSearch(BitStream
, preamble
, sizeof(preamble
), size
, &startIdx
);
452 if (errChk
== 0) return -3; //preamble not found
453 if (*size
!= 96) return -2; //should have found 96 bits
454 //check first 6 spacer bits to verify format
455 if (!BitStream
[startIdx
+5] && !BitStream
[startIdx
+10] && !BitStream
[startIdx
+15] && !BitStream
[startIdx
+20] && !BitStream
[startIdx
+25] && !BitStream
[startIdx
+30]){
456 //confirmed proper separator bits found
457 //return start position
458 return (int) startIdx
;
463 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
464 size_t fsk_wave_demod(uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
466 size_t last_transition
= 0;
469 if (fchigh
==0) fchigh
=10;
470 if (fclow
==0) fclow
=8;
471 //set the threshold close to 0 (graph) or 128 std to avoid static
472 uint8_t threshold_value
= 123;
473 size_t preLastSample
= 0;
474 size_t LastSample
= 0;
475 size_t currSample
= 0;
476 // sync to first lo-hi transition, and threshold
478 // Need to threshold first sample
479 // skip 160 samples to allow antenna/samples to settle
480 if(dest
[160] < threshold_value
) dest
[0] = 0;
484 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
485 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
486 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
487 for(idx
= 161; idx
< size
-20; idx
++) {
488 // threshold current value
490 if (dest
[idx
] < threshold_value
) dest
[idx
] = 0;
493 // Check for 0->1 transition
494 if (dest
[idx
-1] < dest
[idx
]) { // 0 -> 1 transition
495 preLastSample
= LastSample
;
496 LastSample
= currSample
;
497 currSample
= idx
-last_transition
;
498 if (currSample
< (fclow
-2)){ //0-5 = garbage noise (or 0-3)
499 //do nothing with extra garbage
500 } else if (currSample
< (fchigh
-1)) { //6-8 = 8 sample waves or 3-6 = 5
501 if (LastSample
> (fchigh
-2) && (preLastSample
< (fchigh
-1) || preLastSample
== 0 )){
502 dest
[numBits
-1]=1; //correct previous 9 wave surrounded by 8 waves
506 } else if (currSample
> (fchigh
) && !numBits
) { //12 + and first bit = garbage
507 //do nothing with beginning garbage
508 } else if (currSample
== (fclow
+1) && LastSample
== (fclow
-1)) { // had a 7 then a 9 should be two 8's
510 } else { //9+ = 10 sample waves
513 last_transition
= idx
;
516 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
519 //translate 11111100000 to 10
520 size_t aggregate_bits(uint8_t *dest
, size_t size
, uint8_t rfLen
,
521 uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
523 uint8_t lastval
=dest
[0];
527 for( idx
=1; idx
< size
; idx
++) {
529 if (dest
[idx
]==lastval
) continue;
531 //if lastval was 1, we have a 1->0 crossing
532 if (dest
[idx
-1]==1) {
533 n
= (n
* fclow
+ rfLen
/2) / rfLen
;
534 } else {// 0->1 crossing
535 n
= (n
* fchigh
+ rfLen
/2) / rfLen
;
539 memset(dest
+numBits
, dest
[idx
-1]^invert
, n
);
544 // if valid extra bits at the end were all the same frequency - add them in
545 if (n
> rfLen
/fchigh
) {
546 if (dest
[idx
-2]==1) {
547 n
= (n
* fclow
+ rfLen
/2) / rfLen
;
549 n
= (n
* fchigh
+ rfLen
/2) / rfLen
;
551 memset(dest
+numBits
, dest
[idx
-1]^invert
, n
);
557 //by marshmellow (from holiman's base)
558 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
559 int fskdemod(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
562 size
= fsk_wave_demod(dest
, size
, fchigh
, fclow
);
563 size
= aggregate_bits(dest
, size
, rfLen
, invert
, fchigh
, fclow
);
567 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
568 int HIDdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
570 if (justNoise(dest
, *size
)) return -1;
572 size_t numStart
=0, size2
=*size
, startIdx
=0;
574 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
575 if (*size
< 96*2) return -2;
576 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
577 uint8_t preamble
[] = {0,0,0,1,1,1,0,1};
578 // find bitstring in array
579 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
580 if (errChk
== 0) return -3; //preamble not found
582 numStart
= startIdx
+ sizeof(preamble
);
583 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
584 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
585 if (dest
[idx
] == dest
[idx
+1]){
586 return -4; //not manchester data
588 *hi2
= (*hi2
<<1)|(*hi
>>31);
589 *hi
= (*hi
<<1)|(*lo
>>31);
590 //Then, shift in a 0 or one into low
591 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
596 return (int)startIdx
;
599 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
600 int ParadoxdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
602 if (justNoise(dest
, *size
)) return -1;
604 size_t numStart
=0, size2
=*size
, startIdx
=0;
606 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
607 if (*size
< 96) return -2;
609 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
610 uint8_t preamble
[] = {0,0,0,0,1,1,1,1};
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
620 *hi2
= (*hi2
<<1)|(*hi
>>31);
621 *hi
= (*hi
<<1)|(*lo
>>31);
622 //Then, shift in a 0 or one into low
623 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
628 return (int)startIdx
;
631 int IOdemodFSK(uint8_t *dest
, size_t size
)
633 if (justNoise(dest
, size
)) return -1;
634 //make sure buffer has data
635 if (size
< 66*64) return -2;
637 size
= fskdemod(dest
, size
, 64, 1, 10, 8); // FSK2a RF/64
638 if (size
< 65) return -3; //did we get a good demod?
640 //0 10 20 30 40 50 60
642 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
643 //-----------------------------------------------------------------------------
644 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
646 //XSF(version)facility:codeone+codetwo
649 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,1};
650 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), &size
, &startIdx
);
651 if (errChk
== 0) return -4; //preamble not found
653 if (!dest
[startIdx
+8] && dest
[startIdx
+17]==1 && dest
[startIdx
+26]==1 && dest
[startIdx
+35]==1 && dest
[startIdx
+44]==1 && dest
[startIdx
+53]==1){
654 //confirmed proper separator bits found
655 //return start position
656 return (int) startIdx
;
662 // find viking preamble 0xF200 in already demoded data
663 int VikingDemod_AM(uint8_t *dest
, size_t *size
) {
664 //make sure buffer has data
665 if (*size
< 64*2) return -2;
668 uint8_t preamble
[] = {1,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
669 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
670 if (errChk
== 0) return -4; //preamble not found
671 uint32_t checkCalc
= bytebits_to_byte(dest
+startIdx
,8) ^ bytebits_to_byte(dest
+startIdx
+8,8) ^ bytebits_to_byte(dest
+startIdx
+16,8)
672 ^ bytebits_to_byte(dest
+startIdx
+24,8) ^ bytebits_to_byte(dest
+startIdx
+32,8) ^ bytebits_to_byte(dest
+startIdx
+40,8)
673 ^ bytebits_to_byte(dest
+startIdx
+48,8) ^ bytebits_to_byte(dest
+startIdx
+56,8);
674 if ( checkCalc
!= 0xA8 ) return -5;
675 if (*size
!= 64) return -6;
676 //return start position
677 return (int) startIdx
;
680 // find presco preamble 0x10D in already demoded data
681 int PrescoDemod(uint8_t *dest
, size_t *size
) {
682 //make sure buffer has data
683 if (*size
< 64*2) return -2;
686 uint8_t preamble
[] = {1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0};
687 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
688 if (errChk
== 0) return -4; //preamble not found
689 //return start position
690 return (int) startIdx
;
694 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
695 // BitStream must contain previously askrawdemod and biphasedemoded data
696 int FDXBdemodBI(uint8_t *dest
, size_t *size
)
698 //make sure buffer has enough data
699 if (*size
< 128) return -1;
702 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,0,1};
704 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
705 if (errChk
== 0) return -2; //preamble not found
706 return (int)startIdx
;
710 // FSK Demod then try to locate an AWID ID
711 int AWIDdemodFSK(uint8_t *dest
, size_t *size
)
713 //make sure buffer has enough data
714 if (*size
< 96*50) return -1;
716 if (justNoise(dest
, *size
)) return -2;
719 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
720 if (*size
< 96) return -3; //did we get a good demod?
722 uint8_t preamble
[] = {0,0,0,0,0,0,0,1};
724 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
725 if (errChk
== 0) return -4; //preamble not found
726 if (*size
!= 96) return -5;
727 return (int)startIdx
;
731 // FSK Demod then try to locate a Farpointe Data (pyramid) ID
732 int PyramiddemodFSK(uint8_t *dest
, size_t *size
)
734 //make sure buffer has data
735 if (*size
< 128*50) return -5;
737 //test samples are not just noise
738 if (justNoise(dest
, *size
)) return -1;
741 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
742 if (*size
< 128) return -2; //did we get a good demod?
744 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
746 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
747 if (errChk
== 0) return -4; //preamble not found
748 if (*size
!= 128) return -3;
749 return (int)startIdx
;
753 // to detect a wave that has heavily clipped (clean) samples
754 uint8_t DetectCleanAskWave(uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
)
756 bool allArePeaks
= true;
758 size_t loopEnd
= 512+160;
759 if (loopEnd
> size
) loopEnd
= size
;
760 for (size_t i
=160; i
<loopEnd
; i
++){
761 if (dest
[i
]>low
&& dest
[i
]<high
)
767 if (cntPeaks
> 300) return true;
772 // to help detect clocks on heavily clipped samples
773 // based on count of low to low
774 int DetectStrongAskClock(uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
)
776 uint8_t fndClk
[] = {8,16,32,40,50,64,128};
780 // get to first full low to prime loop and skip incomplete first pulse
781 while ((dest
[i
] < high
) && (i
< size
))
783 while ((dest
[i
] > low
) && (i
< size
))
786 // loop through all samples
788 // measure from low to low
789 while ((dest
[i
] > low
) && (i
< size
))
792 while ((dest
[i
] < high
) && (i
< size
))
794 while ((dest
[i
] > low
) && (i
< size
))
796 //get minimum measured distance
797 if (i
-startwave
< minClk
&& i
< size
)
798 minClk
= i
- startwave
;
801 if (g_debugMode
==2) prnt("DEBUG ASK: detectstrongASKclk smallest wave: %d",minClk
);
802 for (uint8_t clkCnt
= 0; clkCnt
<7; clkCnt
++) {
803 if (minClk
>= fndClk
[clkCnt
]-(fndClk
[clkCnt
]/8) && minClk
<= fndClk
[clkCnt
]+1)
804 return fndClk
[clkCnt
];
810 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
811 // maybe somehow adjust peak trimming value based on samples to fix?
812 // return start index of best starting position for that clock and return clock (by reference)
813 int DetectASKClock(uint8_t dest
[], size_t size
, int *clock
, int maxErr
)
816 uint8_t clk
[] = {255,8,16,32,40,50,64,100,128,255};
818 uint8_t loopCnt
= 255; //don't need to loop through entire array...
819 if (size
<= loopCnt
+60) return -1; //not enough samples
820 size
-= 60; //sometimes there is a strange end wave - filter out this....
821 //if we already have a valid clock
824 if (clk
[i
] == *clock
) clockFnd
= i
;
825 //clock found but continue to find best startpos
827 //get high and low peak
829 if (getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75) < 1) return -1;
831 //test for large clean peaks
833 if (DetectCleanAskWave(dest
, size
, peak
, low
)==1){
834 int ans
= DetectStrongAskClock(dest
, size
, peak
, low
);
835 if (g_debugMode
==2) prnt("DEBUG ASK: detectaskclk Clean Ask Wave Detected: clk %d",ans
);
836 for (i
=clkEnd
-1; i
>0; i
--){
840 return 0; // for strong waves i don't use the 'best start position' yet...
841 //break; //clock found but continue to find best startpos [not yet]
847 uint8_t clkCnt
, tol
= 0;
848 uint16_t bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
849 uint8_t bestStart
[]={0,0,0,0,0,0,0,0,0};
851 size_t arrLoc
, loopEnd
;
859 //test each valid clock from smallest to greatest to see which lines up
860 for(; clkCnt
< clkEnd
; clkCnt
++){
861 if (clk
[clkCnt
] <= 32){
866 //if no errors allowed - keep start within the first clock
867 if (!maxErr
&& size
> clk
[clkCnt
]*2 + tol
&& clk
[clkCnt
]<128) loopCnt
=clk
[clkCnt
]*2;
868 bestErr
[clkCnt
]=1000;
869 //try lining up the peaks by moving starting point (try first few clocks)
870 for (ii
=0; ii
< loopCnt
; ii
++){
871 if (dest
[ii
] < peak
&& dest
[ii
] > low
) continue;
874 // now that we have the first one lined up test rest of wave array
875 loopEnd
= ((size
-ii
-tol
) / clk
[clkCnt
]) - 1;
876 for (i
=0; i
< loopEnd
; ++i
){
877 arrLoc
= ii
+ (i
* clk
[clkCnt
]);
878 if (dest
[arrLoc
] >= peak
|| dest
[arrLoc
] <= low
){
879 }else if (dest
[arrLoc
-tol
] >= peak
|| dest
[arrLoc
-tol
] <= low
){
880 }else if (dest
[arrLoc
+tol
] >= peak
|| dest
[arrLoc
+tol
] <= low
){
881 }else{ //error no peak detected
885 //if we found no errors then we can stop here and a low clock (common clocks)
886 // this is correct one - return this clock
887 if (g_debugMode
== 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d",clk
[clkCnt
],errCnt
,ii
,i
);
888 if(errCnt
==0 && clkCnt
<7) {
889 if (!clockFnd
) *clock
= clk
[clkCnt
];
892 //if we found errors see if it is lowest so far and save it as best run
893 if(errCnt
<bestErr
[clkCnt
]){
894 bestErr
[clkCnt
]=errCnt
;
895 bestStart
[clkCnt
]=ii
;
901 for (iii
=1; iii
<clkEnd
; ++iii
){
902 if (bestErr
[iii
] < bestErr
[best
]){
903 if (bestErr
[iii
] == 0) bestErr
[iii
]=1;
904 // current best bit to error ratio vs new bit to error ratio
905 if ( (size
/clk
[best
])/bestErr
[best
] < (size
/clk
[iii
])/bestErr
[iii
] ){
909 if (g_debugMode
== 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d",clk
[iii
],bestErr
[iii
],clk
[best
],bestStart
[best
]);
911 if (!clockFnd
) *clock
= clk
[best
];
912 return bestStart
[best
];
916 //detect psk clock by reading each phase shift
917 // a phase shift is determined by measuring the sample length of each wave
918 int DetectPSKClock(uint8_t dest
[], size_t size
, int clock
)
920 uint8_t clk
[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
921 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
922 if (size
== 0) return 0;
923 if (size
<loopCnt
) loopCnt
= size
-20;
925 //if we already have a valid clock quit
928 if (clk
[i
] == clock
) return clock
;
930 size_t waveStart
=0, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
931 uint8_t clkCnt
, fc
=0, fullWaveLen
=0, tol
=1;
932 uint16_t peakcnt
=0, errCnt
=0, waveLenCnt
=0;
933 uint16_t bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
934 uint16_t peaksdet
[]={0,0,0,0,0,0,0,0,0};
935 fc
= countFC(dest
, size
, 0);
936 if (fc
!=2 && fc
!=4 && fc
!=8) return -1;
937 if (g_debugMode
==2) prnt("DEBUG PSK: FC: %d",fc
);
939 //find first full wave
940 for (i
=160; i
<loopCnt
; i
++){
941 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
942 if (waveStart
== 0) {
944 //prnt("DEBUG: waveStart: %d",waveStart);
947 //prnt("DEBUG: waveEnd: %d",waveEnd);
948 waveLenCnt
= waveEnd
-waveStart
;
949 if (waveLenCnt
> fc
){
950 firstFullWave
= waveStart
;
951 fullWaveLen
=waveLenCnt
;
958 if (g_debugMode
==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave
,fullWaveLen
);
960 //test each valid clock from greatest to smallest to see which lines up
961 for(clkCnt
=7; clkCnt
>= 1 ; clkCnt
--){
962 lastClkBit
= firstFullWave
; //set end of wave as clock align
966 if (g_debugMode
== 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %d",clk
[clkCnt
],lastClkBit
);
968 for (i
= firstFullWave
+fullWaveLen
-1; i
< loopCnt
-2; i
++){
969 //top edge of wave = start of new wave
970 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
971 if (waveStart
== 0) {
976 waveLenCnt
= waveEnd
-waveStart
;
977 if (waveLenCnt
> fc
){
978 //if this wave is a phase shift
979 if (g_debugMode
== 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart
,waveLenCnt
,lastClkBit
+clk
[clkCnt
]-tol
,i
+1,fc
);
980 if (i
+1 >= lastClkBit
+ clk
[clkCnt
] - tol
){ //should be a clock bit
982 lastClkBit
+=clk
[clkCnt
];
983 } else if (i
<lastClkBit
+8){
984 //noise after a phase shift - ignore
985 } else { //phase shift before supposed to based on clock
988 } else if (i
+1 > lastClkBit
+ clk
[clkCnt
] + tol
+ fc
){
989 lastClkBit
+=clk
[clkCnt
]; //no phase shift but clock bit
998 if (errCnt
<= bestErr
[clkCnt
]) bestErr
[clkCnt
]=errCnt
;
999 if (peakcnt
> peaksdet
[clkCnt
]) peaksdet
[clkCnt
]=peakcnt
;
1001 //all tested with errors
1002 //return the highest clk with the most peaks found
1004 for (i
=7; i
>=1; i
--){
1005 if (peaksdet
[i
] > peaksdet
[best
]) {
1008 if (g_debugMode
== 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk
[i
],peaksdet
[i
],bestErr
[i
],clk
[best
]);
1013 int DetectStrongNRZClk(uint8_t *dest
, size_t size
, int peak
, int low
){
1014 //find shortest transition from high to low
1016 size_t transition1
= 0;
1017 int lowestTransition
= 255;
1018 bool lastWasHigh
= false;
1020 //find first valid beginning of a high or low wave
1021 while ((dest
[i
] >= peak
|| dest
[i
] <= low
) && (i
< size
))
1023 while ((dest
[i
] < peak
&& dest
[i
] > low
) && (i
< size
))
1025 lastWasHigh
= (dest
[i
] >= peak
);
1027 if (i
==size
) return 0;
1030 for (;i
< size
; i
++) {
1031 if ((dest
[i
] >= peak
&& !lastWasHigh
) || (dest
[i
] <= low
&& lastWasHigh
)) {
1032 lastWasHigh
= (dest
[i
] >= peak
);
1033 if (i
-transition1
< lowestTransition
) lowestTransition
= i
-transition1
;
1037 if (lowestTransition
== 255) lowestTransition
= 0;
1038 if (g_debugMode
==2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d",lowestTransition
);
1039 return lowestTransition
;
1043 //detect nrz clock by reading #peaks vs no peaks(or errors)
1044 int DetectNRZClock(uint8_t dest
[], size_t size
, int clock
)
1047 uint8_t clk
[]={8,16,32,40,50,64,100,128,255};
1048 size_t loopCnt
= 4096; //don't need to loop through entire array...
1049 if (size
== 0) return 0;
1050 if (size
<loopCnt
) loopCnt
= size
-20;
1051 //if we already have a valid clock quit
1053 if (clk
[i
] == clock
) return clock
;
1055 //get high and low peak
1057 if (getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75) < 1) return 0;
1059 int lowestTransition
= DetectStrongNRZClk(dest
, size
-20, peak
, low
);
1063 uint16_t smplCnt
= 0;
1064 int16_t peakcnt
= 0;
1065 int16_t peaksdet
[] = {0,0,0,0,0,0,0,0};
1066 uint16_t maxPeak
= 255;
1067 bool firstpeak
= false;
1068 //test for large clipped waves
1069 for (i
=0; i
<loopCnt
; i
++){
1070 if (dest
[i
] >= peak
|| dest
[i
] <= low
){
1071 if (!firstpeak
) continue;
1076 if (maxPeak
> smplCnt
){
1078 //prnt("maxPk: %d",maxPeak);
1081 //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt);
1086 bool errBitHigh
= 0;
1088 uint8_t ignoreCnt
= 0;
1089 uint8_t ignoreWindow
= 4;
1090 bool lastPeakHigh
= 0;
1093 //test each valid clock from smallest to greatest to see which lines up
1094 for(clkCnt
=0; clkCnt
< 8; ++clkCnt
){
1095 //ignore clocks smaller than smallest peak
1096 if (clk
[clkCnt
] < maxPeak
- (clk
[clkCnt
]/4)) continue;
1097 //try lining up the peaks by moving starting point (try first 256)
1098 for (ii
=20; ii
< loopCnt
; ++ii
){
1099 if ((dest
[ii
] >= peak
) || (dest
[ii
] <= low
)){
1103 lastBit
= ii
-clk
[clkCnt
];
1104 //loop through to see if this start location works
1105 for (i
= ii
; i
< size
-20; ++i
) {
1106 //if we are at a clock bit
1107 if ((i
>= lastBit
+ clk
[clkCnt
] - tol
) && (i
<= lastBit
+ clk
[clkCnt
] + tol
)) {
1109 if (dest
[i
] >= peak
|| dest
[i
] <= low
) {
1110 //if same peak don't count it
1111 if ((dest
[i
] >= peak
&& !lastPeakHigh
) || (dest
[i
] <= low
&& lastPeakHigh
)) {
1114 lastPeakHigh
= (dest
[i
] >= peak
);
1117 ignoreCnt
= ignoreWindow
;
1118 lastBit
+= clk
[clkCnt
];
1119 } else if (i
== lastBit
+ clk
[clkCnt
] + tol
) {
1120 lastBit
+= clk
[clkCnt
];
1122 //else if not a clock bit and no peaks
1123 } else if (dest
[i
] < peak
&& dest
[i
] > low
){
1126 if (errBitHigh
==true) peakcnt
--;
1131 // else if not a clock bit but we have a peak
1132 } else if ((dest
[i
]>=peak
|| dest
[i
]<=low
) && (!bitHigh
)) {
1133 //error bar found no clock...
1137 if(peakcnt
>peaksdet
[clkCnt
]) {
1138 peaksdet
[clkCnt
]=peakcnt
;
1145 for (iii
=7; iii
> 0; iii
--){
1146 if ((peaksdet
[iii
] >= (peaksdet
[best
]-1)) && (peaksdet
[iii
] <= peaksdet
[best
]+1) && lowestTransition
) {
1147 if (clk
[iii
] > (lowestTransition
- (clk
[iii
]/8)) && clk
[iii
] < (lowestTransition
+ (clk
[iii
]/8))) {
1150 } else if (peaksdet
[iii
] > peaksdet
[best
]){
1153 if (g_debugMode
==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, maxPeak: %d, bestClk: %d, lowestTrs: %d",clk
[iii
],peaksdet
[iii
],maxPeak
, clk
[best
], lowestTransition
);
1160 // convert psk1 demod to psk2 demod
1161 // only transition waves are 1s
1162 void psk1TOpsk2(uint8_t *BitStream
, size_t size
)
1165 uint8_t lastBit
=BitStream
[0];
1166 for (; i
<size
; i
++){
1167 if (BitStream
[i
]==7){
1169 } else if (lastBit
!=BitStream
[i
]){
1170 lastBit
=BitStream
[i
];
1180 // convert psk2 demod to psk1 demod
1181 // from only transition waves are 1s to phase shifts change bit
1182 void psk2TOpsk1(uint8_t *BitStream
, size_t size
)
1185 for (size_t i
=0; i
<size
; i
++){
1186 if (BitStream
[i
]==1){
1194 // redesigned by marshmellow adjusted from existing decode functions
1195 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1196 int indala26decode(uint8_t *bitStream
, size_t *size
, uint8_t *invert
)
1198 //26 bit 40134 format (don't know other formats)
1199 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
1200 uint8_t preamble_i
[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0};
1201 size_t startidx
= 0;
1202 if (!preambleSearch(bitStream
, preamble
, sizeof(preamble
), size
, &startidx
)){
1203 // if didn't find preamble try again inverting
1204 if (!preambleSearch(bitStream
, preamble_i
, sizeof(preamble_i
), size
, &startidx
)) return -1;
1207 if (*size
!= 64 && *size
!= 224) return -2;
1209 for (size_t i
= startidx
; i
< *size
; i
++)
1212 return (int) startidx
;
1215 // by marshmellow - demodulate NRZ wave
1216 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1217 int nrzRawDemod(uint8_t *dest
, size_t *size
, int *clk
, int *invert
){
1218 if (justNoise(dest
, *size
)) return -1;
1219 *clk
= DetectNRZClock(dest
, *size
, *clk
);
1220 if (*clk
==0) return -2;
1221 size_t i
, gLen
= 4096;
1222 if (gLen
>*size
) gLen
= *size
-20;
1224 if (getHiLo(dest
, gLen
, &high
, &low
, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low
1227 //convert wave samples to 1's and 0's
1228 for(i
=20; i
< *size
-20; i
++){
1229 if (dest
[i
] >= high
) bit
= 1;
1230 if (dest
[i
] <= low
) bit
= 0;
1233 //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit)
1236 for(i
=21; i
< *size
-20; i
++) {
1237 //if transition detected or large number of same bits - store the passed bits
1238 if (dest
[i
] != dest
[i
-1] || (i
-lastBit
) == (10 * *clk
)) {
1239 memset(dest
+numBits
, dest
[i
-1] ^ *invert
, (i
- lastBit
+ (*clk
/4)) / *clk
);
1240 numBits
+= (i
- lastBit
+ (*clk
/4)) / *clk
;
1249 //detects the bit clock for FSK given the high and low Field Clocks
1250 uint8_t detectFSKClk(uint8_t *BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
)
1252 uint8_t clk
[] = {8,16,32,40,50,64,100,128,0};
1253 uint16_t rfLens
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1254 uint8_t rfCnts
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1255 uint8_t rfLensFnd
= 0;
1256 uint8_t lastFCcnt
= 0;
1257 uint16_t fcCounter
= 0;
1258 uint16_t rfCounter
= 0;
1259 uint8_t firstBitFnd
= 0;
1261 if (size
== 0) return 0;
1263 uint8_t fcTol
= ((fcHigh
*100 - fcLow
*100)/2 + 50)/100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2);
1268 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1269 // prime i to first peak / up transition
1270 for (i
= 160; i
< size
-20; i
++)
1271 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
]>=BitStream
[i
+1])
1274 for (; i
< size
-20; i
++){
1278 if (BitStream
[i
] <= BitStream
[i
-1] || BitStream
[i
] < BitStream
[i
+1])
1281 // if we got less than the small fc + tolerance then set it to the small fc
1282 if (fcCounter
< fcLow
+fcTol
)
1284 else //set it to the large fc
1287 //look for bit clock (rf/xx)
1288 if ((fcCounter
< lastFCcnt
|| fcCounter
> lastFCcnt
)){
1289 //not the same size as the last wave - start of new bit sequence
1290 if (firstBitFnd
> 1){ //skip first wave change - probably not a complete bit
1291 for (int ii
=0; ii
<15; ii
++){
1292 if (rfLens
[ii
] >= (rfCounter
-4) && rfLens
[ii
] <= (rfCounter
+4)){
1298 if (rfCounter
> 0 && rfLensFnd
< 15){
1299 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1300 rfCnts
[rfLensFnd
]++;
1301 rfLens
[rfLensFnd
++] = rfCounter
;
1307 lastFCcnt
=fcCounter
;
1311 uint8_t rfHighest
=15, rfHighest2
=15, rfHighest3
=15;
1313 for (i
=0; i
<15; i
++){
1314 //get highest 2 RF values (might need to get more values to compare or compare all?)
1315 if (rfCnts
[i
]>rfCnts
[rfHighest
]){
1316 rfHighest3
=rfHighest2
;
1317 rfHighest2
=rfHighest
;
1319 } else if(rfCnts
[i
]>rfCnts
[rfHighest2
]){
1320 rfHighest3
=rfHighest2
;
1322 } else if(rfCnts
[i
]>rfCnts
[rfHighest3
]){
1325 if (g_debugMode
==2) prnt("DEBUG FSK: RF %d, cnts %d",rfLens
[i
], rfCnts
[i
]);
1327 // set allowed clock remainder tolerance to be 1 large field clock length+1
1328 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1329 uint8_t tol1
= fcHigh
+1;
1331 if (g_debugMode
==2) prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d",rfLens
[rfHighest
],rfLens
[rfHighest2
],rfLens
[rfHighest3
]);
1333 // loop to find the highest clock that has a remainder less than the tolerance
1334 // compare samples counted divided by
1335 // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less)
1337 for (; ii
>=2; ii
--){
1338 if (rfLens
[rfHighest
] % clk
[ii
] < tol1
|| rfLens
[rfHighest
] % clk
[ii
] > clk
[ii
]-tol1
){
1339 if (rfLens
[rfHighest2
] % clk
[ii
] < tol1
|| rfLens
[rfHighest2
] % clk
[ii
] > clk
[ii
]-tol1
){
1340 if (rfLens
[rfHighest3
] % clk
[ii
] < tol1
|| rfLens
[rfHighest3
] % clk
[ii
] > clk
[ii
]-tol1
){
1341 if (g_debugMode
==2) prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance",clk
[ii
]);
1348 if (ii
<0) return 0; // oops we went too far
1354 //countFC is to detect the field clock lengths.
1355 //counts and returns the 2 most common wave lengths
1356 //mainly used for FSK field clock detection
1357 uint16_t countFC(uint8_t *BitStream
, size_t size
, uint8_t fskAdj
)
1359 uint8_t fcLens
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1360 uint16_t fcCnts
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1361 uint8_t fcLensFnd
= 0;
1362 uint8_t lastFCcnt
=0;
1363 uint8_t fcCounter
= 0;
1365 if (size
== 0) return 0;
1367 // prime i to first up transition
1368 for (i
= 160; i
< size
-20; i
++)
1369 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1])
1372 for (; i
< size
-20; i
++){
1373 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1]){
1374 // new up transition
1377 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1378 if (lastFCcnt
==5 && fcCounter
==9) fcCounter
--;
1379 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
1380 if ((fcCounter
==9) || fcCounter
==4) fcCounter
++;
1381 // save last field clock count (fc/xx)
1382 lastFCcnt
= fcCounter
;
1384 // find which fcLens to save it to:
1385 for (int ii
=0; ii
<15; ii
++){
1386 if (fcLens
[ii
]==fcCounter
){
1392 if (fcCounter
>0 && fcLensFnd
<15){
1394 fcCnts
[fcLensFnd
]++;
1395 fcLens
[fcLensFnd
++]=fcCounter
;
1404 uint8_t best1
=14, best2
=14, best3
=14;
1406 // go through fclens and find which ones are bigest 2
1407 for (i
=0; i
<15; i
++){
1408 // get the 3 best FC values
1409 if (fcCnts
[i
]>maxCnt1
) {
1414 } else if(fcCnts
[i
]>fcCnts
[best2
]){
1417 } else if(fcCnts
[i
]>fcCnts
[best3
]){
1420 if (g_debugMode
==2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u",fcLens
[i
],fcCnts
[i
],fcLens
[best1
],fcLens
[best2
]);
1422 if (fcLens
[best1
]==0) return 0;
1423 uint8_t fcH
=0, fcL
=0;
1424 if (fcLens
[best1
]>fcLens
[best2
]){
1431 if ((size
-180)/fcH
/3 > fcCnts
[best1
]+fcCnts
[best2
]) {
1432 if (g_debugMode
==2) prnt("DEBUG countfc: fc is too large: %u > %u. Not psk or fsk",(size
-180)/fcH
/3,fcCnts
[best1
]+fcCnts
[best2
]);
1433 return 0; //lots of waves not psk or fsk
1435 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1437 uint16_t fcs
= (((uint16_t)fcH
)<<8) | fcL
;
1438 if (fskAdj
) return fcs
;
1439 return fcLens
[best1
];
1442 //by marshmellow - demodulate PSK1 wave
1443 //uses wave lengths (# Samples)
1444 int pskRawDemod(uint8_t dest
[], size_t *size
, int *clock
, int *invert
)
1446 if (size
== 0) return -1;
1447 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
1448 if (*size
<loopCnt
) loopCnt
= *size
;
1451 uint8_t curPhase
= *invert
;
1452 size_t i
, waveStart
=1, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
1453 uint8_t fc
=0, fullWaveLen
=0, tol
=1;
1454 uint16_t errCnt
=0, waveLenCnt
=0;
1455 fc
= countFC(dest
, *size
, 0);
1456 if (fc
!=2 && fc
!=4 && fc
!=8) return -1;
1457 //PrintAndLog("DEBUG: FC: %d",fc);
1458 *clock
= DetectPSKClock(dest
, *size
, *clock
);
1459 if (*clock
== 0) return -1;
1460 int avgWaveVal
=0, lastAvgWaveVal
=0;
1461 //find first phase shift
1462 for (i
=0; i
<loopCnt
; i
++){
1463 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1465 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1466 waveLenCnt
= waveEnd
-waveStart
;
1467 if (waveLenCnt
> fc
&& waveStart
> fc
&& !(waveLenCnt
> fc
+2)){ //not first peak and is a large wave but not out of whack
1468 lastAvgWaveVal
= avgWaveVal
/(waveLenCnt
);
1469 firstFullWave
= waveStart
;
1470 fullWaveLen
=waveLenCnt
;
1471 //if average wave value is > graph 0 then it is an up wave or a 1
1472 if (lastAvgWaveVal
> 123) curPhase
^= 1; //fudge graph 0 a little 123 vs 128
1478 avgWaveVal
+= dest
[i
+2];
1480 if (firstFullWave
== 0) {
1481 // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
1482 // so skip a little to ensure we are past any Start Signal
1483 firstFullWave
= 160;
1484 memset(dest
, curPhase
, firstFullWave
/ *clock
);
1486 memset(dest
, curPhase
^1, firstFullWave
/ *clock
);
1489 numBits
+= (firstFullWave
/ *clock
);
1490 //set start of wave as clock align
1491 lastClkBit
= firstFullWave
;
1492 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1493 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1495 dest
[numBits
++] = curPhase
; //set first read bit
1496 for (i
= firstFullWave
+ fullWaveLen
- 1; i
< *size
-3; i
++){
1497 //top edge of wave = start of new wave
1498 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1499 if (waveStart
== 0) {
1502 avgWaveVal
= dest
[i
+1];
1505 waveLenCnt
= waveEnd
-waveStart
;
1506 lastAvgWaveVal
= avgWaveVal
/waveLenCnt
;
1507 if (waveLenCnt
> fc
){
1508 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1509 //this wave is a phase shift
1510 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1511 if (i
+1 >= lastClkBit
+ *clock
- tol
){ //should be a clock bit
1513 dest
[numBits
++] = curPhase
;
1514 lastClkBit
+= *clock
;
1515 } else if (i
< lastClkBit
+10+fc
){
1516 //noise after a phase shift - ignore
1517 } else { //phase shift before supposed to based on clock
1519 dest
[numBits
++] = 7;
1521 } else if (i
+1 > lastClkBit
+ *clock
+ tol
+ fc
){
1522 lastClkBit
+= *clock
; //no phase shift but clock bit
1523 dest
[numBits
++] = curPhase
;
1529 avgWaveVal
+= dest
[i
+1];