]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
c00222b3b69ca23a277f041a027c14a02d2e0feb
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 //-----------------------------------------------------------------------------
14 uint8_t justNoise(uint8_t *BitStream
, size_t size
)
16 static const uint8_t THRESHOLD
= 123;
17 //test samples are not just noise
18 uint8_t justNoise1
= 1;
19 for(size_t idx
=0; idx
< size
&& justNoise1
;idx
++){
20 justNoise1
= BitStream
[idx
] < THRESHOLD
;
26 //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
27 int getHiLo(uint8_t *BitStream
, size_t size
, int *high
, int *low
, uint8_t fuzzHi
, uint8_t fuzzLo
)
31 // get high and low thresholds
32 for (size_t i
=0; i
< size
; i
++){
33 if (BitStream
[i
] > *high
) *high
= BitStream
[i
];
34 if (BitStream
[i
] < *low
) *low
= BitStream
[i
];
36 if (*high
< 123) return -1; // just noise
37 *high
= ((*high
-128)*fuzzHi
+ 12800)/100;
38 *low
= ((*low
-128)*fuzzLo
+ 12800)/100;
43 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
44 // returns 1 if passed
45 uint8_t parityTest(uint32_t bits
, uint8_t bitLen
, uint8_t pType
)
48 for (uint8_t i
= 0; i
< bitLen
; i
++){
49 ans
^= ((bits
>> i
) & 1);
51 //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType);
52 return (ans
== pType
);
56 //search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
57 uint8_t preambleSearch(uint8_t *BitStream
, uint8_t *preamble
, size_t pLen
, size_t *size
, size_t *startIdx
)
60 for (int idx
=0; idx
< *size
- pLen
; idx
++){
61 if (memcmp(BitStream
+idx
, preamble
, pLen
) == 0){
68 *size
= idx
- *startIdx
;
77 //takes 1s and 0s and searches for EM410x format - output EM ID
78 uint8_t Em410xDecode(uint8_t *BitStream
, size_t *size
, size_t *startIdx
, uint32_t *hi
, uint64_t *lo
)
80 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
81 // otherwise could be a void with no arguments
84 if (BitStream
[1]>1){ //allow only 1s and 0s
85 // PrintAndLog("no data found");
88 // 111111111 bit pattern represent start of frame
89 // include 0 in front to help get start pos
90 uint8_t preamble
[] = {0,1,1,1,1,1,1,1,1,1};
92 uint32_t parityBits
= 0;
96 errChk
= preambleSearch(BitStream
, preamble
, sizeof(preamble
), size
, startIdx
);
97 if (errChk
== 0 || *size
< 64) return 0;
98 if (*size
> 64) FmtLen
= 22;
99 *startIdx
+= 1; //get rid of 0 from preamble
101 for (i
=0; i
<FmtLen
; i
++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
102 parityBits
= bytebits_to_byte(BitStream
+(i
*5)+idx
,5);
103 //check even parity - quit if failed
104 if (parityTest(parityBits
, 5, 0) == 0) return 0;
105 //set uint64 with ID from BitStream
106 for (uint8_t ii
=0; ii
<4; ii
++){
107 *hi
= (*hi
<< 1) | (*lo
>> 63);
108 *lo
= (*lo
<< 1) | (BitStream
[(i
*5)+ii
+idx
]);
111 if (errChk
!= 0) return 1;
112 //skip last 5 bit parity test for simplicity.
117 // demodulates strong heavily clipped samples
118 int cleanAskRawDemod(uint8_t *BinStream
, size_t *size
, int clk
, int invert
, int high
, int low
)
120 size_t bitCnt
=0, smplCnt
=0, errCnt
=0;
121 uint8_t waveHigh
= 0;
122 //PrintAndLog("clk: %d", clk);
123 for (size_t i
=0; i
< *size
; i
++){
124 if (BinStream
[i
] >= high
&& waveHigh
){
126 } else if (BinStream
[i
] <= low
&& !waveHigh
){
128 } else { //transition
129 if ((BinStream
[i
] >= high
&& !waveHigh
) || (BinStream
[i
] <= low
&& waveHigh
)){
130 if (smplCnt
> clk
-(clk
/4)-1) { //full clock
131 if (smplCnt
> clk
+ (clk
/4)+1) { //too many samples
133 BinStream
[bitCnt
++]=77;
134 } else if (waveHigh
) {
135 BinStream
[bitCnt
++] = invert
;
136 BinStream
[bitCnt
++] = invert
;
137 } else if (!waveHigh
) {
138 BinStream
[bitCnt
++] = invert
^ 1;
139 BinStream
[bitCnt
++] = invert
^ 1;
143 } else if (smplCnt
> (clk
/2) - (clk
/4)-1) {
145 BinStream
[bitCnt
++] = invert
;
146 } else if (!waveHigh
) {
147 BinStream
[bitCnt
++] = invert
^ 1;
151 } else if (!bitCnt
) {
153 waveHigh
= (BinStream
[i
] >= high
);
157 //transition bit oops
159 } else { //haven't hit new high or new low yet
169 //takes 3 arguments - clock, invert, maxErr as integers
170 //attempts to demodulate ask while decoding manchester
171 //prints binary found and saves in graphbuffer for further commands
172 int askmandemod(uint8_t *BinStream
, size_t *size
, int *clk
, int *invert
, int maxErr
)
175 int start
= DetectASKClock(BinStream
, *size
, clk
, maxErr
); //clock default
176 if (*clk
==0 || start
< 0) return -3;
177 if (*invert
!= 1) *invert
=0;
178 uint8_t initLoopMax
= 255;
179 if (initLoopMax
> *size
) initLoopMax
= *size
;
180 // Detect high and lows
181 // 25% fuzz in case highs and lows aren't clipped [marshmellow]
183 if (getHiLo(BinStream
, initLoopMax
, &high
, &low
, 75, 75) < 1) return -2; //just noise
185 // if clean clipped waves detected run alternate demod
186 if (DetectCleanAskWave(BinStream
, *size
, high
, low
)) {
187 cleanAskRawDemod(BinStream
, size
, *clk
, *invert
, high
, low
);
188 return manrawdecode(BinStream
, size
);
191 // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
192 int lastBit
; //set first clock check
193 uint16_t bitnum
= 0; //output counter
194 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
195 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
196 uint16_t errCnt
= 0, MaxBits
= 512;
197 lastBit
= start
- *clk
;
198 for (i
= start
; i
< *size
; ++i
) {
199 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
200 //high found and we are expecting a bar
202 BinStream
[bitnum
++] = *invert
;
203 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
204 //low found and we are expecting a bar
206 BinStream
[bitnum
++] = *invert
^ 1;
207 } else if ((i
-lastBit
)>(*clk
+tol
)){
208 //should have hit a high or low based on clock!!
209 //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);
211 BinStream
[bitnum
++] = 77;
214 lastBit
+= *clk
;//skip over error
216 if (bitnum
>= MaxBits
) break;
223 //encode binary data into binary manchester
224 int ManchesterEncode(uint8_t *BitStream
, size_t size
)
226 size_t modIdx
=20000, i
=0;
227 if (size
>modIdx
) return -1;
228 for (size_t idx
=0; idx
< size
; idx
++){
229 BitStream
[idx
+modIdx
++] = BitStream
[idx
];
230 BitStream
[idx
+modIdx
++] = BitStream
[idx
]^1;
232 for (; i
<(size
*2); i
++){
233 BitStream
[i
] = BitStream
[i
+20000];
239 //take 10 and 01 and manchester decode
240 //run through 2 times and take least errCnt
241 int manrawdecode(uint8_t * BitStream
, size_t *size
)
243 uint16_t bitnum
=0, MaxBits
= 512, errCnt
= 0;
245 uint16_t bestErr
= 1000, bestRun
= 0;
246 if (size
== 0) return -1;
247 for (ii
=0;ii
<2;++ii
){
248 for (i
=ii
; i
<*size
-2; i
+=2)
249 if (BitStream
[i
]==BitStream
[i
+1])
258 for (i
=bestRun
; i
< *size
-2; i
+=2){
259 if(BitStream
[i
] == 1 && (BitStream
[i
+1] == 0)){
260 BitStream
[bitnum
++]=0;
261 } else if((BitStream
[i
] == 0) && BitStream
[i
+1] == 1){
262 BitStream
[bitnum
++]=1;
264 BitStream
[bitnum
++]=77;
266 if(bitnum
>MaxBits
) break;
273 //take 01 or 10 = 1 and 11 or 00 = 0
274 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
275 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
276 int BiphaseRawDecode(uint8_t *BitStream
, size_t *size
, int offset
, int invert
)
281 uint16_t MaxBits
=512;
282 //if not enough samples - error
283 if (*size
< 51) return -1;
284 //check for phase change faults - skip one sample if faulty
285 uint8_t offsetA
= 1, offsetB
= 1;
287 if (BitStream
[i
+1]==BitStream
[i
+2]) offsetA
=0;
288 if (BitStream
[i
+2]==BitStream
[i
+3]) offsetB
=0;
290 if (!offsetA
&& offsetB
) offset
++;
291 for (i
=offset
; i
<*size
-3; i
+=2){
292 //check for phase error
293 if (BitStream
[i
+1]==BitStream
[i
+2]) {
294 BitStream
[bitnum
++]=77;
297 if((BitStream
[i
]==1 && BitStream
[i
+1]==0) || (BitStream
[i
]==0 && BitStream
[i
+1]==1)){
298 BitStream
[bitnum
++]=1^invert
;
299 } else if((BitStream
[i
]==0 && BitStream
[i
+1]==0) || (BitStream
[i
]==1 && BitStream
[i
+1]==1)){
300 BitStream
[bitnum
++]=invert
;
302 BitStream
[bitnum
++]=77;
305 if(bitnum
>MaxBits
) break;
312 void askAmp(uint8_t *BitStream
, size_t size
)
316 for(size_t i
= 1; i
<size
; i
++){
317 if (BitStream
[i
]-BitStream
[i
-1]>=30) //large jump up
319 else if(BitStream
[i
]-BitStream
[i
-1]<=-20) //large jump down
322 shiftedVal
=BitStream
[i
]+shift
;
326 else if (shiftedVal
<0)
328 BitStream
[i
-1] = shiftedVal
;
334 //takes 3 arguments - clock, invert and maxErr as integers
335 //attempts to demodulate ask only
336 int askrawdemod(uint8_t *BinStream
, size_t *size
, int *clk
, int *invert
, int maxErr
, uint8_t amp
)
338 if (*size
==0) return -1;
339 int start
= DetectASKClock(BinStream
, *size
, clk
, maxErr
); //clock default
340 if (*clk
==0 || start
< 0) return -1;
341 if (*invert
!= 1) *invert
= 0;
342 if (amp
==1) askAmp(BinStream
, *size
);
344 uint8_t initLoopMax
= 255;
345 if (initLoopMax
> *size
) initLoopMax
= *size
;
346 // Detect high and lows
347 //25% clip in case highs and lows aren't clipped [marshmellow]
349 if (getHiLo(BinStream
, initLoopMax
, &high
, &low
, 75, 75) < 1)
350 return -1; //just noise
352 // if clean clipped waves detected run alternate demod
353 if (DetectCleanAskWave(BinStream
, *size
, high
, low
))
354 return cleanAskRawDemod(BinStream
, size
, *clk
, *invert
, high
, low
);
356 int lastBit
; //set first clock check - can go negative
357 size_t i
, errCnt
= 0, bitnum
= 0; //output counter
359 size_t MaxBits
= 1024;
360 lastBit
= start
- *clk
;
362 for (i
= start
; i
< *size
; ++i
) {
363 if (i
- lastBit
> *clk
){
364 if (BinStream
[i
] >= high
) {
365 BinStream
[bitnum
++] = *invert
;
366 } else if (BinStream
[i
] <= low
) {
367 BinStream
[bitnum
++] = *invert
^ 1;
370 BinStream
[bitnum
++]=77;
376 } else if (i
-lastBit
> (*clk
/2) && midBit
== 0){
377 if (BinStream
[i
] >= high
) {
378 BinStream
[bitnum
++] = *invert
;
379 } else if (BinStream
[i
] <= low
) {
380 BinStream
[bitnum
++] = *invert
^ 1;
383 BinStream
[bitnum
] = BinStream
[bitnum
-1];
388 if (bitnum
>= MaxBits
) break;
394 // demod gProxIIDemod
395 // error returns as -x
396 // success returns start position in BitStream
397 // BitStream must contain previously askrawdemod and biphasedemoded data
398 int gProxII_Demod(uint8_t BitStream
[], size_t *size
)
401 uint8_t preamble
[] = {1,1,1,1,1,0};
403 uint8_t errChk
= preambleSearch(BitStream
, preamble
, sizeof(preamble
), size
, &startIdx
);
404 if (errChk
== 0) return -3; //preamble not found
405 if (*size
!= 96) return -2; //should have found 96 bits
406 //check first 6 spacer bits to verify format
407 if (!BitStream
[startIdx
+5] && !BitStream
[startIdx
+10] && !BitStream
[startIdx
+15] && !BitStream
[startIdx
+20] && !BitStream
[startIdx
+25] && !BitStream
[startIdx
+30]){
408 //confirmed proper separator bits found
409 //return start position
410 return (int) startIdx
;
415 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
416 size_t fsk_wave_demod(uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
418 size_t last_transition
= 0;
421 if (fchigh
==0) fchigh
=10;
422 if (fclow
==0) fclow
=8;
423 //set the threshold close to 0 (graph) or 128 std to avoid static
424 uint8_t threshold_value
= 123;
426 // sync to first lo-hi transition, and threshold
428 // Need to threshold first sample
430 if(dest
[0] < threshold_value
) dest
[0] = 0;
434 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
435 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
436 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
437 for(idx
= 1; idx
< size
; idx
++) {
438 // threshold current value
440 if (dest
[idx
] < threshold_value
) dest
[idx
] = 0;
443 // Check for 0->1 transition
444 if (dest
[idx
-1] < dest
[idx
]) { // 0 -> 1 transition
445 if ((idx
-last_transition
)<(fclow
-2)){ //0-5 = garbage noise
446 //do nothing with extra garbage
447 } else if ((idx
-last_transition
) < (fchigh
-1)) { //6-8 = 8 waves
449 } else if ((idx
-last_transition
) > (fchigh
+1) && !numBits
) { //12 + and first bit = garbage
450 //do nothing with beginning garbage
451 } else { //9+ = 10 waves
454 last_transition
= idx
;
457 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
460 //translate 11111100000 to 10
461 size_t aggregate_bits(uint8_t *dest
, size_t size
, uint8_t rfLen
,
462 uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
464 uint8_t lastval
=dest
[0];
468 for( idx
=1; idx
< size
; idx
++) {
470 if (dest
[idx
]==lastval
) continue;
472 //if lastval was 1, we have a 1->0 crossing
473 if (dest
[idx
-1]==1) {
474 if (!numBits
&& n
< rfLen
/fclow
) {
479 n
= (n
* fclow
+ rfLen
/2) / rfLen
;
480 } else {// 0->1 crossing
481 //test first bitsample too small
482 if (!numBits
&& n
< rfLen
/fchigh
) {
487 n
= (n
* fchigh
+ rfLen
/2) / rfLen
;
491 memset(dest
+numBits
, dest
[idx
-1]^invert
, n
);
496 // if valid extra bits at the end were all the same frequency - add them in
497 if (n
> rfLen
/fchigh
) {
498 if (dest
[idx
-2]==1) {
499 n
= (n
* fclow
+ rfLen
/2) / rfLen
;
501 n
= (n
* fchigh
+ rfLen
/2) / rfLen
;
503 memset(dest
+numBits
, dest
[idx
-1]^invert
, n
);
508 //by marshmellow (from holiman's base)
509 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
510 int fskdemod(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
513 size
= fsk_wave_demod(dest
, size
, fchigh
, fclow
);
514 size
= aggregate_bits(dest
, size
, rfLen
, invert
, fchigh
, fclow
);
518 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
519 int HIDdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
521 if (justNoise(dest
, *size
)) return -1;
523 size_t numStart
=0, size2
=*size
, startIdx
=0;
525 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
526 if (*size
< 96*2) return -2;
527 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
528 uint8_t preamble
[] = {0,0,0,1,1,1,0,1};
529 // find bitstring in array
530 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
531 if (errChk
== 0) return -3; //preamble not found
533 numStart
= startIdx
+ sizeof(preamble
);
534 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
535 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
536 if (dest
[idx
] == dest
[idx
+1]){
537 return -4; //not manchester data
539 *hi2
= (*hi2
<<1)|(*hi
>>31);
540 *hi
= (*hi
<<1)|(*lo
>>31);
541 //Then, shift in a 0 or one into low
542 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
547 return (int)startIdx
;
550 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
551 int ParadoxdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
553 if (justNoise(dest
, *size
)) return -1;
555 size_t numStart
=0, size2
=*size
, startIdx
=0;
557 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
558 if (*size
< 96) return -2;
560 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
561 uint8_t preamble
[] = {0,0,0,0,1,1,1,1};
563 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
564 if (errChk
== 0) return -3; //preamble not found
566 numStart
= startIdx
+ sizeof(preamble
);
567 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
568 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
569 if (dest
[idx
] == dest
[idx
+1])
570 return -4; //not manchester data
571 *hi2
= (*hi2
<<1)|(*hi
>>31);
572 *hi
= (*hi
<<1)|(*lo
>>31);
573 //Then, shift in a 0 or one into low
574 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
579 return (int)startIdx
;
582 uint32_t bytebits_to_byte(uint8_t* src
, size_t numbits
)
585 for(int i
= 0 ; i
< numbits
; i
++)
587 num
= (num
<< 1) | (*src
);
593 int IOdemodFSK(uint8_t *dest
, size_t size
)
595 if (justNoise(dest
, size
)) return -1;
596 //make sure buffer has data
597 if (size
< 66*64) return -2;
599 size
= fskdemod(dest
, size
, 64, 1, 10, 8); // FSK2a RF/64
600 if (size
< 65) return -3; //did we get a good demod?
602 //0 10 20 30 40 50 60
604 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
605 //-----------------------------------------------------------------------------
606 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
608 //XSF(version)facility:codeone+codetwo
611 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,1};
612 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), &size
, &startIdx
);
613 if (errChk
== 0) return -4; //preamble not found
615 if (!dest
[startIdx
+8] && dest
[startIdx
+17]==1 && dest
[startIdx
+26]==1 && dest
[startIdx
+35]==1 && dest
[startIdx
+44]==1 && dest
[startIdx
+53]==1){
616 //confirmed proper separator bits found
617 //return start position
618 return (int) startIdx
;
624 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
625 // Parity Type (1 for odd 0 for even), and binary Length (length to run)
626 size_t removeParity(uint8_t *BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
)
628 uint32_t parityWd
= 0;
629 size_t j
= 0, bitCnt
= 0;
630 for (int word
= 0; word
< (bLen
); word
+=pLen
){
631 for (int bit
=0; bit
< pLen
; bit
++){
632 parityWd
= (parityWd
<< 1) | BitStream
[startIdx
+word
+bit
];
633 BitStream
[j
++] = (BitStream
[startIdx
+word
+bit
]);
636 // if parity fails then return 0
637 if (parityTest(parityWd
, pLen
, pType
) == 0) return -1;
641 // if we got here then all the parities passed
642 //return ID start index and size
647 // FSK Demod then try to locate an AWID ID
648 int AWIDdemodFSK(uint8_t *dest
, size_t *size
)
650 //make sure buffer has enough data
651 if (*size
< 96*50) return -1;
653 if (justNoise(dest
, *size
)) return -2;
656 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
657 if (*size
< 96) return -3; //did we get a good demod?
659 uint8_t preamble
[] = {0,0,0,0,0,0,0,1};
661 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
662 if (errChk
== 0) return -4; //preamble not found
663 if (*size
!= 96) return -5;
664 return (int)startIdx
;
668 // FSK Demod then try to locate an Farpointe Data (pyramid) ID
669 int PyramiddemodFSK(uint8_t *dest
, size_t *size
)
671 //make sure buffer has data
672 if (*size
< 128*50) return -5;
674 //test samples are not just noise
675 if (justNoise(dest
, *size
)) return -1;
678 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
679 if (*size
< 128) return -2; //did we get a good demod?
681 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
683 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
684 if (errChk
== 0) return -4; //preamble not found
685 if (*size
!= 128) return -3;
686 return (int)startIdx
;
690 uint8_t DetectCleanAskWave(uint8_t dest
[], size_t size
, int high
, int low
)
694 size_t loopEnd
= 572;
695 if (loopEnd
> size
) loopEnd
= size
;
696 for (size_t i
=60; i
<loopEnd
; i
++){
697 if (dest
[i
]>low
&& dest
[i
]<high
)
703 if (cntPeaks
> 300) return 1;
709 // to help detect clocks on heavily clipped samples
710 // based on counts between zero crossings
711 int DetectStrongAskClock(uint8_t dest
[], size_t size
)
713 int clk
[]={0,8,16,32,40,50,64,100,128};
719 for (;idx
< size
; idx
++){
724 if (highCnt
!= 0) highCnt2
= highCnt
;
726 } else if (cnt
> highCnt2
) {
733 } else if (dest
[idx
] <= 128){
737 if (highCnt
!= 0) highCnt2
= highCnt
;
739 } else if (cnt
> highCnt2
) {
749 for (idx
=8; idx
>0; idx
--){
751 if (clk
[idx
] >= highCnt
- tol
&& clk
[idx
] <= highCnt
+ tol
)
753 if (clk
[idx
] >= highCnt2
- tol
&& clk
[idx
] <= highCnt2
+ tol
)
760 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
761 // maybe somehow adjust peak trimming value based on samples to fix?
762 // return start index of best starting position for that clock and return clock (by reference)
763 int DetectASKClock(uint8_t dest
[], size_t size
, int *clock
, int maxErr
)
766 uint8_t clk
[]={255,8,16,32,40,50,64,100,128,255};
767 uint8_t loopCnt
= 255; //don't need to loop through entire array...
768 if (size
==0) return -1;
769 if (size
<= loopCnt
) loopCnt
= size
-1; //not enough samples
771 //if we already have a valid clock
774 if (clk
[i
] == *clock
) clockFnd
=i
;
775 //clock found but continue to find best startpos
777 //get high and low peak
779 if (getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75) < 1) return -1;
781 //test for large clean peaks
782 if (DetectCleanAskWave(dest
, size
, peak
, low
)==1){
783 int ans
= DetectStrongAskClock(dest
, size
);
788 break; //clock found but continue to find best startpos
793 uint8_t clkCnt
, tol
= 0;
794 uint16_t bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
795 uint8_t bestStart
[]={0,0,0,0,0,0,0,0,0};
797 size_t arrLoc
, loopEnd
;
798 //test each valid clock from smallest to greatest to see which lines up
800 if (clockFnd
>0) clkEnd
=clockFnd
+1;
803 for(clkCnt
=clockFnd
; clkCnt
< clkEnd
; clkCnt
++){
804 if (clk
[clkCnt
] == 32){
809 if (!maxErr
&& loopCnt
>clk
[clkCnt
]*3) loopCnt
=clk
[clkCnt
]*3;
810 bestErr
[clkCnt
]=1000;
811 //try lining up the peaks by moving starting point (try first few clocks)
812 for (ii
=0; ii
< loopCnt
-tol
-clk
[clkCnt
]; ii
++){
813 if (dest
[ii
] < peak
&& dest
[ii
] > low
) continue;
816 // now that we have the first one lined up test rest of wave array
817 loopEnd
= ((size
-ii
-tol
) / clk
[clkCnt
]) - 1;
818 for (i
=0; i
< loopEnd
; ++i
){
819 arrLoc
= ii
+ (i
* clk
[clkCnt
]);
820 if (dest
[arrLoc
] >= peak
|| dest
[arrLoc
] <= low
){
821 }else if (dest
[arrLoc
-tol
] >= peak
|| dest
[arrLoc
-tol
] <= low
){
822 }else if (dest
[arrLoc
+tol
] >= peak
|| dest
[arrLoc
+tol
] <= low
){
823 }else{ //error no peak detected
827 //if we found no errors then we can stop here
828 // this is correct one - return this clock
829 //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
830 if(errCnt
==0 && clkCnt
<6) {
831 *clock
= clk
[clkCnt
];
834 //if we found errors see if it is lowest so far and save it as best run
835 if(errCnt
<bestErr
[clkCnt
]){
836 bestErr
[clkCnt
]=errCnt
;
837 bestStart
[clkCnt
]=ii
;
843 for (iii
=0; iii
<8; ++iii
){
844 if (bestErr
[iii
] < bestErr
[best
]){
845 if (bestErr
[iii
] == 0) bestErr
[iii
]=1;
846 // current best bit to error ratio vs new bit to error ratio
847 if ( (size
/clk
[best
])/bestErr
[best
] < (size
/clk
[iii
])/bestErr
[iii
] ){
852 if (bestErr
[best
] > maxErr
) return -1;
854 return bestStart
[best
];
858 //detect psk clock by reading each phase shift
859 // a phase shift is determined by measuring the sample length of each wave
860 int DetectPSKClock(uint8_t dest
[], size_t size
, int clock
)
862 uint8_t clk
[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
863 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
864 if (size
== 0) return 0;
865 if (size
<loopCnt
) loopCnt
= size
;
867 //if we already have a valid clock quit
870 if (clk
[i
] == clock
) return clock
;
872 size_t waveStart
=0, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
873 uint8_t clkCnt
, fc
=0, fullWaveLen
=0, tol
=1;
874 uint16_t peakcnt
=0, errCnt
=0, waveLenCnt
=0;
875 uint16_t bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
876 uint16_t peaksdet
[]={0,0,0,0,0,0,0,0,0};
877 fc
= countFC(dest
, size
, 0);
878 if (fc
!=2 && fc
!=4 && fc
!=8) return -1;
879 //PrintAndLog("DEBUG: FC: %d",fc);
881 //find first full wave
882 for (i
=0; i
<loopCnt
; i
++){
883 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
884 if (waveStart
== 0) {
886 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
889 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
890 waveLenCnt
= waveEnd
-waveStart
;
891 if (waveLenCnt
> fc
){
892 firstFullWave
= waveStart
;
893 fullWaveLen
=waveLenCnt
;
900 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
902 //test each valid clock from greatest to smallest to see which lines up
903 for(clkCnt
=7; clkCnt
>= 1 ; clkCnt
--){
904 lastClkBit
= firstFullWave
; //set end of wave as clock align
908 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
910 for (i
= firstFullWave
+fullWaveLen
-1; i
< loopCnt
-2; i
++){
911 //top edge of wave = start of new wave
912 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
913 if (waveStart
== 0) {
918 waveLenCnt
= waveEnd
-waveStart
;
919 if (waveLenCnt
> fc
){
920 //if this wave is a phase shift
921 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc);
922 if (i
+1 >= lastClkBit
+ clk
[clkCnt
] - tol
){ //should be a clock bit
924 lastClkBit
+=clk
[clkCnt
];
925 } else if (i
<lastClkBit
+8){
926 //noise after a phase shift - ignore
927 } else { //phase shift before supposed to based on clock
930 } else if (i
+1 > lastClkBit
+ clk
[clkCnt
] + tol
+ fc
){
931 lastClkBit
+=clk
[clkCnt
]; //no phase shift but clock bit
940 if (errCnt
<= bestErr
[clkCnt
]) bestErr
[clkCnt
]=errCnt
;
941 if (peakcnt
> peaksdet
[clkCnt
]) peaksdet
[clkCnt
]=peakcnt
;
943 //all tested with errors
944 //return the highest clk with the most peaks found
946 for (i
=7; i
>=1; i
--){
947 if (peaksdet
[i
] > peaksdet
[best
]) {
950 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
956 //detect nrz clock by reading #peaks vs no peaks(or errors)
957 int DetectNRZClock(uint8_t dest
[], size_t size
, int clock
)
960 uint8_t clk
[]={8,16,32,40,50,64,100,128,255};
961 size_t loopCnt
= 4096; //don't need to loop through entire array...
962 if (size
== 0) return 0;
963 if (size
<loopCnt
) loopCnt
= size
;
965 //if we already have a valid clock quit
967 if (clk
[i
] == clock
) return clock
;
969 //get high and low peak
971 if (getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75) < 1) return 0;
973 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
978 uint16_t peaksdet
[]={0,0,0,0,0,0,0,0};
980 //test for large clipped waves
981 for (i
=0; i
<loopCnt
; i
++){
982 if (dest
[i
] >= peak
|| dest
[i
] <= low
){
985 if (peakcnt
>0 && maxPeak
< peakcnt
){
992 //test each valid clock from smallest to greatest to see which lines up
993 for(clkCnt
=0; clkCnt
< 8; ++clkCnt
){
994 //ignore clocks smaller than largest peak
995 if (clk
[clkCnt
]<maxPeak
) continue;
997 //try lining up the peaks by moving starting point (try first 256)
998 for (ii
=0; ii
< loopCnt
; ++ii
){
999 if ((dest
[ii
] >= peak
) || (dest
[ii
] <= low
)){
1001 // now that we have the first one lined up test rest of wave array
1002 for (i
=0; i
< ((int)((size
-ii
-tol
)/clk
[clkCnt
])-1); ++i
){
1003 if (dest
[ii
+(i
*clk
[clkCnt
])]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])]<=low
){
1007 if(peakcnt
>peaksdet
[clkCnt
]) {
1008 peaksdet
[clkCnt
]=peakcnt
;
1015 for (iii
=7; iii
> 0; iii
--){
1016 if (peaksdet
[iii
] > peaksdet
[best
]){
1019 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
1025 // convert psk1 demod to psk2 demod
1026 // only transition waves are 1s
1027 void psk1TOpsk2(uint8_t *BitStream
, size_t size
)
1030 uint8_t lastBit
=BitStream
[0];
1031 for (; i
<size
; i
++){
1032 if (BitStream
[i
]==77){
1034 } else if (lastBit
!=BitStream
[i
]){
1035 lastBit
=BitStream
[i
];
1045 // convert psk2 demod to psk1 demod
1046 // from only transition waves are 1s to phase shifts change bit
1047 void psk2TOpsk1(uint8_t *BitStream
, size_t size
)
1050 for (size_t i
=0; i
<size
; i
++){
1051 if (BitStream
[i
]==1){
1059 // redesigned by marshmellow adjusted from existing decode functions
1060 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1061 int indala26decode(uint8_t *bitStream
, size_t *size
, uint8_t *invert
)
1063 //26 bit 40134 format (don't know other formats)
1065 int long_wait
=29;//29 leading zeros in format
1071 // Finding the start of a UID
1072 for (start
= 0; start
<= *size
- 250; start
++) {
1073 first
= bitStream
[start
];
1074 for (i
= start
; i
< start
+ long_wait
; i
++) {
1075 if (bitStream
[i
] != first
) {
1079 if (i
== (start
+ long_wait
)) {
1083 if (start
== *size
- 250 + 1) {
1084 // did not find start sequence
1087 // Inverting signal if needed
1089 for (i
= start
; i
< *size
; i
++) {
1090 bitStream
[i
] = !bitStream
[i
];
1096 //found start once now test length by finding next one
1097 for (ii
=start
+29; ii
<= *size
- 250; ii
++) {
1098 first2
= bitStream
[ii
];
1099 for (iii
= ii
; iii
< ii
+ long_wait
; iii
++) {
1100 if (bitStream
[iii
] != first2
) {
1104 if (iii
== (ii
+ long_wait
)) {
1108 if (ii
== *size
- 250 + 1){
1109 // did not find second start sequence
1116 for (ii
= 0; ii
< bitCnt
; ii
++) {
1117 bitStream
[ii
] = bitStream
[i
++];
1123 // by marshmellow - demodulate NRZ wave (both similar enough)
1124 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1125 // there probably is a much simpler way to do this....
1126 int nrzRawDemod(uint8_t *dest
, size_t *size
, int *clk
, int *invert
, int maxErr
)
1128 if (justNoise(dest
, *size
)) return -1;
1129 *clk
= DetectNRZClock(dest
, *size
, *clk
);
1130 if (*clk
==0) return -2;
1131 size_t i
, gLen
= 4096;
1132 if (gLen
>*size
) gLen
= *size
;
1134 if (getHiLo(dest
, gLen
, &high
, &low
, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low
1135 int lastBit
= 0; //set first clock check
1136 size_t iii
= 0, bitnum
= 0; //bitnum counter
1137 uint16_t errCnt
= 0, MaxBits
= 1000;
1138 size_t bestErrCnt
= maxErr
+1;
1139 size_t bestPeakCnt
= 0, bestPeakStart
= 0;
1140 uint8_t bestFirstPeakHigh
=0, firstPeakHigh
=0, curBit
=0, bitHigh
=0, errBitHigh
=0;
1141 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
1143 uint8_t ignoreWindow
=4;
1144 uint8_t ignoreCnt
=ignoreWindow
; //in case of noise near peak
1145 //loop to find first wave that works - align to clock
1146 for (iii
=0; iii
< gLen
; ++iii
){
1147 if ((dest
[iii
]>=high
) || (dest
[iii
]<=low
)){
1148 if (dest
[iii
]>=high
) firstPeakHigh
=1;
1149 else firstPeakHigh
=0;
1153 //loop through to see if this start location works
1154 for (i
= iii
; i
< *size
; ++i
) {
1155 // if we are at a clock bit
1156 if ((i
>= lastBit
+ *clk
- tol
) && (i
<= lastBit
+ *clk
+ tol
)) {
1158 if (dest
[i
] >= high
|| dest
[i
] <= low
) {
1162 ignoreCnt
= ignoreWindow
;
1164 } else if (i
== lastBit
+ *clk
+ tol
) {
1167 //else if no bars found
1168 } else if (dest
[i
] < high
&& dest
[i
] > low
){
1171 if (errBitHigh
==1) errCnt
++;
1176 } else if ((dest
[i
]>=high
|| dest
[i
]<=low
) && (bitHigh
==0)) {
1177 //error bar found no clock...
1180 if (((i
-iii
) / *clk
)>=MaxBits
) break;
1182 //we got more than 64 good bits and not all errors
1183 if (((i
-iii
) / *clk
) > 64 && (errCnt
<= (maxErr
))) {
1184 //possible good read
1185 if (!errCnt
|| peakCnt
> bestPeakCnt
){
1186 bestFirstPeakHigh
=firstPeakHigh
;
1187 bestErrCnt
= errCnt
;
1188 bestPeakCnt
= peakCnt
;
1189 bestPeakStart
= iii
;
1190 if (!errCnt
) break; //great read - finish
1195 //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart);
1196 if (bestErrCnt
> maxErr
) return bestErrCnt
;
1198 //best run is good enough set to best run and set overwrite BinStream
1199 lastBit
= bestPeakStart
- *clk
;
1200 memset(dest
, bestFirstPeakHigh
^1, bestPeakStart
/ *clk
);
1201 bitnum
+= (bestPeakStart
/ *clk
);
1202 for (i
= bestPeakStart
; i
< *size
; ++i
) {
1203 // if expecting a clock bit
1204 if ((i
>= lastBit
+ *clk
- tol
) && (i
<= lastBit
+ *clk
+ tol
)) {
1206 if (dest
[i
] >= high
|| dest
[i
] <= low
) {
1210 ignoreCnt
= ignoreWindow
;
1212 if (dest
[i
] >= high
) curBit
^= 1;
1213 dest
[bitnum
++] = curBit
;
1215 //else no bars found in clock area
1216 } else if (i
== lastBit
+ *clk
+ tol
) {
1217 dest
[bitnum
++] = curBit
;
1220 //else if no bars found
1221 } else if (dest
[i
] < high
&& dest
[i
] > low
){
1222 if (ignoreCnt
== 0){
1224 if (errBitHigh
== 1){
1225 dest
[bitnum
++] = 77;
1232 } else if ((dest
[i
] >= high
|| dest
[i
] <= low
) && (bitHigh
== 0)) {
1233 //error bar found no clock...
1236 if (bitnum
>= MaxBits
) break;
1243 //detects the bit clock for FSK given the high and low Field Clocks
1244 uint8_t detectFSKClk(uint8_t *BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
)
1246 uint8_t clk
[] = {8,16,32,40,50,64,100,128,0};
1247 uint16_t rfLens
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1248 uint8_t rfCnts
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1249 uint8_t rfLensFnd
= 0;
1250 uint8_t lastFCcnt
= 0;
1251 uint16_t fcCounter
= 0;
1252 uint16_t rfCounter
= 0;
1253 uint8_t firstBitFnd
= 0;
1255 if (size
== 0) return 0;
1257 uint8_t fcTol
= (uint8_t)(0.5+(float)(fcHigh
-fcLow
)/2);
1262 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1263 // prime i to first up transition
1264 for (i
= 1; i
< size
-1; i
++)
1265 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
]>=BitStream
[i
+1])
1268 for (; i
< size
-1; i
++){
1272 if (BitStream
[i
] <= BitStream
[i
-1] || BitStream
[i
] < BitStream
[i
+1])
1275 // if we got less than the small fc + tolerance then set it to the small fc
1276 if (fcCounter
< fcLow
+fcTol
)
1278 else //set it to the large fc
1281 //look for bit clock (rf/xx)
1282 if ((fcCounter
< lastFCcnt
|| fcCounter
> lastFCcnt
)){
1283 //not the same size as the last wave - start of new bit sequence
1284 if (firstBitFnd
> 1){ //skip first wave change - probably not a complete bit
1285 for (int ii
=0; ii
<15; ii
++){
1286 if (rfLens
[ii
] == rfCounter
){
1292 if (rfCounter
> 0 && rfLensFnd
< 15){
1293 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1294 rfCnts
[rfLensFnd
]++;
1295 rfLens
[rfLensFnd
++] = rfCounter
;
1301 lastFCcnt
=fcCounter
;
1305 uint8_t rfHighest
=15, rfHighest2
=15, rfHighest3
=15;
1307 for (i
=0; i
<15; i
++){
1308 //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1309 //get highest 2 RF values (might need to get more values to compare or compare all?)
1310 if (rfCnts
[i
]>rfCnts
[rfHighest
]){
1311 rfHighest3
=rfHighest2
;
1312 rfHighest2
=rfHighest
;
1314 } else if(rfCnts
[i
]>rfCnts
[rfHighest2
]){
1315 rfHighest3
=rfHighest2
;
1317 } else if(rfCnts
[i
]>rfCnts
[rfHighest3
]){
1321 // set allowed clock remainder tolerance to be 1 large field clock length+1
1322 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1323 uint8_t tol1
= fcHigh
+1;
1325 //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1327 // loop to find the highest clock that has a remainder less than the tolerance
1328 // compare samples counted divided by
1330 for (; ii
>=0; ii
--){
1331 if (rfLens
[rfHighest
] % clk
[ii
] < tol1
|| rfLens
[rfHighest
] % clk
[ii
] > clk
[ii
]-tol1
){
1332 if (rfLens
[rfHighest2
] % clk
[ii
] < tol1
|| rfLens
[rfHighest2
] % clk
[ii
] > clk
[ii
]-tol1
){
1333 if (rfLens
[rfHighest3
] % clk
[ii
] < tol1
|| rfLens
[rfHighest3
] % clk
[ii
] > clk
[ii
]-tol1
){
1340 if (ii
<0) return 0; // oops we went too far
1346 //countFC is to detect the field clock lengths.
1347 //counts and returns the 2 most common wave lengths
1348 //mainly used for FSK field clock detection
1349 uint16_t countFC(uint8_t *BitStream
, size_t size
, uint8_t fskAdj
)
1351 uint8_t fcLens
[] = {0,0,0,0,0,0,0,0,0,0};
1352 uint16_t fcCnts
[] = {0,0,0,0,0,0,0,0,0,0};
1353 uint8_t fcLensFnd
= 0;
1354 uint8_t lastFCcnt
=0;
1355 uint8_t fcCounter
= 0;
1357 if (size
== 0) return 0;
1359 // prime i to first up transition
1360 for (i
= 1; i
< size
-1; i
++)
1361 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1])
1364 for (; i
< size
-1; i
++){
1365 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1]){
1366 // new up transition
1369 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1370 if (lastFCcnt
==5 && fcCounter
==9) fcCounter
--;
1371 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
1372 if ((fcCounter
==9) || fcCounter
==4) fcCounter
++;
1373 // save last field clock count (fc/xx)
1374 lastFCcnt
= fcCounter
;
1376 // find which fcLens to save it to:
1377 for (int ii
=0; ii
<10; ii
++){
1378 if (fcLens
[ii
]==fcCounter
){
1384 if (fcCounter
>0 && fcLensFnd
<10){
1386 fcCnts
[fcLensFnd
]++;
1387 fcLens
[fcLensFnd
++]=fcCounter
;
1396 uint8_t best1
=9, best2
=9, best3
=9;
1398 // go through fclens and find which ones are bigest 2
1399 for (i
=0; i
<10; i
++){
1400 // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt);
1401 // get the 3 best FC values
1402 if (fcCnts
[i
]>maxCnt1
) {
1407 } else if(fcCnts
[i
]>fcCnts
[best2
]){
1410 } else if(fcCnts
[i
]>fcCnts
[best3
]){
1414 uint8_t fcH
=0, fcL
=0;
1415 if (fcLens
[best1
]>fcLens
[best2
]){
1423 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1425 uint16_t fcs
= (((uint16_t)fcH
)<<8) | fcL
;
1426 // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
1427 if (fskAdj
) return fcs
;
1428 return fcLens
[best1
];
1431 //by marshmellow - demodulate PSK1 wave
1432 //uses wave lengths (# Samples)
1433 int pskRawDemod(uint8_t dest
[], size_t *size
, int *clock
, int *invert
)
1435 if (size
== 0) return -1;
1436 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
1437 if (*size
<loopCnt
) loopCnt
= *size
;
1439 uint8_t curPhase
= *invert
;
1440 size_t i
, waveStart
=1, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
1441 uint8_t fc
=0, fullWaveLen
=0, tol
=1;
1442 uint16_t errCnt
=0, waveLenCnt
=0;
1443 fc
= countFC(dest
, *size
, 0);
1444 if (fc
!=2 && fc
!=4 && fc
!=8) return -1;
1445 //PrintAndLog("DEBUG: FC: %d",fc);
1446 *clock
= DetectPSKClock(dest
, *size
, *clock
);
1447 if (*clock
== 0) return -1;
1448 int avgWaveVal
=0, lastAvgWaveVal
=0;
1449 //find first phase shift
1450 for (i
=0; i
<loopCnt
; i
++){
1451 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1453 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1454 waveLenCnt
= waveEnd
-waveStart
;
1455 if (waveLenCnt
> fc
&& waveStart
> fc
){ //not first peak and is a large wave
1456 lastAvgWaveVal
= avgWaveVal
/(waveLenCnt
);
1457 firstFullWave
= waveStart
;
1458 fullWaveLen
=waveLenCnt
;
1459 //if average wave value is > graph 0 then it is an up wave or a 1
1460 if (lastAvgWaveVal
> 123) curPhase
^= 1; //fudge graph 0 a little 123 vs 128
1466 avgWaveVal
+= dest
[i
+2];
1468 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1469 lastClkBit
= firstFullWave
; //set start of wave as clock align
1470 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1474 memset(dest
, curPhase
^1, firstFullWave
/ *clock
);
1475 numBits
+= (firstFullWave
/ *clock
);
1476 dest
[numBits
++] = curPhase
; //set first read bit
1477 for (i
= firstFullWave
+ fullWaveLen
- 1; i
< *size
-3; i
++){
1478 //top edge of wave = start of new wave
1479 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1480 if (waveStart
== 0) {
1483 avgWaveVal
= dest
[i
+1];
1486 waveLenCnt
= waveEnd
-waveStart
;
1487 lastAvgWaveVal
= avgWaveVal
/waveLenCnt
;
1488 if (waveLenCnt
> fc
){
1489 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1490 //this wave is a phase shift
1491 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1492 if (i
+1 >= lastClkBit
+ *clock
- tol
){ //should be a clock bit
1494 dest
[numBits
++] = curPhase
;
1495 lastClkBit
+= *clock
;
1496 } else if (i
< lastClkBit
+10+fc
){
1497 //noise after a phase shift - ignore
1498 } else { //phase shift before supposed to based on clock
1500 dest
[numBits
++] = 77;
1502 } else if (i
+1 > lastClkBit
+ *clock
+ tol
+ fc
){
1503 lastClkBit
+= *clock
; //no phase shift but clock bit
1504 dest
[numBits
++] = curPhase
;
1510 avgWaveVal
+= dest
[i
+1];