]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
a3a7a50087727fc1f85a7a5a8df881537e21e04e
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 ) return 0 ; //allow only 1s and 0s
86 // 111111111 bit pattern represent start of frame
87 // include 0 in front to help get start pos
88 uint8_t preamble
[] = { 0 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 };
90 uint32_t parityBits
= 0 ;
94 errChk
= preambleSearch ( BitStream
, preamble
, sizeof ( preamble
), size
, startIdx
);
95 if ( errChk
== 0 || * size
< 64 ) return 0 ;
96 if (* size
> 64 ) FmtLen
= 22 ;
97 * startIdx
+= 1 ; //get rid of 0 from preamble
99 for ( i
= 0 ; i
< FmtLen
; i
++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
100 parityBits
= bytebits_to_byte ( BitStream
+( i
* 5 )+ idx
, 5 );
101 //check even parity - quit if failed
102 if ( parityTest ( parityBits
, 5 , 0 ) == 0 ) return 0 ;
103 //set uint64 with ID from BitStream
104 for ( uint8_t ii
= 0 ; ii
< 4 ; ii
++){
105 * hi
= (* hi
<< 1 ) | (* lo
>> 63 );
106 * lo
= (* lo
<< 1 ) | ( BitStream
[( i
* 5 )+ ii
+ idx
]);
109 if ( errChk
!= 0 ) return 1 ;
110 //skip last 5 bit parity test for simplicity.
116 //demodulates strong heavily clipped samples
117 int cleanAskRawDemod ( uint8_t * BinStream
, size_t * size
, int clk
, int invert
, int high
, int low
)
119 size_t bitCnt
= 0 , smplCnt
= 0 , errCnt
= 0 ;
120 uint8_t waveHigh
= 0 ;
121 for ( size_t i
= 0 ; i
< * size
; i
++){
122 if ( BinStream
[ i
] >= high
&& waveHigh
){
124 } else if ( BinStream
[ i
] <= low
&& ! waveHigh
){
126 } else { //transition
127 if (( BinStream
[ i
] >= high
&& ! waveHigh
) || ( BinStream
[ i
] <= low
&& waveHigh
)){
128 if ( smplCnt
> clk
-( clk
/ 4 )- 1 ) { //full clock
129 if ( smplCnt
> clk
+ ( clk
/ 4 )+ 1 ) { //too many samples
131 BinStream
[ bitCnt
++]= 7 ;
132 } else if ( waveHigh
) {
133 BinStream
[ bitCnt
++] = invert
;
134 BinStream
[ bitCnt
++] = invert
;
135 } else if (! waveHigh
) {
136 BinStream
[ bitCnt
++] = invert
^ 1 ;
137 BinStream
[ bitCnt
++] = invert
^ 1 ;
141 } else if ( smplCnt
> ( clk
/ 2 ) - ( clk
/ 4 )- 1 ) {
143 BinStream
[ bitCnt
++] = invert
;
144 } else if (! waveHigh
) {
145 BinStream
[ bitCnt
++] = invert
^ 1 ;
149 } else if (! bitCnt
) {
151 waveHigh
= ( BinStream
[ i
] >= high
);
155 //transition bit oops
157 } else { //haven't hit new high or new low yet
167 void askAmp ( uint8_t * BitStream
, size_t size
)
169 for ( size_t i
= 1 ; i
< size
; i
++){
170 if ( BitStream
[ i
]- BitStream
[ i
- 1 ]>= 30 ) //large jump up
172 else if ( BitStream
[ i
]- BitStream
[ i
- 1 ]<=- 20 ) //large jump down
179 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
180 int askdemod ( uint8_t * BinStream
, size_t * size
, int * clk
, int * invert
, int maxErr
, uint8_t amp
, uint8_t askType
)
182 if (* size
== 0 ) return - 1 ;
183 int start
= DetectASKClock ( BinStream
, * size
, clk
, maxErr
); //clock default
184 if (* clk
== 0 || start
< 0 ) return - 3 ;
185 if (* invert
!= 1 ) * invert
= 0 ;
186 if ( amp
== 1 ) askAmp ( BinStream
, * size
);
188 uint8_t initLoopMax
= 255 ;
189 if ( initLoopMax
> * size
) initLoopMax
= * size
;
190 // Detect high and lows
191 //25% clip in case highs and lows aren't clipped [marshmellow]
193 if ( getHiLo ( BinStream
, initLoopMax
, & high
, & low
, 75 , 75 ) < 1 )
194 return - 2 ; //just noise
197 // if clean clipped waves detected run alternate demod
198 if ( DetectCleanAskWave ( BinStream
, * size
, high
, low
)) {
199 errCnt
= cleanAskRawDemod ( BinStream
, size
, * clk
, * invert
, high
, low
);
200 if ( askType
) //askman
201 return manrawdecode ( BinStream
, size
, 0 );
206 int lastBit
; //set first clock check - can go negative
207 size_t i
, bitnum
= 0 ; //output counter
209 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
210 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
211 size_t MaxBits
= 1024 ;
212 lastBit
= start
- * clk
;
214 for ( i
= start
; i
< * size
; ++ i
) {
215 if ( i
- lastBit
>= * clk
- tol
){
216 if ( BinStream
[ i
] >= high
) {
217 BinStream
[ bitnum
++] = * invert
;
218 } else if ( BinStream
[ i
] <= low
) {
219 BinStream
[ bitnum
++] = * invert
^ 1 ;
220 } else if ( i
- lastBit
>= * clk
+ tol
) {
222 BinStream
[ bitnum
++]= 7 ;
225 } else { //in tolerance - looking for peak
230 } else if ( i
- lastBit
>= (* clk
/ 2 - tol
) && ! midBit
&& ! askType
){
231 if ( BinStream
[ i
] >= high
) {
232 BinStream
[ bitnum
++] = * invert
;
233 } else if ( BinStream
[ i
] <= low
) {
234 BinStream
[ bitnum
++] = * invert
^ 1 ;
235 } else if ( i
- lastBit
>= * clk
/ 2 + tol
) {
236 BinStream
[ bitnum
] = BinStream
[ bitnum
- 1 ];
238 } else { //in tolerance - looking for peak
243 if ( bitnum
>= MaxBits
) break ;
250 //take 10 and 01 and manchester decode
251 //run through 2 times and take least errCnt
252 int manrawdecode ( uint8_t * BitStream
, size_t * size
, uint8_t invert
)
254 uint16_t bitnum
= 0 , MaxBits
= 512 , errCnt
= 0 ;
256 uint16_t bestErr
= 1000 , bestRun
= 0 ;
257 if (* size
< 16 ) return - 1 ;
258 //find correct start position [alignment]
259 for ( ii
= 0 ; ii
< 2 ;++ ii
){
260 for ( i
= ii
; i
<* size
- 3 ; i
+= 2 )
261 if ( BitStream
[ i
]== BitStream
[ i
+ 1 ])
271 for ( i
= bestRun
; i
< * size
- 3 ; i
+= 2 ){
272 if ( BitStream
[ i
] == 1 && ( BitStream
[ i
+ 1 ] == 0 )){
273 BitStream
[ bitnum
++]= invert
;
274 } else if (( BitStream
[ i
] == 0 ) && BitStream
[ i
+ 1 ] == 1 ){
275 BitStream
[ bitnum
++]= invert
^ 1 ;
277 BitStream
[ bitnum
++]= 7 ;
279 if ( bitnum
> MaxBits
) break ;
286 //encode binary data into binary manchester
287 int ManchesterEncode ( uint8_t * BitStream
, size_t size
)
289 size_t modIdx
= 20000 , i
= 0 ;
290 if ( size
> modIdx
) return - 1 ;
291 for ( size_t idx
= 0 ; idx
< size
; idx
++){
292 BitStream
[ idx
+ modIdx
++] = BitStream
[ idx
];
293 BitStream
[ idx
+ modIdx
++] = BitStream
[ idx
]^ 1 ;
295 for (; i
<( size
* 2 ); i
++){
296 BitStream
[ i
] = BitStream
[ i
+ 20000 ];
302 //take 01 or 10 = 1 and 11 or 00 = 0
303 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
304 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
305 int BiphaseRawDecode ( uint8_t * BitStream
, size_t * size
, int offset
, int invert
)
310 uint16_t MaxBits
= 512 ;
311 //if not enough samples - error
312 if (* size
< 51 ) return - 1 ;
313 //check for phase change faults - skip one sample if faulty
314 uint8_t offsetA
= 1 , offsetB
= 1 ;
316 if ( BitStream
[ i
+ 1 ]== BitStream
[ i
+ 2 ]) offsetA
= 0 ;
317 if ( BitStream
[ i
+ 2 ]== BitStream
[ i
+ 3 ]) offsetB
= 0 ;
319 if (! offsetA
&& offsetB
) offset
++;
320 for ( i
= offset
; i
<* size
- 3 ; i
+= 2 ){
321 //check for phase error
322 if ( BitStream
[ i
+ 1 ]== BitStream
[ i
+ 2 ]) {
323 BitStream
[ bitnum
++]= 7 ;
326 if (( BitStream
[ i
]== 1 && BitStream
[ i
+ 1 ]== 0 ) || ( BitStream
[ i
]== 0 && BitStream
[ i
+ 1 ]== 1 )){
327 BitStream
[ bitnum
++]= 1 ^ invert
;
328 } else if (( BitStream
[ i
]== 0 && BitStream
[ i
+ 1 ]== 0 ) || ( BitStream
[ i
]== 1 && BitStream
[ i
+ 1 ]== 1 )){
329 BitStream
[ bitnum
++]= invert
;
331 BitStream
[ bitnum
++]= 7 ;
334 if ( bitnum
> MaxBits
) break ;
341 // demod gProxIIDemod
342 // error returns as -x
343 // success returns start position in BitStream
344 // BitStream must contain previously askrawdemod and biphasedemoded data
345 int gProxII_Demod ( uint8_t BitStream
[], size_t * size
)
348 uint8_t preamble
[] = { 1 , 1 , 1 , 1 , 1 , 0 };
350 uint8_t errChk
= preambleSearch ( BitStream
, preamble
, sizeof ( preamble
), size
, & startIdx
);
351 if ( errChk
== 0 ) return - 3 ; //preamble not found
352 if (* size
!= 96 ) return - 2 ; //should have found 96 bits
353 //check first 6 spacer bits to verify format
354 if (! BitStream
[ startIdx
+ 5 ] && ! BitStream
[ startIdx
+ 10 ] && ! BitStream
[ startIdx
+ 15 ] && ! BitStream
[ startIdx
+ 20 ] && ! BitStream
[ startIdx
+ 25 ] && ! BitStream
[ startIdx
+ 30 ]){
355 //confirmed proper separator bits found
356 //return start position
357 return ( int ) startIdx
;
362 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
363 size_t fsk_wave_demod ( uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
365 size_t last_transition
= 0 ;
368 if ( fchigh
== 0 ) fchigh
= 10 ;
369 if ( fclow
== 0 ) fclow
= 8 ;
370 //set the threshold close to 0 (graph) or 128 std to avoid static
371 uint8_t threshold_value
= 123 ;
373 // sync to first lo-hi transition, and threshold
375 // Need to threshold first sample
377 if ( dest
[ 0 ] < threshold_value
) dest
[ 0 ] = 0 ;
381 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
382 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
383 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
384 for ( idx
= 1 ; idx
< size
; idx
++) {
385 // threshold current value
387 if ( dest
[ idx
] < threshold_value
) dest
[ idx
] = 0 ;
390 // Check for 0->1 transition
391 if ( dest
[ idx
- 1 ] < dest
[ idx
]) { // 0 -> 1 transition
392 if (( idx
- last_transition
)<( fclow
- 2 )){ //0-5 = garbage noise
393 //do nothing with extra garbage
394 } else if (( idx
- last_transition
) < ( fchigh
- 1 )) { //6-8 = 8 waves
396 } else if (( idx
- last_transition
) > ( fchigh
+ 1 ) && ! numBits
) { //12 + and first bit = garbage
397 //do nothing with beginning garbage
398 } else { //9+ = 10 waves
401 last_transition
= idx
;
404 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
407 //translate 11111100000 to 10
408 size_t aggregate_bits ( uint8_t * dest
, size_t size
, uint8_t rfLen
,
409 uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
411 uint8_t lastval
= dest
[ 0 ];
415 for ( idx
= 1 ; idx
< size
; idx
++) {
417 if ( dest
[ idx
]== lastval
) continue ;
419 //if lastval was 1, we have a 1->0 crossing
420 if ( dest
[ idx
- 1 ]== 1 ) {
421 if (! numBits
&& n
< rfLen
/ fclow
) {
426 n
= ( n
* fclow
+ rfLen
/ 2 ) / rfLen
;
427 } else { // 0->1 crossing
428 //test first bitsample too small
429 if (! numBits
&& n
< rfLen
/ fchigh
) {
434 n
= ( n
* fchigh
+ rfLen
/ 2 ) / rfLen
;
438 memset ( dest
+ numBits
, dest
[ idx
- 1 ]^ invert
, n
);
443 // if valid extra bits at the end were all the same frequency - add them in
444 if ( n
> rfLen
/ fchigh
) {
445 if ( dest
[ idx
- 2 ]== 1 ) {
446 n
= ( n
* fclow
+ rfLen
/ 2 ) / rfLen
;
448 n
= ( n
* fchigh
+ rfLen
/ 2 ) / rfLen
;
450 memset ( dest
+ numBits
, dest
[ idx
- 1 ]^ invert
, n
);
455 //by marshmellow (from holiman's base)
456 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
457 int fskdemod ( uint8_t * dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
460 size
= fsk_wave_demod ( dest
, size
, fchigh
, fclow
);
461 size
= aggregate_bits ( dest
, size
, rfLen
, invert
, fchigh
, fclow
);
465 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
466 int HIDdemodFSK ( uint8_t * dest
, size_t * size
, uint32_t * hi2
, uint32_t * hi
, uint32_t * lo
)
468 if ( justNoise ( dest
, * size
)) return - 1 ;
470 size_t numStart
= 0 , size2
=* size
, startIdx
= 0 ;
472 * size
= fskdemod ( dest
, size2
, 50 , 1 , 10 , 8 ); //fsk2a
473 if (* size
< 96 * 2 ) return - 2 ;
474 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
475 uint8_t preamble
[] = { 0 , 0 , 0 , 1 , 1 , 1 , 0 , 1 };
476 // find bitstring in array
477 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
478 if ( errChk
== 0 ) return - 3 ; //preamble not found
480 numStart
= startIdx
+ sizeof ( preamble
);
481 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
482 for ( size_t idx
= numStart
; ( idx
- numStart
) < * size
- sizeof ( preamble
); idx
+= 2 ){
483 if ( dest
[ idx
] == dest
[ idx
+ 1 ]){
484 return - 4 ; //not manchester data
486 * hi2
= (* hi2
<< 1 )|(* hi
>> 31 );
487 * hi
= (* hi
<< 1 )|(* lo
>> 31 );
488 //Then, shift in a 0 or one into low
489 if ( dest
[ idx
] && ! dest
[ idx
+ 1 ]) // 1 0
494 return ( int ) startIdx
;
497 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
498 int ParadoxdemodFSK ( uint8_t * dest
, size_t * size
, uint32_t * hi2
, uint32_t * hi
, uint32_t * lo
)
500 if ( justNoise ( dest
, * size
)) return - 1 ;
502 size_t numStart
= 0 , size2
=* size
, startIdx
= 0 ;
504 * size
= fskdemod ( dest
, size2
, 50 , 1 , 10 , 8 ); //fsk2a
505 if (* size
< 96 ) return - 2 ;
507 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
508 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 1 , 1 , 1 , 1 };
510 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
511 if ( errChk
== 0 ) return - 3 ; //preamble not found
513 numStart
= startIdx
+ sizeof ( preamble
);
514 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
515 for ( size_t idx
= numStart
; ( idx
- numStart
) < * size
- sizeof ( preamble
); idx
+= 2 ){
516 if ( dest
[ idx
] == dest
[ idx
+ 1 ])
517 return - 4 ; //not manchester data
518 * hi2
= (* hi2
<< 1 )|(* hi
>> 31 );
519 * hi
= (* hi
<< 1 )|(* lo
>> 31 );
520 //Then, shift in a 0 or one into low
521 if ( dest
[ idx
] && ! dest
[ idx
+ 1 ]) // 1 0
526 return ( int ) startIdx
;
529 uint32_t bytebits_to_byte ( uint8_t * src
, size_t numbits
)
532 for ( int i
= 0 ; i
< numbits
; i
++)
534 num
= ( num
<< 1 ) | (* src
);
540 //least significant bit first
541 uint32_t bytebits_to_byteLSBF ( uint8_t * src
, size_t numbits
)
544 for ( int i
= 0 ; i
< numbits
; i
++)
546 num
= ( num
<< 1 ) | *( src
+ ( numbits
-( i
+ 1 )));
551 int IOdemodFSK ( uint8_t * dest
, size_t size
)
553 if ( justNoise ( dest
, size
)) return - 1 ;
554 //make sure buffer has data
555 if ( size
< 66 * 64 ) return - 2 ;
557 size
= fskdemod ( dest
, size
, 64 , 1 , 10 , 8 ); // FSK2a RF/64
558 if ( size
< 65 ) return - 3 ; //did we get a good demod?
560 //0 10 20 30 40 50 60
562 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
563 //-----------------------------------------------------------------------------
564 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
566 //XSF(version)facility:codeone+codetwo
569 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
570 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), & size
, & startIdx
);
571 if ( errChk
== 0 ) return - 4 ; //preamble not found
573 if (! dest
[ startIdx
+ 8 ] && dest
[ startIdx
+ 17 ]== 1 && dest
[ startIdx
+ 26 ]== 1 && dest
[ startIdx
+ 35 ]== 1 && dest
[ startIdx
+ 44 ]== 1 && dest
[ startIdx
+ 53 ]== 1 ){
574 //confirmed proper separator bits found
575 //return start position
576 return ( int ) startIdx
;
582 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
583 // Parity Type (1 for odd; 0 for even; 2 Always 1's), and binary Length (length to run)
584 size_t removeParity ( uint8_t * BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
)
586 uint32_t parityWd
= 0 ;
587 size_t j
= 0 , bitCnt
= 0 ;
588 for ( int word
= 0 ; word
< ( bLen
); word
+= pLen
){
589 for ( int bit
= 0 ; bit
< pLen
; bit
++){
590 parityWd
= ( parityWd
<< 1 ) | BitStream
[ startIdx
+ word
+ bit
];
591 BitStream
[ j
++] = ( BitStream
[ startIdx
+ word
+ bit
]);
593 j
--; // overwrite parity with next data
594 // if parity fails then return 0
595 if ( pType
== 2 ) { // then marker bit which should be a 1
596 if (! BitStream
[ j
]) return 0 ;
598 if ( parityTest ( parityWd
, pLen
, pType
) == 0 ) return 0 ;
603 // if we got here then all the parities passed
604 //return ID start index and size
608 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
609 // BitStream must contain previously askrawdemod and biphasedemoded data
610 int FDXBdemodBI ( uint8_t * dest
, size_t * size
)
612 //make sure buffer has enough data
613 if (* size
< 128 ) return - 1 ;
616 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
618 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
619 if ( errChk
== 0 ) return - 2 ; //preamble not found
620 return ( int ) startIdx
;
624 // FSK Demod then try to locate an AWID ID
625 int AWIDdemodFSK ( uint8_t * dest
, size_t * size
)
627 //make sure buffer has enough data
628 if (* size
< 96 * 50 ) return - 1 ;
630 if ( justNoise ( dest
, * size
)) return - 2 ;
633 * size
= fskdemod ( dest
, * size
, 50 , 1 , 10 , 8 ); // fsk2a RF/50
634 if (* size
< 96 ) return - 3 ; //did we get a good demod?
636 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
638 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
639 if ( errChk
== 0 ) return - 4 ; //preamble not found
640 if (* size
!= 96 ) return - 5 ;
641 return ( int ) startIdx
;
645 // FSK Demod then try to locate an Farpointe Data (pyramid) ID
646 int PyramiddemodFSK ( uint8_t * dest
, size_t * size
)
648 //make sure buffer has data
649 if (* size
< 128 * 50 ) return - 5 ;
651 //test samples are not just noise
652 if ( justNoise ( dest
, * size
)) return - 1 ;
655 * size
= fskdemod ( dest
, * size
, 50 , 1 , 10 , 8 ); // fsk2a RF/50
656 if (* size
< 128 ) return - 2 ; //did we get a good demod?
658 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
660 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
661 if ( errChk
== 0 ) return - 4 ; //preamble not found
662 if (* size
!= 128 ) return - 3 ;
663 return ( int ) startIdx
;
667 // to detect a wave that has heavily clipped (clean) samples
668 uint8_t DetectCleanAskWave ( uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
)
672 size_t loopEnd
= 512 + 60 ;
673 if ( loopEnd
> size
) loopEnd
= size
;
674 for ( size_t i
= 60 ; i
< loopEnd
; i
++){
675 if ( dest
[ i
]> low
&& dest
[ i
]< high
)
681 if ( cntPeaks
> 300 ) return 1 ;
687 // to help detect clocks on heavily clipped samples
688 // based on count of low to low
689 int DetectStrongAskClock ( uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
)
691 uint8_t fndClk
[] = { 8 , 16 , 32 , 40 , 50 , 64 , 128 };
695 // get to first full low to prime loop and skip incomplete first pulse
696 while (( dest
[ i
] < high
) && ( i
< size
))
698 while (( dest
[ i
] > low
) && ( i
< size
))
701 // loop through all samples
703 // measure from low to low
704 while (( dest
[ i
] > low
) && ( i
< size
))
707 while (( dest
[ i
] < high
) && ( i
< size
))
709 while (( dest
[ i
] > low
) && ( i
< size
))
711 //get minimum measured distance
712 if ( i
- startwave
< minClk
&& i
< size
)
713 minClk
= i
- startwave
;
716 for ( uint8_t clkCnt
= 0 ; clkCnt
< 7 ; clkCnt
++) {
717 if ( minClk
>= fndClk
[ clkCnt
]-( fndClk
[ clkCnt
]/ 8 ) && minClk
<= fndClk
[ clkCnt
]+ 1 )
718 return fndClk
[ clkCnt
];
724 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
725 // maybe somehow adjust peak trimming value based on samples to fix?
726 // return start index of best starting position for that clock and return clock (by reference)
727 int DetectASKClock ( uint8_t dest
[], size_t size
, int * clock
, int maxErr
)
730 uint8_t clk
[] = { 255 , 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 };
732 uint8_t loopCnt
= 255 ; //don't need to loop through entire array...
733 if ( size
<= loopCnt
) return - 1 ; //not enough samples
735 //if we already have a valid clock
738 if ( clk
[ i
] == * clock
) clockFnd
= i
;
739 //clock found but continue to find best startpos
741 //get high and low peak
743 if ( getHiLo ( dest
, loopCnt
, & peak
, & low
, 75 , 75 ) < 1 ) return - 1 ;
745 //test for large clean peaks
747 if ( DetectCleanAskWave ( dest
, size
, peak
, low
)== 1 ){
748 int ans
= DetectStrongAskClock ( dest
, size
, peak
, low
);
749 for ( i
= clkEnd
- 1 ; i
> 0 ; i
--){
753 return 0 ; // for strong waves i don't use the 'best start position' yet...
754 //break; //clock found but continue to find best startpos [not yet]
761 uint8_t clkCnt
, tol
= 0 ;
762 uint16_t bestErr
[]={ 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 };
763 uint8_t bestStart
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
765 size_t arrLoc
, loopEnd
;
773 //test each valid clock from smallest to greatest to see which lines up
774 for (; clkCnt
< clkEnd
; clkCnt
++){
775 if ( clk
[ clkCnt
] <= 32 ){
780 //if no errors allowed - keep start within the first clock
781 if (! maxErr
&& size
> clk
[ clkCnt
]* 2 + tol
&& clk
[ clkCnt
]< 128 ) loopCnt
= clk
[ clkCnt
]* 2 ;
782 bestErr
[ clkCnt
]= 1000 ;
783 //try lining up the peaks by moving starting point (try first few clocks)
784 for ( ii
= 0 ; ii
< loopCnt
; ii
++){
785 if ( dest
[ ii
] < peak
&& dest
[ ii
] > low
) continue ;
788 // now that we have the first one lined up test rest of wave array
789 loopEnd
= (( size
- ii
- tol
) / clk
[ clkCnt
]) - 1 ;
790 for ( i
= 0 ; i
< loopEnd
; ++ i
){
791 arrLoc
= ii
+ ( i
* clk
[ clkCnt
]);
792 if ( dest
[ arrLoc
] >= peak
|| dest
[ arrLoc
] <= low
){
793 } else if ( dest
[ arrLoc
- tol
] >= peak
|| dest
[ arrLoc
- tol
] <= low
){
794 } else if ( dest
[ arrLoc
+ tol
] >= peak
|| dest
[ arrLoc
+ tol
] <= low
){
795 } else { //error no peak detected
799 //if we found no errors then we can stop here and a low clock (common clocks)
800 // this is correct one - return this clock
801 //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
802 if ( errCnt
== 0 && clkCnt
< 7 ) {
803 if (! clockFnd
) * clock
= clk
[ clkCnt
];
806 //if we found errors see if it is lowest so far and save it as best run
807 if ( errCnt
< bestErr
[ clkCnt
]){
808 bestErr
[ clkCnt
]= errCnt
;
809 bestStart
[ clkCnt
]= ii
;
815 for ( iii
= 1 ; iii
< clkEnd
; ++ iii
){
816 if ( bestErr
[ iii
] < bestErr
[ best
]){
817 if ( bestErr
[ iii
] == 0 ) bestErr
[ iii
]= 1 ;
818 // current best bit to error ratio vs new bit to error ratio
819 if ( ( size
/ clk
[ best
])/ bestErr
[ best
] < ( size
/ clk
[ iii
])/ bestErr
[ iii
] ){
824 //if (bestErr[best] > maxErr) return -1;
825 if (! clockFnd
) * clock
= clk
[ best
];
826 return bestStart
[ best
];
830 //detect psk clock by reading each phase shift
831 // a phase shift is determined by measuring the sample length of each wave
832 int DetectPSKClock ( uint8_t dest
[], size_t size
, int clock
)
834 uint8_t clk
[]={ 255 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 }; //255 is not a valid clock
835 uint16_t loopCnt
= 4096 ; //don't need to loop through entire array...
836 if ( size
== 0 ) return 0 ;
837 if ( size
< loopCnt
) loopCnt
= size
;
839 //if we already have a valid clock quit
842 if ( clk
[ i
] == clock
) return clock
;
844 size_t waveStart
= 0 , waveEnd
= 0 , firstFullWave
= 0 , lastClkBit
= 0 ;
845 uint8_t clkCnt
, fc
= 0 , fullWaveLen
= 0 , tol
= 1 ;
846 uint16_t peakcnt
= 0 , errCnt
= 0 , waveLenCnt
= 0 ;
847 uint16_t bestErr
[]={ 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 };
848 uint16_t peaksdet
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
849 fc
= countFC ( dest
, size
, 0 );
850 if ( fc
!= 2 && fc
!= 4 && fc
!= 8 ) return - 1 ;
851 //PrintAndLog("DEBUG: FC: %d",fc);
853 //find first full wave
854 for ( i
= 0 ; i
< loopCnt
; i
++){
855 if ( dest
[ i
] < dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
856 if ( waveStart
== 0 ) {
858 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
861 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
862 waveLenCnt
= waveEnd
- waveStart
;
863 if ( waveLenCnt
> fc
){
864 firstFullWave
= waveStart
;
865 fullWaveLen
= waveLenCnt
;
872 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
874 //test each valid clock from greatest to smallest to see which lines up
875 for ( clkCnt
= 7 ; clkCnt
>= 1 ; clkCnt
--){
876 lastClkBit
= firstFullWave
; //set end of wave as clock align
880 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
882 for ( i
= firstFullWave
+ fullWaveLen
- 1 ; i
< loopCnt
- 2 ; i
++){
883 //top edge of wave = start of new wave
884 if ( dest
[ i
] < dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
885 if ( waveStart
== 0 ) {
890 waveLenCnt
= waveEnd
- waveStart
;
891 if ( waveLenCnt
> fc
){
892 //if this wave is a phase shift
893 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc);
894 if ( i
+ 1 >= lastClkBit
+ clk
[ clkCnt
] - tol
){ //should be a clock bit
896 lastClkBit
+= clk
[ clkCnt
];
897 } else if ( i
< lastClkBit
+ 8 ){
898 //noise after a phase shift - ignore
899 } else { //phase shift before supposed to based on clock
902 } else if ( i
+ 1 > lastClkBit
+ clk
[ clkCnt
] + tol
+ fc
){
903 lastClkBit
+= clk
[ clkCnt
]; //no phase shift but clock bit
912 if ( errCnt
<= bestErr
[ clkCnt
]) bestErr
[ clkCnt
]= errCnt
;
913 if ( peakcnt
> peaksdet
[ clkCnt
]) peaksdet
[ clkCnt
]= peakcnt
;
915 //all tested with errors
916 //return the highest clk with the most peaks found
918 for ( i
= 7 ; i
>= 1 ; i
--){
919 if ( peaksdet
[ i
] > peaksdet
[ best
]) {
922 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
928 //detect nrz clock by reading #peaks vs no peaks(or errors)
929 int DetectNRZClock ( uint8_t dest
[], size_t size
, int clock
)
932 uint8_t clk
[]={ 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 };
933 size_t loopCnt
= 4096 ; //don't need to loop through entire array...
934 if ( size
== 0 ) return 0 ;
935 if ( size
< loopCnt
) loopCnt
= size
;
937 //if we already have a valid clock quit
939 if ( clk
[ i
] == clock
) return clock
;
941 //get high and low peak
943 if ( getHiLo ( dest
, loopCnt
, & peak
, & low
, 75 , 75 ) < 1 ) return 0 ;
945 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
950 uint16_t peaksdet
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
952 //test for large clipped waves
953 for ( i
= 0 ; i
< loopCnt
; i
++){
954 if ( dest
[ i
] >= peak
|| dest
[ i
] <= low
){
957 if ( peakcnt
> 0 && maxPeak
< peakcnt
){
964 //test each valid clock from smallest to greatest to see which lines up
965 for ( clkCnt
= 0 ; clkCnt
< 8 ; ++ clkCnt
){
966 //ignore clocks smaller than largest peak
967 if ( clk
[ clkCnt
]< maxPeak
) continue ;
969 //try lining up the peaks by moving starting point (try first 256)
970 for ( ii
= 0 ; ii
< loopCnt
; ++ ii
){
971 if (( dest
[ ii
] >= peak
) || ( dest
[ ii
] <= low
)){
973 // now that we have the first one lined up test rest of wave array
974 for ( i
= 0 ; i
< (( int )(( size
- ii
- tol
)/ clk
[ clkCnt
])- 1 ); ++ i
){
975 if ( dest
[ ii
+( i
* clk
[ clkCnt
])]>= peak
|| dest
[ ii
+( i
* clk
[ clkCnt
])]<= low
){
979 if ( peakcnt
> peaksdet
[ clkCnt
]) {
980 peaksdet
[ clkCnt
]= peakcnt
;
987 for ( iii
= 7 ; iii
> 0 ; iii
--){
988 if ( peaksdet
[ iii
] > peaksdet
[ best
]){
991 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
997 // convert psk1 demod to psk2 demod
998 // only transition waves are 1s
999 void psk1TOpsk2 ( uint8_t * BitStream
, size_t size
)
1002 uint8_t lastBit
= BitStream
[ 0 ];
1003 for (; i
< size
; i
++){
1004 if ( BitStream
[ i
]== 7 ){
1006 } else if ( lastBit
!= BitStream
[ i
]){
1007 lastBit
= BitStream
[ i
];
1017 // convert psk2 demod to psk1 demod
1018 // from only transition waves are 1s to phase shifts change bit
1019 void psk2TOpsk1 ( uint8_t * BitStream
, size_t size
)
1022 for ( size_t i
= 0 ; i
< size
; i
++){
1023 if ( BitStream
[ i
]== 1 ){
1031 // redesigned by marshmellow adjusted from existing decode functions
1032 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1033 int indala26decode ( uint8_t * bitStream
, size_t * size
, uint8_t * invert
)
1035 //26 bit 40134 format (don't know other formats)
1037 int long_wait
= 29 ; //29 leading zeros in format
1043 // Finding the start of a UID
1044 for ( start
= 0 ; start
<= * size
- 250 ; start
++) {
1045 first
= bitStream
[ start
];
1046 for ( i
= start
; i
< start
+ long_wait
; i
++) {
1047 if ( bitStream
[ i
] != first
) {
1051 if ( i
== ( start
+ long_wait
)) {
1055 if ( start
== * size
- 250 + 1 ) {
1056 // did not find start sequence
1059 // Inverting signal if needed
1061 for ( i
= start
; i
< * size
; i
++) {
1062 bitStream
[ i
] = ! bitStream
[ i
];
1068 //found start once now test length by finding next one
1069 for ( ii
= start
+ 29 ; ii
<= * size
- 250 ; ii
++) {
1070 first2
= bitStream
[ ii
];
1071 for ( iii
= ii
; iii
< ii
+ long_wait
; iii
++) {
1072 if ( bitStream
[ iii
] != first2
) {
1076 if ( iii
== ( ii
+ long_wait
)) {
1080 if ( ii
== * size
- 250 + 1 ){
1081 // did not find second start sequence
1088 for ( ii
= 0 ; ii
< bitCnt
; ii
++) {
1089 bitStream
[ ii
] = bitStream
[ i
++];
1095 // by marshmellow - demodulate NRZ wave (both similar enough)
1096 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1097 // there probably is a much simpler way to do this....
1098 int nrzRawDemod ( uint8_t * dest
, size_t * size
, int * clk
, int * invert
, int maxErr
)
1100 if ( justNoise ( dest
, * size
)) return - 1 ;
1101 * clk
= DetectNRZClock ( dest
, * size
, * clk
);
1102 if (* clk
== 0 ) return - 2 ;
1103 size_t i
, gLen
= 4096 ;
1104 if ( gLen
>* size
) gLen
= * size
;
1106 if ( getHiLo ( dest
, gLen
, & high
, & low
, 75 , 75 ) < 1 ) return - 3 ; //25% fuzz on high 25% fuzz on low
1107 int lastBit
= 0 ; //set first clock check
1108 size_t iii
= 0 , bitnum
= 0 ; //bitnum counter
1109 uint16_t errCnt
= 0 , MaxBits
= 1000 ;
1110 size_t bestErrCnt
= maxErr
+ 1 ;
1111 size_t bestPeakCnt
= 0 , bestPeakStart
= 0 ;
1112 uint8_t bestFirstPeakHigh
= 0 , firstPeakHigh
= 0 , curBit
= 0 , bitHigh
= 0 , errBitHigh
= 0 ;
1113 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
1115 uint8_t ignoreWindow
= 4 ;
1116 uint8_t ignoreCnt
= ignoreWindow
; //in case of noise near peak
1117 //loop to find first wave that works - align to clock
1118 for ( iii
= 0 ; iii
< gLen
; ++ iii
){
1119 if (( dest
[ iii
]>= high
) || ( dest
[ iii
]<= low
)){
1120 if ( dest
[ iii
]>= high
) firstPeakHigh
= 1 ;
1121 else firstPeakHigh
= 0 ;
1125 //loop through to see if this start location works
1126 for ( i
= iii
; i
< * size
; ++ i
) {
1127 // if we are at a clock bit
1128 if (( i
>= lastBit
+ * clk
- tol
) && ( i
<= lastBit
+ * clk
+ tol
)) {
1130 if ( dest
[ i
] >= high
|| dest
[ i
] <= low
) {
1134 ignoreCnt
= ignoreWindow
;
1136 } else if ( i
== lastBit
+ * clk
+ tol
) {
1139 //else if no bars found
1140 } else if ( dest
[ i
] < high
&& dest
[ i
] > low
){
1143 if ( errBitHigh
== 1 ) errCnt
++;
1148 } else if (( dest
[ i
]>= high
|| dest
[ i
]<= low
) && ( bitHigh
== 0 )) {
1149 //error bar found no clock...
1152 if ((( i
- iii
) / * clk
)>= MaxBits
) break ;
1154 //we got more than 64 good bits and not all errors
1155 if ((( i
- iii
) / * clk
) > 64 && ( errCnt
<= ( maxErr
))) {
1156 //possible good read
1157 if (! errCnt
|| peakCnt
> bestPeakCnt
){
1158 bestFirstPeakHigh
= firstPeakHigh
;
1159 bestErrCnt
= errCnt
;
1160 bestPeakCnt
= peakCnt
;
1161 bestPeakStart
= iii
;
1162 if (! errCnt
) break ; //great read - finish
1167 //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart);
1168 if ( bestErrCnt
> maxErr
) return bestErrCnt
;
1170 //best run is good enough set to best run and set overwrite BinStream
1171 lastBit
= bestPeakStart
- * clk
;
1172 memset ( dest
, bestFirstPeakHigh
^ 1 , bestPeakStart
/ * clk
);
1173 bitnum
+= ( bestPeakStart
/ * clk
);
1174 for ( i
= bestPeakStart
; i
< * size
; ++ i
) {
1175 // if expecting a clock bit
1176 if (( i
>= lastBit
+ * clk
- tol
) && ( i
<= lastBit
+ * clk
+ tol
)) {
1178 if ( dest
[ i
] >= high
|| dest
[ i
] <= low
) {
1182 ignoreCnt
= ignoreWindow
;
1184 if ( dest
[ i
] >= high
) curBit
^= 1 ;
1185 dest
[ bitnum
++] = curBit
;
1187 //else no bars found in clock area
1188 } else if ( i
== lastBit
+ * clk
+ tol
) {
1189 dest
[ bitnum
++] = curBit
;
1192 //else if no bars found
1193 } else if ( dest
[ i
] < high
&& dest
[ i
] > low
){
1194 if ( ignoreCnt
== 0 ){
1196 if ( errBitHigh
== 1 ){
1204 } else if (( dest
[ i
] >= high
|| dest
[ i
] <= low
) && ( bitHigh
== 0 )) {
1205 //error bar found no clock...
1208 if ( bitnum
>= MaxBits
) break ;
1215 //detects the bit clock for FSK given the high and low Field Clocks
1216 uint8_t detectFSKClk ( uint8_t * BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
)
1218 uint8_t clk
[] = { 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 0 };
1219 uint16_t rfLens
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1220 uint8_t rfCnts
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1221 uint8_t rfLensFnd
= 0 ;
1222 uint8_t lastFCcnt
= 0 ;
1223 uint16_t fcCounter
= 0 ;
1224 uint16_t rfCounter
= 0 ;
1225 uint8_t firstBitFnd
= 0 ;
1227 if ( size
== 0 ) return 0 ;
1229 uint8_t fcTol
= ( uint8_t )( 0.5 +( float )( fcHigh
- fcLow
)/ 2 );
1234 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1235 // prime i to first up transition
1236 for ( i
= 1 ; i
< size
- 1 ; i
++)
1237 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
]>= BitStream
[ i
+ 1 ])
1240 for (; i
< size
- 1 ; i
++){
1244 if ( BitStream
[ i
] <= BitStream
[ i
- 1 ] || BitStream
[ i
] < BitStream
[ i
+ 1 ])
1247 // if we got less than the small fc + tolerance then set it to the small fc
1248 if ( fcCounter
< fcLow
+ fcTol
)
1250 else //set it to the large fc
1253 //look for bit clock (rf/xx)
1254 if (( fcCounter
< lastFCcnt
|| fcCounter
> lastFCcnt
)){
1255 //not the same size as the last wave - start of new bit sequence
1256 if ( firstBitFnd
> 1 ){ //skip first wave change - probably not a complete bit
1257 for ( int ii
= 0 ; ii
< 15 ; ii
++){
1258 if ( rfLens
[ ii
] == rfCounter
){
1264 if ( rfCounter
> 0 && rfLensFnd
< 15 ){
1265 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1266 rfCnts
[ rfLensFnd
]++;
1267 rfLens
[ rfLensFnd
++] = rfCounter
;
1273 lastFCcnt
= fcCounter
;
1277 uint8_t rfHighest
= 15 , rfHighest2
= 15 , rfHighest3
= 15 ;
1279 for ( i
= 0 ; i
< 15 ; i
++){
1280 //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1281 //get highest 2 RF values (might need to get more values to compare or compare all?)
1282 if ( rfCnts
[ i
]> rfCnts
[ rfHighest
]){
1283 rfHighest3
= rfHighest2
;
1284 rfHighest2
= rfHighest
;
1286 } else if ( rfCnts
[ i
]> rfCnts
[ rfHighest2
]){
1287 rfHighest3
= rfHighest2
;
1289 } else if ( rfCnts
[ i
]> rfCnts
[ rfHighest3
]){
1293 // set allowed clock remainder tolerance to be 1 large field clock length+1
1294 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1295 uint8_t tol1
= fcHigh
+ 1 ;
1297 //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1299 // loop to find the highest clock that has a remainder less than the tolerance
1300 // compare samples counted divided by
1302 for (; ii
>= 0 ; ii
--){
1303 if ( rfLens
[ rfHighest
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest
] % clk
[ ii
] > clk
[ ii
]- tol1
){
1304 if ( rfLens
[ rfHighest2
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest2
] % clk
[ ii
] > clk
[ ii
]- tol1
){
1305 if ( rfLens
[ rfHighest3
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest3
] % clk
[ ii
] > clk
[ ii
]- tol1
){
1312 if ( ii
< 0 ) return 0 ; // oops we went too far
1318 //countFC is to detect the field clock lengths.
1319 //counts and returns the 2 most common wave lengths
1320 //mainly used for FSK field clock detection
1321 uint16_t countFC ( uint8_t * BitStream
, size_t size
, uint8_t fskAdj
)
1323 uint8_t fcLens
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1324 uint16_t fcCnts
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1325 uint8_t fcLensFnd
= 0 ;
1326 uint8_t lastFCcnt
= 0 ;
1327 uint8_t fcCounter
= 0 ;
1329 if ( size
== 0 ) return 0 ;
1331 // prime i to first up transition
1332 for ( i
= 1 ; i
< size
- 1 ; i
++)
1333 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
] >= BitStream
[ i
+ 1 ])
1336 for (; i
< size
- 1 ; i
++){
1337 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
] >= BitStream
[ i
+ 1 ]){
1338 // new up transition
1341 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1342 if ( lastFCcnt
== 5 && fcCounter
== 9 ) fcCounter
--;
1343 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
1344 if (( fcCounter
== 9 ) || fcCounter
== 4 ) fcCounter
++;
1345 // save last field clock count (fc/xx)
1346 lastFCcnt
= fcCounter
;
1348 // find which fcLens to save it to:
1349 for ( int ii
= 0 ; ii
< 10 ; ii
++){
1350 if ( fcLens
[ ii
]== fcCounter
){
1356 if ( fcCounter
> 0 && fcLensFnd
< 10 ){
1358 fcCnts
[ fcLensFnd
]++;
1359 fcLens
[ fcLensFnd
++]= fcCounter
;
1368 uint8_t best1
= 9 , best2
= 9 , best3
= 9 ;
1370 // go through fclens and find which ones are bigest 2
1371 for ( i
= 0 ; i
< 10 ; i
++){
1372 // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt);
1373 // get the 3 best FC values
1374 if ( fcCnts
[ i
]> maxCnt1
) {
1379 } else if ( fcCnts
[ i
]> fcCnts
[ best2
]){
1382 } else if ( fcCnts
[ i
]> fcCnts
[ best3
]){
1386 uint8_t fcH
= 0 , fcL
= 0 ;
1387 if ( fcLens
[ best1
]> fcLens
[ best2
]){
1395 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1397 uint16_t fcs
= ((( uint16_t ) fcH
)<< 8 ) | fcL
;
1398 // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
1399 if ( fskAdj
) return fcs
;
1400 return fcLens
[ best1
];
1403 //by marshmellow - demodulate PSK1 wave
1404 //uses wave lengths (# Samples)
1405 int pskRawDemod ( uint8_t dest
[], size_t * size
, int * clock
, int * invert
)
1407 if ( size
== 0 ) return - 1 ;
1408 uint16_t loopCnt
= 4096 ; //don't need to loop through entire array...
1409 if (* size
< loopCnt
) loopCnt
= * size
;
1411 uint8_t curPhase
= * invert
;
1412 size_t i
, waveStart
= 1 , waveEnd
= 0 , firstFullWave
= 0 , lastClkBit
= 0 ;
1413 uint8_t fc
= 0 , fullWaveLen
= 0 , tol
= 1 ;
1414 uint16_t errCnt
= 0 , waveLenCnt
= 0 ;
1415 fc
= countFC ( dest
, * size
, 0 );
1416 if ( fc
!= 2 && fc
!= 4 && fc
!= 8 ) return - 1 ;
1417 //PrintAndLog("DEBUG: FC: %d",fc);
1418 * clock
= DetectPSKClock ( dest
, * size
, * clock
);
1419 if (* clock
== 0 ) return - 1 ;
1420 int avgWaveVal
= 0 , lastAvgWaveVal
= 0 ;
1421 //find first phase shift
1422 for ( i
= 0 ; i
< loopCnt
; i
++){
1423 if ( dest
[ i
]+ fc
< dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
1425 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1426 waveLenCnt
= waveEnd
- waveStart
;
1427 if ( waveLenCnt
> fc
&& waveStart
> fc
){ //not first peak and is a large wave
1428 lastAvgWaveVal
= avgWaveVal
/( waveLenCnt
);
1429 firstFullWave
= waveStart
;
1430 fullWaveLen
= waveLenCnt
;
1431 //if average wave value is > graph 0 then it is an up wave or a 1
1432 if ( lastAvgWaveVal
> 123 ) curPhase
^= 1 ; //fudge graph 0 a little 123 vs 128
1438 avgWaveVal
+= dest
[ i
+ 2 ];
1440 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1441 lastClkBit
= firstFullWave
; //set start of wave as clock align
1442 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1446 memset ( dest
, curPhase
^ 1 , firstFullWave
/ * clock
);
1447 numBits
+= ( firstFullWave
/ * clock
);
1448 dest
[ numBits
++] = curPhase
; //set first read bit
1449 for ( i
= firstFullWave
+ fullWaveLen
- 1 ; i
< * size
- 3 ; i
++){
1450 //top edge of wave = start of new wave
1451 if ( dest
[ i
]+ fc
< dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
1452 if ( waveStart
== 0 ) {
1455 avgWaveVal
= dest
[ i
+ 1 ];
1458 waveLenCnt
= waveEnd
- waveStart
;
1459 lastAvgWaveVal
= avgWaveVal
/ waveLenCnt
;
1460 if ( waveLenCnt
> fc
){
1461 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1462 //this wave is a phase shift
1463 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1464 if ( i
+ 1 >= lastClkBit
+ * clock
- tol
){ //should be a clock bit
1466 dest
[ numBits
++] = curPhase
;
1467 lastClkBit
+= * clock
;
1468 } else if ( i
< lastClkBit
+ 10 + fc
){
1469 //noise after a phase shift - ignore
1470 } else { //phase shift before supposed to based on clock
1472 dest
[ numBits
++] = 7 ;
1474 } else if ( i
+ 1 > lastClkBit
+ * clock
+ tol
+ fc
){
1475 lastClkBit
+= * clock
; //no phase shift but clock bit
1476 dest
[ numBits
++] = curPhase
;
1482 avgWaveVal
+= dest
[ i
+ 1 ];