]>
git.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
e6552e129fafb8c775da743f9482b7f609c22a96
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 values of a wave 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
= ((*high
-128)*fuzzHi
+ 12800)/100;
40 *low
= ((*low
-128)*fuzzLo
+ 12800)/100;
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 success=1 or fail=0 and 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
;
79 //takes 1s and 0s and searches for EM410x format - output EM ID
80 uint8_t Em410xDecode(uint8_t *BitStream
, size_t *size
, size_t *startIdx
, uint32_t *hi
, uint64_t *lo
)
82 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
83 // otherwise could be a void with no arguments
86 if (BitStream
[1]>1){ //allow only 1s and 0s
87 // PrintAndLog("no data found");
90 // 111111111 bit pattern represent start of frame
91 // include 0 in front to help get start pos
92 uint8_t preamble
[] = {0,1,1,1,1,1,1,1,1,1};
94 uint32_t parityBits
= 0;
98 errChk
= preambleSearch(BitStream
, preamble
, sizeof(preamble
), size
, startIdx
);
99 if (errChk
== 0 || *size
< 64) return 0;
100 if (*size
> 64) FmtLen
= 22;
101 *startIdx
+= 1; //get rid of 0 from preamble
103 for (i
=0; i
<FmtLen
; i
++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
104 parityBits
= bytebits_to_byte(BitStream
+(i
*5)+idx
,5);
106 if (parityTest(parityBits
, 5, 0) == 0){
110 //set uint64 with ID from BitStream
111 for (uint8_t ii
=0; ii
<4; ii
++){
112 *hi
= (*hi
<< 1) | (*lo
>> 63);
113 *lo
= (*lo
<< 1) | (BitStream
[(i
*5)+ii
+idx
]);
116 if (errChk
!= 0) return 1;
117 //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;
153 //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance
154 if (!maxErr
) gLen
=*clk
*2;
156 uint16_t MaxBits
= 500;
157 uint32_t bestStart
= *size
;
158 int bestErrCnt
= maxErr
+1;
159 // PrintAndLog("DEBUG - lastbit - %d",lastBit);
160 // loop to find first wave that works
161 for (iii
=0; iii
< gLen
; ++iii
){
162 if ((BinStream
[iii
] >= high
) || (BinStream
[iii
] <= low
)){
165 // loop through to see if this start location works
166 for (i
= iii
; i
< *size
; ++i
) {
167 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
169 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
170 //low found and we are expecting a bar
173 //mid value found or no bar supposed to be here
174 if ((i
-lastBit
)>(*clk
+tol
)){
175 //should have hit a high or low based on clock!!
178 //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);
181 lastBit
+=*clk
;//skip over until hit too many errors
182 if (errCnt
>(maxErr
)) break; //allow 1 error for every 1000 samples else start over
185 if ((i
-iii
) >(MaxBits
* *clk
)) break; //got plenty of bits
187 //we got more than 64 good bits and not all errors
188 if ((((i
-iii
)/ *clk
) > (64)) && (errCnt
<=maxErr
)) {
193 break; //great read - finish
195 if (errCnt
<bestErrCnt
){ //set this as new best run
202 if (bestErrCnt
<=maxErr
){
203 //best run is good enough set to best run and set overwrite BinStream
205 lastBit
= bestStart
- *clk
;
207 for (i
= iii
; i
< *size
; ++i
) {
208 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
210 BinStream
[bitnum
] = *invert
;
212 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
213 //low found and we are expecting a bar
215 BinStream
[bitnum
] = 1-*invert
;
218 //mid value found or no bar supposed to be here
219 if ((i
-lastBit
)>(*clk
+tol
)){
220 //should have hit a high or low based on clock!!
223 //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);
225 BinStream
[bitnum
]=77;
229 lastBit
+=*clk
;//skip over error
232 if (bitnum
>=MaxBits
) break;
244 //encode binary data into binary manchester
245 int ManchesterEncode(uint8_t *BitStream
, size_t size
)
247 size_t modIdx
=20000, i
=0;
248 if (size
>modIdx
) return -1;
249 for (size_t idx
=0; idx
< size
; idx
++){
250 BitStream
[idx
+modIdx
++] = BitStream
[idx
];
251 BitStream
[idx
+modIdx
++] = BitStream
[idx
]^1;
253 for (; i
<(size
*2); i
++){
254 BitStream
[i
] = BitStream
[i
+20000];
260 //take 10 and 01 and manchester decode
261 //run through 2 times and take least errCnt
262 int manrawdecode(uint8_t * BitStream
, size_t *size
)
264 uint16_t bitnum
=0, MaxBits
= 512, errCnt
= 0;
266 uint16_t bestErr
= 1000, bestRun
= 0;
267 if (size
== 0) return -1;
268 for (ii
=0;ii
<2;++ii
){
270 for (i
=i
+ii
;i
<*size
-2;i
+=2){
271 if(BitStream
[i
]==1 && (BitStream
[i
+1]==0)){
272 } else if((BitStream
[i
]==0)&& BitStream
[i
+1]==1){
276 if(bitnum
>MaxBits
) break;
288 for (i
=i
+ii
; i
< *size
-2; i
+=2){
289 if(BitStream
[i
] == 1 && (BitStream
[i
+1] == 0)){
290 BitStream
[bitnum
++]=0;
291 } else if((BitStream
[i
] == 0) && BitStream
[i
+1] == 1){
292 BitStream
[bitnum
++]=1;
294 BitStream
[bitnum
++]=77;
297 if(bitnum
>MaxBits
) break;
305 //take 01 or 10 = 1 and 11 or 00 = 0
306 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
307 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
308 int BiphaseRawDecode(uint8_t *BitStream
, size_t *size
, int offset
, int invert
)
313 uint16_t MaxBits
=512;
314 //if not enough samples - error
315 if (*size
< 51) return -1;
316 //check for phase change faults - skip one sample if faulty
317 uint8_t offsetA
= 1, offsetB
= 1;
319 if (BitStream
[i
+1]==BitStream
[i
+2]) offsetA
=0;
320 if (BitStream
[i
+2]==BitStream
[i
+3]) offsetB
=0;
322 if (!offsetA
&& offsetB
) offset
++;
323 for (i
=offset
; i
<*size
-3; i
+=2){
324 //check for phase error
325 if (BitStream
[i
+1]==BitStream
[i
+2]) {
326 BitStream
[bitnum
++]=77;
329 if((BitStream
[i
]==1 && BitStream
[i
+1]==0) || (BitStream
[i
]==0 && BitStream
[i
+1]==1)){
330 BitStream
[bitnum
++]=1^invert
;
331 } else if((BitStream
[i
]==0 && BitStream
[i
+1]==0) || (BitStream
[i
]==1 && BitStream
[i
+1]==1)){
332 BitStream
[bitnum
++]=invert
;
334 BitStream
[bitnum
++]=77;
337 if(bitnum
>MaxBits
) break;
344 void askAmp(uint8_t *BitStream
, size_t size
)
348 for(int i
= 1; i
<size
; i
++){
349 if (BitStream
[i
]-BitStream
[i
-1]>=30) //large jump up
351 else if(BitStream
[i
]-BitStream
[i
-1]<=-20) //large jump down
354 shiftedVal
=BitStream
[i
]+shift
;
358 else if (shiftedVal
<0)
360 BitStream
[i
-1] = shiftedVal
;
365 int cleanAskRawDemod(uint8_t *BinStream
, size_t *size
, int clk
, int invert
, int high
, int low
)
367 size_t bitCnt
=0, smplCnt
=0, errCnt
=0;
368 uint8_t waveHigh
= 0;
369 //PrintAndLog("clk: %d", clk);
370 for (size_t i
=0; i
< *size
; i
++){
371 if (BinStream
[i
] >= high
&& waveHigh
){
373 } else if (BinStream
[i
] <= low
&& !waveHigh
){
375 } else { //transition
376 if ((BinStream
[i
] >= high
&& !waveHigh
) || (BinStream
[i
] <= low
&& waveHigh
)){
377 if (smplCnt
> clk
-(clk
/4)-1) { //full clock
378 if (smplCnt
> clk
+ (clk
/4)+1) { //too many samples
380 BinStream
[bitCnt
++]=77;
381 } else if (waveHigh
) {
382 BinStream
[bitCnt
++] = invert
;
383 BinStream
[bitCnt
++] = invert
;
384 } else if (!waveHigh
) {
385 BinStream
[bitCnt
++] = invert
^ 1;
386 BinStream
[bitCnt
++] = invert
^ 1;
390 } else if (smplCnt
> (clk
/2) - (clk
/4)-1) {
392 BinStream
[bitCnt
++] = invert
;
393 } else if (!waveHigh
) {
394 BinStream
[bitCnt
++] = invert
^ 1;
398 } else if (!bitCnt
) {
400 waveHigh
= (BinStream
[i
] >= high
);
404 //transition bit oops
406 } else { //haven't hit new high or new low yet
416 //takes 3 arguments - clock, invert and maxErr as integers
417 //attempts to demodulate ask only
418 int askrawdemod(uint8_t *BinStream
, size_t *size
, int *clk
, int *invert
, int maxErr
, uint8_t amp
)
421 if (*size
==0) return -1;
422 int start
= DetectASKClock(BinStream
, *size
, clk
, 20); //clock default
423 if (*clk
==0) return -1;
424 if (start
<0) return -1;
425 if (*invert
!= 0 && *invert
!= 1) *invert
=0;
426 if (amp
==1) askAmp(BinStream
, *size
);
428 uint32_t initLoopMax
= 200;
429 if (initLoopMax
> *size
) initLoopMax
=*size
;
430 // Detect high and lows
431 //25% clip in case highs and lows aren't clipped [marshmellow]
434 ans
= getHiLo(BinStream
, initLoopMax
, &high
, &low
, clip
, clip
);
435 if (ans
<1) return -1; //just noise
437 if (DetectCleanAskWave(BinStream
, *size
, high
, low
)) {
438 //PrintAndLog("Clean");
439 return cleanAskRawDemod(BinStream
, size
, *clk
, *invert
, high
, low
);
442 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
443 int lastBit
= 0; //set first clock check
444 uint32_t bitnum
= 0; //output counter
445 uint8_t tol
= 0; //clock tolerance adjust - waves will be accepted as within the clock
446 // if they fall + or - this value + clock from last valid wave
447 if (*clk
== 32) tol
=0; //clock tolerance may not be needed anymore currently set to
448 // + or - 1 but could be increased for poor waves or removed entirely
450 uint32_t gLen
= *size
;
451 if (gLen
> 500) gLen
=500;
452 //if 0 errors allowed then only try first 2 clock cycles as we want a low tolerance
453 if (!maxErr
) gLen
= *clk
* 2;
455 uint32_t bestStart
= *size
;
456 uint32_t bestErrCnt
= maxErr
; //(*size/1000);
458 uint16_t MaxBits
=1000;
460 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
461 //loop to find first wave that works
462 for (iii
=start
; iii
< gLen
; ++iii
){
463 if ((BinStream
[iii
]>=high
) || (BinStream
[iii
]<=low
)){
466 //loop through to see if this start location works
467 for (i
= iii
; i
< *size
; ++i
) {
468 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
471 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
472 //low found and we are expecting a bar
475 } else if ((BinStream
[i
]<=low
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
478 } else if ((BinStream
[i
]>=high
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
481 } else if ((i
-lastBit
)>((*clk
/2)+tol
) && (midBit
==0)){
485 //mid value found or no bar supposed to be here
487 if ((i
-lastBit
)>(*clk
+tol
)){
488 //should have hit a high or low based on clock!!
490 //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);
493 lastBit
+=*clk
;//skip over until hit too many errors
494 if (errCnt
> maxErr
){
500 if ((i
-iii
)>(MaxBits
* *clk
)) break; //got enough bits
502 //we got more than 64 good bits and not all errors
503 if ((((i
-iii
)/ *clk
) > (64)) && (errCnt
<=maxErr
)) {
508 break; //great read - finish
510 if (errCnt
<bestErrCnt
){ //set this as new best run
517 if (bestErrCnt
<=maxErr
){
518 //best run is good enough - set to best run and overwrite BinStream
520 lastBit
= bestStart
- *clk
;
522 for (i
= iii
; i
< *size
; ++i
) {
523 if ((BinStream
[i
] >= high
) && ((i
-lastBit
) > (*clk
-tol
))){
525 BinStream
[bitnum
] = *invert
;
528 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
) > (*clk
-tol
))){
529 //low found and we are expecting a bar
531 BinStream
[bitnum
] = 1 - *invert
;
534 } else if ((BinStream
[i
]<=low
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
537 BinStream
[bitnum
] = 1 - *invert
;
539 } else if ((BinStream
[i
]>=high
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
542 BinStream
[bitnum
] = *invert
;
544 } else if ((i
-lastBit
)>((*clk
/2)+tol
) && (midBit
==0)){
547 if (bitnum
!=0) BinStream
[bitnum
] = BinStream
[bitnum
-1];
551 //mid value found or no bar supposed to be here
552 if ((i
-lastBit
)>(*clk
+tol
)){
553 //should have hit a high or low based on clock!!
556 //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);
558 BinStream
[bitnum
]=77;
561 lastBit
+=*clk
;//skip over error
564 if (bitnum
>= MaxBits
) break;
575 // demod gProxIIDemod
576 // error returns as -x
577 // success returns start position in BitStream
578 // BitStream must contain previously askrawdemod and biphasedemoded data
579 int gProxII_Demod(uint8_t BitStream
[], size_t *size
)
582 uint8_t preamble
[] = {1,1,1,1,1,0};
584 uint8_t errChk
= preambleSearch(BitStream
, preamble
, sizeof(preamble
), size
, &startIdx
);
585 if (errChk
== 0) return -3; //preamble not found
586 if (*size
!= 96) return -2; //should have found 96 bits
587 //check first 6 spacer bits to verify format
588 if (!BitStream
[startIdx
+5] && !BitStream
[startIdx
+10] && !BitStream
[startIdx
+15] && !BitStream
[startIdx
+20] && !BitStream
[startIdx
+25] && !BitStream
[startIdx
+30]){
589 //confirmed proper separator bits found
590 //return start position
591 return (int) startIdx
;
596 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
597 size_t fsk_wave_demod(uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
599 uint32_t last_transition
= 0;
602 if (fchigh
==0) fchigh
=10;
603 if (fclow
==0) fclow
=8;
604 //set the threshold close to 0 (graph) or 128 std to avoid static
605 uint8_t threshold_value
= 123;
607 // sync to first lo-hi transition, and threshold
609 // Need to threshold first sample
611 if(dest
[0] < threshold_value
) dest
[0] = 0;
615 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
616 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
617 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
618 for(idx
= 1; idx
< size
; idx
++) {
619 // threshold current value
621 if (dest
[idx
] < threshold_value
) dest
[idx
] = 0;
624 // Check for 0->1 transition
625 if (dest
[idx
-1] < dest
[idx
]) { // 0 -> 1 transition
626 if ((idx
-last_transition
)<(fclow
-2)){ //0-5 = garbage noise
627 //do nothing with extra garbage
628 } else if ((idx
-last_transition
) < (fchigh
-1)) { //6-8 = 8 waves
630 } else if ((idx
-last_transition
) > (fchigh
+1) && !numBits
) { //12 + and first bit = garbage
631 //do nothing with beginning garbage
632 } else { //9+ = 10 waves
635 last_transition
= idx
;
639 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
642 //translate 11111100000 to 10
643 size_t aggregate_bits(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t maxConsequtiveBits
,
644 uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
646 uint8_t lastval
=dest
[0];
650 for( idx
=1; idx
< size
; idx
++) {
652 if (dest
[idx
]==lastval
) {
657 //if lastval was 1, we have a 1->0 crossing
658 if (dest
[idx
-1]==1) {
659 if (!numBits
&& n
< rfLen
/fclow
) {
664 n
= (n
* fclow
+ rfLen
/2) / rfLen
;
665 } else {// 0->1 crossing
666 //test first bitsample too small
667 if (!numBits
&& n
< rfLen
/fchigh
) {
672 n
= (n
* fchigh
+ rfLen
/2) / rfLen
; //-1 for fudge factor
676 if(n
< maxConsequtiveBits
) //Consecutive
678 if(invert
==0){ //invert bits
679 memset(dest
+numBits
, dest
[idx
-1] , n
);
681 memset(dest
+numBits
, dest
[idx
-1]^1 , n
);
689 // if valid extra bits at the end were all the same frequency - add them in
690 if (n
> rfLen
/fclow
&& n
> rfLen
/fchigh
) {
691 if (dest
[idx
-2]==1) {
692 n
= ((n
+1) * fclow
+ rfLen
/2) / rfLen
;
693 } else {// 0->1 crossing
694 n
= ((n
+1) * fchigh
+ (rfLen
-1)/2) / (rfLen
-1); //-1 for fudge factor
696 memset(dest
, dest
[idx
-1]^invert
, n
);
701 //by marshmellow (from holiman's base)
702 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
703 int fskdemod(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
706 size
= fsk_wave_demod(dest
, size
, fchigh
, fclow
);
707 size
= aggregate_bits(dest
, size
, rfLen
, 192, invert
, fchigh
, fclow
);
711 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
712 int HIDdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
714 if (justNoise(dest
, *size
)) return -1;
716 size_t numStart
=0, size2
=*size
, startIdx
=0;
718 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
719 if (*size
< 96) return -2;
720 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
721 uint8_t preamble
[] = {0,0,0,1,1,1,0,1};
722 // find bitstring in array
723 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
724 if (errChk
== 0) return -3; //preamble not found
726 numStart
= startIdx
+ sizeof(preamble
);
727 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
728 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
729 if (dest
[idx
] == dest
[idx
+1]){
730 return -4; //not manchester data
732 *hi2
= (*hi2
<<1)|(*hi
>>31);
733 *hi
= (*hi
<<1)|(*lo
>>31);
734 //Then, shift in a 0 or one into low
735 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
740 return (int)startIdx
;
743 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
744 int ParadoxdemodFSK(uint8_t *dest
, size_t *size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
746 if (justNoise(dest
, *size
)) return -1;
748 size_t numStart
=0, size2
=*size
, startIdx
=0;
750 *size
= fskdemod(dest
, size2
,50,1,10,8); //fsk2a
751 if (*size
< 96) return -2;
753 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
754 uint8_t preamble
[] = {0,0,0,0,1,1,1,1};
756 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
757 if (errChk
== 0) return -3; //preamble not found
759 numStart
= startIdx
+ sizeof(preamble
);
760 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
761 for (size_t idx
= numStart
; (idx
-numStart
) < *size
- sizeof(preamble
); idx
+=2){
762 if (dest
[idx
] == dest
[idx
+1])
763 return -4; //not manchester data
764 *hi2
= (*hi2
<<1)|(*hi
>>31);
765 *hi
= (*hi
<<1)|(*lo
>>31);
766 //Then, shift in a 0 or one into low
767 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
772 return (int)startIdx
;
775 uint32_t bytebits_to_byte(uint8_t* src
, size_t numbits
)
778 for(int i
= 0 ; i
< numbits
; i
++)
780 num
= (num
<< 1) | (*src
);
786 int IOdemodFSK(uint8_t *dest
, size_t size
)
788 if (justNoise(dest
, size
)) return -1;
789 //make sure buffer has data
790 if (size
< 66*64) return -2;
792 size
= fskdemod(dest
, size
, 64, 1, 10, 8); // FSK2a RF/64
793 if (size
< 65) return -3; //did we get a good demod?
795 //0 10 20 30 40 50 60
797 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
798 //-----------------------------------------------------------------------------
799 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
801 //XSF(version)facility:codeone+codetwo
804 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,1};
805 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), &size
, &startIdx
);
806 if (errChk
== 0) return -4; //preamble not found
808 if (!dest
[startIdx
+8] && dest
[startIdx
+17]==1 && dest
[startIdx
+26]==1 && dest
[startIdx
+35]==1 && dest
[startIdx
+44]==1 && dest
[startIdx
+53]==1){
809 //confirmed proper separator bits found
810 //return start position
811 return (int) startIdx
;
817 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
818 // Parity Type (1 for odd 0 for even), and binary Length (length to run)
819 size_t removeParity(uint8_t *BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
)
821 uint32_t parityWd
= 0;
822 size_t j
= 0, bitCnt
= 0;
823 for (int word
= 0; word
< (bLen
); word
+=pLen
){
824 for (int bit
=0; bit
< pLen
; bit
++){
825 parityWd
= (parityWd
<< 1) | BitStream
[startIdx
+word
+bit
];
826 BitStream
[j
++] = (BitStream
[startIdx
+word
+bit
]);
829 // if parity fails then return 0
830 if (parityTest(parityWd
, pLen
, pType
) == 0) return -1;
834 // if we got here then all the parities passed
835 //return ID start index and size
840 // FSK Demod then try to locate an AWID ID
841 int AWIDdemodFSK(uint8_t *dest
, size_t *size
)
843 //make sure buffer has enough data
844 if (*size
< 96*50) return -1;
846 if (justNoise(dest
, *size
)) return -2;
849 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
850 if (*size
< 96) return -3; //did we get a good demod?
852 uint8_t preamble
[] = {0,0,0,0,0,0,0,1};
854 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
855 if (errChk
== 0) return -4; //preamble not found
856 if (*size
!= 96) return -5;
857 return (int)startIdx
;
861 // FSK Demod then try to locate an Farpointe Data (pyramid) ID
862 int PyramiddemodFSK(uint8_t *dest
, size_t *size
)
864 //make sure buffer has data
865 if (*size
< 128*50) return -5;
867 //test samples are not just noise
868 if (justNoise(dest
, *size
)) return -1;
871 *size
= fskdemod(dest
, *size
, 50, 1, 10, 8); // fsk2a RF/50
872 if (*size
< 128) return -2; //did we get a good demod?
874 uint8_t preamble
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
876 uint8_t errChk
= preambleSearch(dest
, preamble
, sizeof(preamble
), size
, &startIdx
);
877 if (errChk
== 0) return -4; //preamble not found
878 if (*size
!= 128) return -3;
879 return (int)startIdx
;
883 uint8_t DetectCleanAskWave(uint8_t dest
[], size_t size
, int high
, int low
)
887 size_t loopEnd
= 572;
888 if (loopEnd
> size
) loopEnd
= size
;
889 for (size_t i
=60; i
<loopEnd
; i
++){
890 if (dest
[i
]>low
&& dest
[i
]<high
)
896 if (cntPeaks
> 300) return 1;
901 int DetectStrongAskClock(uint8_t dest
[], size_t size
)
903 int clk
[]={0,8,16,32,40,50,64,100,128,256};
909 for (;idx
< size
; idx
++){
914 if (highCnt
!= 0) highCnt2
= highCnt
;
916 } else if (cnt
> highCnt2
) {
923 } else if (dest
[idx
] <= 128){
927 if (highCnt
!= 0) highCnt2
= highCnt
;
929 } else if (cnt
> highCnt2
) {
939 for (idx
=8; idx
>0; idx
--){
941 if (clk
[idx
] >= highCnt
- tol
&& clk
[idx
] <= highCnt
+ tol
)
943 if (clk
[idx
] >= highCnt2
- tol
&& clk
[idx
] <= highCnt2
+ tol
)
950 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
951 // maybe somehow adjust peak trimming value based on samples to fix?
952 // return start index of best starting position for that clock and return clock (by reference)
953 int DetectASKClock(uint8_t dest
[], size_t size
, int *clock
, int maxErr
)
956 int clk
[]={8,16,32,40,50,64,100,128,256};
957 int loopCnt
= 256; //don't need to loop through entire array...
958 if (size
== 0) return -1;
959 if (size
<loopCnt
) loopCnt
= size
;
960 //if we already have a valid clock quit
963 if (clk
[i
] == *clock
) return 0;
965 //get high and low peak
967 getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75);
969 //test for large clean peaks
970 if (DetectCleanAskWave(dest
, size
, peak
, low
)==1){
971 int ans
= DetectStrongAskClock(dest
, size
);
982 int bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
983 int bestStart
[]={0,0,0,0,0,0,0,0,0};
985 //test each valid clock from smallest to greatest to see which lines up
986 for(clkCnt
=0; clkCnt
< 8; clkCnt
++){
987 if (clk
[clkCnt
] == 32){
992 if (!maxErr
) loopCnt
=clk
[clkCnt
]*2;
993 bestErr
[clkCnt
]=1000;
994 //try lining up the peaks by moving starting point (try first 256)
995 for (ii
=0; ii
< loopCnt
; ii
++){
996 if ((dest
[ii
] >= peak
) || (dest
[ii
] <= low
)){
998 // now that we have the first one lined up test rest of wave array
999 for (i
=0; i
<((int)((size
-ii
-tol
)/clk
[clkCnt
])-1); ++i
){
1000 if (dest
[ii
+(i
*clk
[clkCnt
])]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])]<=low
){
1001 }else if(dest
[ii
+(i
*clk
[clkCnt
])-tol
]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])-tol
]<=low
){
1002 }else if(dest
[ii
+(i
*clk
[clkCnt
])+tol
]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])+tol
]<=low
){
1003 }else{ //error no peak detected
1007 //if we found no errors then we can stop here
1008 // this is correct one - return this clock
1009 //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
1010 if(errCnt
==0 && clkCnt
<6) {
1011 *clock
= clk
[clkCnt
];
1014 //if we found errors see if it is lowest so far and save it as best run
1015 if(errCnt
<bestErr
[clkCnt
]){
1016 bestErr
[clkCnt
]=errCnt
;
1017 bestStart
[clkCnt
]=ii
;
1024 for (iii
=0; iii
<8; ++iii
){
1025 if (bestErr
[iii
]<bestErr
[best
]){
1026 if (bestErr
[iii
]==0) bestErr
[iii
]=1;
1027 // current best bit to error ratio vs new bit to error ratio
1028 if (((size
/clk
[best
])/bestErr
[best
] < (size
/clk
[iii
])/bestErr
[iii
]) ){
1033 if (bestErr
[best
]>maxErr
) return -1;
1035 return bestStart
[best
];
1039 //detect psk clock by reading each phase shift
1040 // a phase shift is determined by measuring the sample length of each wave
1041 int DetectPSKClock(uint8_t dest
[], size_t size
, int clock
)
1043 uint8_t clk
[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
1044 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
1045 if (size
== 0) return 0;
1046 if (size
<loopCnt
) loopCnt
= size
;
1048 //if we already have a valid clock quit
1051 if (clk
[i
] == clock
) return clock
;
1053 size_t waveStart
=0, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
1054 uint8_t clkCnt
, fc
=0, fullWaveLen
=0, tol
=1;
1055 uint16_t peakcnt
=0, errCnt
=0, waveLenCnt
=0;
1056 uint16_t bestErr
[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
1057 uint16_t peaksdet
[]={0,0,0,0,0,0,0,0,0};
1058 countFC(dest
, size
, &fc
);
1059 //PrintAndLog("DEBUG: FC: %d",fc);
1061 //find first full wave
1062 for (i
=0; i
<loopCnt
; i
++){
1063 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1064 if (waveStart
== 0) {
1066 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
1069 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1070 waveLenCnt
= waveEnd
-waveStart
;
1071 if (waveLenCnt
> fc
){
1072 firstFullWave
= waveStart
;
1073 fullWaveLen
=waveLenCnt
;
1080 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1082 //test each valid clock from greatest to smallest to see which lines up
1083 for(clkCnt
=7; clkCnt
>= 1 ; clkCnt
--){
1084 lastClkBit
= firstFullWave
; //set end of wave as clock align
1088 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
1090 for (i
= firstFullWave
+fullWaveLen
-1; i
< loopCnt
-2; i
++){
1091 //top edge of wave = start of new wave
1092 if (dest
[i
] < dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1093 if (waveStart
== 0) {
1098 waveLenCnt
= waveEnd
-waveStart
;
1099 if (waveLenCnt
> fc
){
1100 //if this wave is a phase shift
1101 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc);
1102 if (i
+1 >= lastClkBit
+ clk
[clkCnt
] - tol
){ //should be a clock bit
1104 lastClkBit
+=clk
[clkCnt
];
1105 } else if (i
<lastClkBit
+8){
1106 //noise after a phase shift - ignore
1107 } else { //phase shift before supposed to based on clock
1110 } else if (i
+1 > lastClkBit
+ clk
[clkCnt
] + tol
+ fc
){
1111 lastClkBit
+=clk
[clkCnt
]; //no phase shift but clock bit
1120 if (errCnt
<= bestErr
[clkCnt
]) bestErr
[clkCnt
]=errCnt
;
1121 if (peakcnt
> peaksdet
[clkCnt
]) peaksdet
[clkCnt
]=peakcnt
;
1123 //all tested with errors
1124 //return the highest clk with the most peaks found
1126 for (i
=7; i
>=1; i
--){
1127 if (peaksdet
[i
] > peaksdet
[best
]) {
1130 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
1136 //detect nrz clock by reading #peaks vs no peaks(or errors)
1137 int DetectNRZClock(uint8_t dest
[], size_t size
, int clock
)
1140 int clk
[]={8,16,32,40,50,64,100,128,256};
1141 int loopCnt
= 4096; //don't need to loop through entire array...
1142 if (size
== 0) return 0;
1143 if (size
<loopCnt
) loopCnt
= size
;
1145 //if we already have a valid clock quit
1147 if (clk
[i
] == clock
) return clock
;
1149 //get high and low peak
1151 getHiLo(dest
, loopCnt
, &peak
, &low
, 75, 75);
1153 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
1158 int peaksdet
[]={0,0,0,0,0,0,0,0};
1160 //test for large clipped waves
1161 for (i
=0; i
<loopCnt
; i
++){
1162 if (dest
[i
] >= peak
|| dest
[i
] <= low
){
1165 if (peakcnt
>0 && maxPeak
< peakcnt
){
1172 //test each valid clock from smallest to greatest to see which lines up
1173 for(clkCnt
=0; clkCnt
< 8; ++clkCnt
){
1174 //ignore clocks smaller than largest peak
1175 if (clk
[clkCnt
]<maxPeak
) continue;
1177 //try lining up the peaks by moving starting point (try first 256)
1178 for (ii
=0; ii
< loopCnt
; ++ii
){
1179 if ((dest
[ii
] >= peak
) || (dest
[ii
] <= low
)){
1181 // now that we have the first one lined up test rest of wave array
1182 for (i
=0; i
< ((int)((size
-ii
-tol
)/clk
[clkCnt
])-1); ++i
){
1183 if (dest
[ii
+(i
*clk
[clkCnt
])]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])]<=low
){
1187 if(peakcnt
>peaksdet
[clkCnt
]) {
1188 peaksdet
[clkCnt
]=peakcnt
;
1195 for (iii
=7; iii
> 0; iii
--){
1196 if (peaksdet
[iii
] > peaksdet
[best
]){
1199 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
1205 // convert psk1 demod to psk2 demod
1206 // only transition waves are 1s
1207 void psk1TOpsk2(uint8_t *BitStream
, size_t size
)
1210 uint8_t lastBit
=BitStream
[0];
1211 for (; i
<size
; i
++){
1212 if (BitStream
[i
]==77){
1214 } else if (lastBit
!=BitStream
[i
]){
1215 lastBit
=BitStream
[i
];
1225 // convert psk2 demod to psk1 demod
1226 // from only transition waves are 1s to phase shifts change bit
1227 void psk2TOpsk1(uint8_t *BitStream
, size_t size
)
1230 for (size_t i
=0; i
<size
; i
++){
1231 if (BitStream
[i
]==1){
1239 // redesigned by marshmellow adjusted from existing decode functions
1240 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1241 int indala26decode(uint8_t *bitStream
, size_t *size
, uint8_t *invert
)
1243 //26 bit 40134 format (don't know other formats)
1245 int long_wait
=29;//29 leading zeros in format
1251 // Finding the start of a UID
1252 for (start
= 0; start
<= *size
- 250; start
++) {
1253 first
= bitStream
[start
];
1254 for (i
= start
; i
< start
+ long_wait
; i
++) {
1255 if (bitStream
[i
] != first
) {
1259 if (i
== (start
+ long_wait
)) {
1263 if (start
== *size
- 250 + 1) {
1264 // did not find start sequence
1267 // Inverting signal if needed
1269 for (i
= start
; i
< *size
; i
++) {
1270 bitStream
[i
] = !bitStream
[i
];
1276 //found start once now test length by finding next one
1277 for (ii
=start
+29; ii
<= *size
- 250; ii
++) {
1278 first2
= bitStream
[ii
];
1279 for (iii
= ii
; iii
< ii
+ long_wait
; iii
++) {
1280 if (bitStream
[iii
] != first2
) {
1284 if (iii
== (ii
+ long_wait
)) {
1288 if (ii
== *size
- 250 + 1){
1289 // did not find second start sequence
1296 for (ii
= 0; ii
< bitCnt
; ii
++) {
1297 bitStream
[ii
] = bitStream
[i
++];
1303 // by marshmellow - demodulate NRZ wave (both similar enough)
1304 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1305 // there probably is a much simpler way to do this....
1306 int nrzRawDemod(uint8_t *dest
, size_t *size
, int *clk
, int *invert
, int maxErr
)
1308 if (justNoise(dest
, *size
)) return -1;
1309 *clk
= DetectNRZClock(dest
, *size
, *clk
);
1310 if (*clk
==0) return -2;
1312 uint32_t gLen
= 4096;
1313 if (gLen
>*size
) gLen
= *size
;
1315 if (getHiLo(dest
, gLen
, &high
, &low
, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low
1316 int lastBit
= 0; //set first clock check
1317 uint32_t bitnum
= 0; //output counter
1318 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
1321 uint16_t MaxBits
= 1000;
1322 uint32_t bestErrCnt
= maxErr
+1;
1323 uint32_t bestPeakCnt
= 0;
1324 uint32_t bestPeakStart
=0;
1325 uint8_t bestFirstPeakHigh
=0;
1326 uint8_t firstPeakHigh
=0;
1329 uint8_t errBitHigh
=0;
1331 uint8_t ignoreWindow
=4;
1332 uint8_t ignoreCnt
=ignoreWindow
; //in case of noice near peak
1333 //loop to find first wave that works - align to clock
1334 for (iii
=0; iii
< gLen
; ++iii
){
1335 if ((dest
[iii
]>=high
) || (dest
[iii
]<=low
)){
1336 if (dest
[iii
]>=high
) firstPeakHigh
=1;
1337 else firstPeakHigh
=0;
1342 //loop through to see if this start location works
1343 for (i
= iii
; i
< *size
; ++i
) {
1344 //if we found a high bar and we are at a clock bit
1345 if ((dest
[i
]>=high
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1351 ignoreCnt
=ignoreWindow
;
1352 //else if low bar found and we are at a clock point
1353 }else if ((dest
[i
]<=low
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1359 ignoreCnt
=ignoreWindow
;
1360 //else if no bars found
1361 }else if(dest
[i
] < high
&& dest
[i
] > low
) {
1371 //if we are past a clock point
1372 if (i
>= lastBit
+*clk
+tol
){ //clock val
1376 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1377 }else if ((dest
[i
]>=high
|| dest
[i
]<=low
) && (i
<lastBit
+*clk
-tol
|| i
>lastBit
+*clk
+tol
) && (bitHigh
==0)){
1378 //error bar found no clock...
1381 if (bitnum
>=MaxBits
) break;
1383 //we got more than 64 good bits and not all errors
1384 if (bitnum
> (64) && (errCnt
<= (maxErr
))) {
1385 //possible good read
1388 bestFirstPeakHigh
=firstPeakHigh
;
1389 bestErrCnt
= errCnt
;
1390 bestPeakCnt
= peakCnt
;
1391 bestPeakStart
= iii
;
1392 break; //great read - finish
1394 if (errCnt
< bestErrCnt
){ //set this as new best run
1395 bestErrCnt
= errCnt
;
1398 if (peakCnt
> bestPeakCnt
){
1399 bestFirstPeakHigh
=firstPeakHigh
;
1400 bestPeakCnt
=peakCnt
;
1406 //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart);
1407 if (bestErrCnt
<= maxErr
){
1408 //best run is good enough set to best run and set overwrite BinStream
1410 lastBit
=bestPeakStart
-*clk
;
1412 memset(dest
, bestFirstPeakHigh
^1, bestPeakStart
/ *clk
);
1413 bitnum
+= (bestPeakStart
/ *clk
);
1414 for (i
= iii
; i
< *size
; ++i
) {
1415 //if we found a high bar and we are at a clock bit
1416 if ((dest
[i
] >= high
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1420 dest
[bitnum
]=curBit
;
1423 ignoreCnt
=ignoreWindow
;
1424 //else if low bar found and we are at a clock point
1425 }else if ((dest
[i
]<=low
) && (i
>=lastBit
+*clk
-tol
&& i
<=lastBit
+*clk
+tol
)){
1429 dest
[bitnum
]=curBit
;
1432 ignoreCnt
=ignoreWindow
;
1433 //else if no bars found
1434 }else if(dest
[i
]<high
&& dest
[i
]>low
) {
1437 //if peak is done was it an error peak?
1447 //if we are past a clock point
1448 if (i
>=lastBit
+*clk
+tol
){ //clock val
1450 dest
[bitnum
]=curBit
;
1453 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1454 }else if ((dest
[i
]>=high
|| dest
[i
]<=low
) && ((i
<lastBit
+*clk
-tol
) || (i
>lastBit
+*clk
+tol
)) && (bitHigh
==0)){
1455 //error bar found no clock...
1458 if (bitnum
>= MaxBits
) break;
1473 //detects the bit clock for FSK given the high and low Field Clocks
1474 uint8_t detectFSKClk(uint8_t *BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
)
1476 uint8_t clk
[] = {8,16,32,40,50,64,100,128,0};
1477 uint16_t rfLens
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1478 uint8_t rfCnts
[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1479 uint8_t rfLensFnd
= 0;
1480 uint8_t lastFCcnt
=0;
1481 uint32_t fcCounter
= 0;
1482 uint16_t rfCounter
= 0;
1483 uint8_t firstBitFnd
= 0;
1485 if (size
== 0) return 0;
1487 uint8_t fcTol
= (uint8_t)(0.5+(float)(fcHigh
-fcLow
)/2);
1492 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1493 // prime i to first up transition
1494 for (i
= 1; i
< size
-1; i
++)
1495 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
]>=BitStream
[i
+1])
1498 for (; i
< size
-1; i
++){
1499 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
]>=BitStream
[i
+1]){
1503 // if we got less than the small fc + tolerance then set it to the small fc
1504 if (fcCounter
< fcLow
+fcTol
)
1506 else //set it to the large fc
1509 //look for bit clock (rf/xx)
1510 if ((fcCounter
<lastFCcnt
|| fcCounter
>lastFCcnt
)){
1511 //not the same size as the last wave - start of new bit sequence
1513 if (firstBitFnd
>1){ //skip first wave change - probably not a complete bit
1514 for (int ii
=0; ii
<15; ii
++){
1515 if (rfLens
[ii
]==rfCounter
){
1521 if (rfCounter
>0 && rfLensFnd
<15){
1522 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1523 rfCnts
[rfLensFnd
]++;
1524 rfLens
[rfLensFnd
++]=rfCounter
;
1530 lastFCcnt
=fcCounter
;
1539 uint8_t rfHighest
=15, rfHighest2
=15, rfHighest3
=15;
1541 for (i
=0; i
<15; i
++){
1542 //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1543 //get highest 2 RF values (might need to get more values to compare or compare all?)
1544 if (rfCnts
[i
]>rfCnts
[rfHighest
]){
1545 rfHighest3
=rfHighest2
;
1546 rfHighest2
=rfHighest
;
1548 } else if(rfCnts
[i
]>rfCnts
[rfHighest2
]){
1549 rfHighest3
=rfHighest2
;
1551 } else if(rfCnts
[i
]>rfCnts
[rfHighest3
]){
1555 // set allowed clock remainder tolerance to be 1 large field clock length+1
1556 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1557 uint8_t tol1
= fcHigh
+1;
1559 //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1561 // loop to find the highest clock that has a remainder less than the tolerance
1562 // compare samples counted divided by
1564 for (; ii
>=0; ii
--){
1565 if (rfLens
[rfHighest
] % clk
[ii
] < tol1
|| rfLens
[rfHighest
] % clk
[ii
] > clk
[ii
]-tol1
){
1566 if (rfLens
[rfHighest2
] % clk
[ii
] < tol1
|| rfLens
[rfHighest2
] % clk
[ii
] > clk
[ii
]-tol1
){
1567 if (rfLens
[rfHighest3
] % clk
[ii
] < tol1
|| rfLens
[rfHighest3
] % clk
[ii
] > clk
[ii
]-tol1
){
1574 if (ii
<0) return 0; // oops we went too far
1580 //countFC is to detect the field clock lengths.
1581 //counts and returns the 2 most common wave lengths
1582 //mainly used for FSK field clock detection
1583 uint16_t countFC(uint8_t *BitStream
, size_t size
, uint8_t *mostFC
)
1585 uint8_t fcLens
[] = {0,0,0,0,0,0,0,0,0,0};
1586 uint16_t fcCnts
[] = {0,0,0,0,0,0,0,0,0,0};
1587 uint8_t fcLensFnd
= 0;
1588 uint8_t lastFCcnt
=0;
1589 uint32_t fcCounter
= 0;
1591 if (size
== 0) return 0;
1593 // prime i to first up transition
1594 for (i
= 1; i
< size
-1; i
++)
1595 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1])
1598 for (; i
< size
-1; i
++){
1599 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1]){
1600 // new up transition
1603 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1604 if (lastFCcnt
==5 && fcCounter
==9) fcCounter
--;
1605 //if odd and not rc/5 add one (for when we get a fc 9 instead of 10)
1606 if ((fcCounter
==9 && fcCounter
& 1) || fcCounter
==4) fcCounter
++;
1608 // save last field clock count (fc/xx)
1609 // find which fcLens to save it to:
1610 for (int ii
=0; ii
<10; ii
++){
1611 if (fcLens
[ii
]==fcCounter
){
1617 if (fcCounter
>0 && fcLensFnd
<10){
1619 fcCnts
[fcLensFnd
]++;
1620 fcLens
[fcLensFnd
++]=fcCounter
;
1629 uint8_t best1
=9, best2
=9, best3
=9;
1631 // go through fclens and find which ones are bigest 2
1632 for (i
=0; i
<10; i
++){
1633 // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt);
1634 // get the 3 best FC values
1635 if (fcCnts
[i
]>maxCnt1
) {
1640 } else if(fcCnts
[i
]>fcCnts
[best2
]){
1643 } else if(fcCnts
[i
]>fcCnts
[best3
]){
1647 uint8_t fcH
=0, fcL
=0;
1648 if (fcLens
[best1
]>fcLens
[best2
]){
1656 *mostFC
=fcLens
[best1
];
1657 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1659 uint16_t fcs
= (((uint16_t)fcH
)<<8) | fcL
;
1660 // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
1666 //countPSK_FC is to detect the psk carrier clock length.
1667 //counts and returns the 1 most common wave length
1668 uint8_t countPSK_FC(uint8_t *BitStream
, size_t size
)
1670 uint8_t fcLens
[] = {0,0,0,0,0,0,0,0,0,0};
1671 uint16_t fcCnts
[] = {0,0,0,0,0,0,0,0,0,0};
1672 uint8_t fcLensFnd
= 0;
1673 uint32_t fcCounter
= 0;
1675 if (size
== 0) return 0;
1677 // prime i to first up transition
1678 for (i
= 1; i
< size
-1; i
++)
1679 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1])
1682 for (; i
< size
-1; i
++){
1683 if (BitStream
[i
] > BitStream
[i
-1] && BitStream
[i
] >= BitStream
[i
+1]){
1684 // new up transition
1687 // save last field clock count (fc/xx)
1688 // find which fcLens to save it to:
1689 for (int ii
=0; ii
<10; ii
++){
1690 if (fcLens
[ii
]==fcCounter
){
1696 if (fcCounter
>0 && fcLensFnd
<10){
1698 fcCnts
[fcLensFnd
]++;
1699 fcLens
[fcLensFnd
++]=fcCounter
;
1710 // go through fclens and find which ones are bigest
1711 for (i
=0; i
<10; i
++){
1712 //PrintAndLog("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]);
1713 // get the best FC value
1714 if (fcCnts
[i
]>maxCnt1
) {
1719 return fcLens
[best1
];
1722 //by marshmellow - demodulate PSK1 wave
1723 //uses wave lengths (# Samples)
1724 int pskRawDemod(uint8_t dest
[], size_t *size
, int *clock
, int *invert
)
1726 uint16_t loopCnt
= 4096; //don't need to loop through entire array...
1727 if (size
== 0) return -1;
1728 if (*size
<loopCnt
) loopCnt
= *size
;
1730 uint8_t curPhase
= *invert
;
1731 size_t i
, waveStart
=1, waveEnd
=0, firstFullWave
=0, lastClkBit
=0;
1732 uint8_t fc
=0, fullWaveLen
=0, tol
=1;
1733 uint16_t errCnt
=0, waveLenCnt
=0;
1734 fc
= countPSK_FC(dest
, *size
);
1735 if (fc
!=2 && fc
!=4 && fc
!=8) return -1;
1736 //PrintAndLog("DEBUG: FC: %d",fc);
1737 *clock
= DetectPSKClock(dest
, *size
, *clock
);
1738 if (*clock
==0) return -1;
1739 int avgWaveVal
=0, lastAvgWaveVal
=0;
1740 //find first phase shift
1741 for (i
=0; i
<loopCnt
; i
++){
1742 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1744 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1745 waveLenCnt
= waveEnd
-waveStart
;
1746 if (waveLenCnt
> fc
&& waveStart
> fc
){ //not first peak and is a large wave
1747 lastAvgWaveVal
= avgWaveVal
/(waveLenCnt
);
1748 firstFullWave
= waveStart
;
1749 fullWaveLen
=waveLenCnt
;
1750 //if average wave value is > graph 0 then it is an up wave or a 1
1751 if (lastAvgWaveVal
> 123) curPhase
^=1; //fudge graph 0 a little 123 vs 128
1757 avgWaveVal
+=dest
[i
+2];
1759 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1760 lastClkBit
= firstFullWave
; //set start of wave as clock align
1761 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1766 memset(dest
,curPhase
^1,firstFullWave
/ *clock
);
1767 numBits
+= (firstFullWave
/ *clock
);
1768 dest
[numBits
++] = curPhase
; //set first read bit
1769 for (i
= firstFullWave
+fullWaveLen
-1; i
< *size
-3; i
++){
1770 //top edge of wave = start of new wave
1771 if (dest
[i
]+fc
< dest
[i
+1] && dest
[i
+1] >= dest
[i
+2]){
1772 if (waveStart
== 0) {
1775 avgWaveVal
= dest
[i
+1];
1778 waveLenCnt
= waveEnd
-waveStart
;
1779 lastAvgWaveVal
= avgWaveVal
/waveLenCnt
;
1780 if (waveLenCnt
> fc
){
1781 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1782 //if this wave is a phase shift
1783 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1784 if (i
+1 >= lastClkBit
+ *clock
- tol
){ //should be a clock bit
1786 dest
[numBits
++] = curPhase
;
1787 lastClkBit
+= *clock
;
1788 } else if (i
<lastClkBit
+10+fc
){
1789 //noise after a phase shift - ignore
1790 } else { //phase shift before supposed to based on clock
1792 dest
[numBits
++] = 77;
1794 } else if (i
+1 > lastClkBit
+ *clock
+ tol
+ fc
){
1795 lastClkBit
+= *clock
; //no phase shift but clock bit
1796 dest
[numBits
++] = curPhase
;
1802 avgWaveVal
+=dest
[i
+1];