]>
git.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
7297c4e69835f3ab491a3b9a58776adad91f2c5c
1 //-----------------------------------------------------------------------------
4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
5 // at your option, any later version. See the LICENSE.txt file for the text of
7 //-----------------------------------------------------------------------------
8 // Low frequency demod/decode commands
9 //-----------------------------------------------------------------------------
16 /* //un_comment to allow debug print calls when used not on device
17 void dummy(char *fmt, ...){}
21 #define prnt PrintAndLog
28 uint8_t justNoise ( uint8_t * BitStream
, size_t size
)
30 static const uint8_t THRESHOLD
= 123 ;
31 //test samples are not just noise
32 uint8_t justNoise1
= 1 ;
33 for ( size_t idx
= 0 ; idx
< size
&& justNoise1
; idx
++){
34 justNoise1
= BitStream
[ idx
] < THRESHOLD
;
40 //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
41 int getHiLo ( uint8_t * BitStream
, size_t size
, int * high
, int * low
, uint8_t fuzzHi
, uint8_t fuzzLo
)
45 // get high and low thresholds
46 for ( size_t i
= 0 ; i
< size
; i
++){
47 if ( BitStream
[ i
] > * high
) * high
= BitStream
[ i
];
48 if ( BitStream
[ i
] < * low
) * low
= BitStream
[ i
];
50 if (* high
< 123 ) return - 1 ; // just noise
51 * high
= ((* high
- 128 )* fuzzHi
+ 12800 )/ 100 ;
52 * low
= ((* low
- 128 )* fuzzLo
+ 12800 )/ 100 ;
57 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
58 // returns 1 if passed
59 uint8_t parityTest ( uint32_t bits
, uint8_t bitLen
, uint8_t pType
)
62 for ( uint8_t i
= 0 ; i
< bitLen
; i
++){
63 ans
^= (( bits
>> i
) & 1 );
65 //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType);
66 return ( ans
== pType
);
70 //search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
71 uint8_t preambleSearch ( uint8_t * BitStream
, uint8_t * preamble
, size_t pLen
, size_t * size
, size_t * startIdx
)
74 for ( int idx
= 0 ; idx
< * size
- pLen
; idx
++){
75 if ( memcmp ( BitStream
+ idx
, preamble
, pLen
) == 0 ){
82 * size
= idx
- * startIdx
;
91 //takes 1s and 0s and searches for EM410x format - output EM ID
92 uint8_t Em410xDecode ( uint8_t * BitStream
, size_t * size
, size_t * startIdx
, uint32_t * hi
, uint64_t * lo
)
94 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
95 // otherwise could be a void with no arguments
98 if ( BitStream
[ 1 ]> 1 ) return 0 ; //allow only 1s and 0s
100 // 111111111 bit pattern represent start of frame
101 // include 0 in front to help get start pos
102 uint8_t preamble
[] = { 0 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 };
104 uint32_t parityBits
= 0 ;
108 errChk
= preambleSearch ( BitStream
, preamble
, sizeof ( preamble
), size
, startIdx
);
109 if ( errChk
== 0 || * size
< 64 ) return 0 ;
110 if (* size
> 64 ) FmtLen
= 22 ;
111 * startIdx
+= 1 ; //get rid of 0 from preamble
113 for ( i
= 0 ; i
< FmtLen
; i
++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
114 parityBits
= bytebits_to_byte ( BitStream
+( i
* 5 )+ idx
, 5 );
115 //check even parity - quit if failed
116 if ( parityTest ( parityBits
, 5 , 0 ) == 0 ) return 0 ;
117 //set uint64 with ID from BitStream
118 for ( uint8_t ii
= 0 ; ii
< 4 ; ii
++){
119 * hi
= (* hi
<< 1 ) | (* lo
>> 63 );
120 * lo
= (* lo
<< 1 ) | ( BitStream
[( i
* 5 )+ ii
+ idx
]);
123 if ( errChk
!= 0 ) return 1 ;
124 //skip last 5 bit parity test for simplicity.
130 //demodulates strong heavily clipped samples
131 int cleanAskRawDemod ( uint8_t * BinStream
, size_t * size
, int clk
, int invert
, int high
, int low
)
133 size_t bitCnt
= 0 , smplCnt
= 0 , errCnt
= 0 ;
134 uint8_t waveHigh
= 0 ;
135 for ( size_t i
= 0 ; i
< * size
; i
++){
136 if ( BinStream
[ i
] >= high
&& waveHigh
){
138 } else if ( BinStream
[ i
] <= low
&& ! waveHigh
){
140 } else { //transition
141 if (( BinStream
[ i
] >= high
&& ! waveHigh
) || ( BinStream
[ i
] <= low
&& waveHigh
)){
142 if ( smplCnt
> clk
-( clk
/ 4 )- 1 ) { //full clock
143 if ( smplCnt
> clk
+ ( clk
/ 4 )+ 1 ) { //too many samples
145 BinStream
[ bitCnt
++]= 7 ;
146 } else if ( waveHigh
) {
147 BinStream
[ bitCnt
++] = invert
;
148 BinStream
[ bitCnt
++] = invert
;
149 } else if (! waveHigh
) {
150 BinStream
[ bitCnt
++] = invert
^ 1 ;
151 BinStream
[ bitCnt
++] = invert
^ 1 ;
155 } else if ( smplCnt
> ( clk
/ 2 ) - ( clk
/ 4 )- 1 ) {
157 BinStream
[ bitCnt
++] = invert
;
158 } else if (! waveHigh
) {
159 BinStream
[ bitCnt
++] = invert
^ 1 ;
163 } else if (! bitCnt
) {
165 waveHigh
= ( BinStream
[ i
] >= high
);
169 //transition bit oops
171 } else { //haven't hit new high or new low yet
181 void askAmp ( uint8_t * BitStream
, size_t size
)
183 for ( size_t i
= 1 ; i
< size
; i
++){
184 if ( BitStream
[ i
]- BitStream
[ i
- 1 ]>= 30 ) //large jump up
186 else if ( BitStream
[ i
]- BitStream
[ i
- 1 ]<=- 20 ) //large jump down
193 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
194 int askdemod ( uint8_t * BinStream
, size_t * size
, int * clk
, int * invert
, int maxErr
, uint8_t amp
, uint8_t askType
)
196 if (* size
== 0 ) return - 1 ;
197 int start
= DetectASKClock ( BinStream
, * size
, clk
, maxErr
); //clock default
198 if (* clk
== 0 || start
< 0 ) return - 3 ;
199 if (* invert
!= 1 ) * invert
= 0 ;
200 if ( amp
== 1 ) askAmp ( BinStream
, * size
);
202 uint8_t initLoopMax
= 255 ;
203 if ( initLoopMax
> * size
) initLoopMax
= * size
;
204 // Detect high and lows
205 //25% clip in case highs and lows aren't clipped [marshmellow]
207 if ( getHiLo ( BinStream
, initLoopMax
, & high
, & low
, 75 , 75 ) < 1 )
208 return - 2 ; //just noise
211 // if clean clipped waves detected run alternate demod
212 if ( DetectCleanAskWave ( BinStream
, * size
, high
, low
)) {
213 errCnt
= cleanAskRawDemod ( BinStream
, size
, * clk
, * invert
, high
, low
);
214 if ( askType
) //askman
215 return manrawdecode ( BinStream
, size
, 0 );
220 int lastBit
; //set first clock check - can go negative
221 size_t i
, bitnum
= 0 ; //output counter
223 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
224 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
225 size_t MaxBits
= 1024 ;
226 lastBit
= start
- * clk
;
228 for ( i
= start
; i
< * size
; ++ i
) {
229 if ( i
- lastBit
>= * clk
- tol
){
230 if ( BinStream
[ i
] >= high
) {
231 BinStream
[ bitnum
++] = * invert
;
232 } else if ( BinStream
[ i
] <= low
) {
233 BinStream
[ bitnum
++] = * invert
^ 1 ;
234 } else if ( i
- lastBit
>= * clk
+ tol
) {
236 BinStream
[ bitnum
++]= 7 ;
239 } else { //in tolerance - looking for peak
244 } else if ( i
- lastBit
>= (* clk
/ 2 - tol
) && ! midBit
&& ! askType
){
245 if ( BinStream
[ i
] >= high
) {
246 BinStream
[ bitnum
++] = * invert
;
247 } else if ( BinStream
[ i
] <= low
) {
248 BinStream
[ bitnum
++] = * invert
^ 1 ;
249 } else if ( i
- lastBit
>= * clk
/ 2 + tol
) {
250 BinStream
[ bitnum
] = BinStream
[ bitnum
- 1 ];
252 } else { //in tolerance - looking for peak
257 if ( bitnum
>= MaxBits
) break ;
264 //take 10 and 01 and manchester decode
265 //run through 2 times and take least errCnt
266 int manrawdecode ( uint8_t * BitStream
, size_t * size
, uint8_t invert
)
268 uint16_t bitnum
= 0 , MaxBits
= 512 , errCnt
= 0 ;
270 uint16_t bestErr
= 1000 , bestRun
= 0 ;
271 if (* size
< 16 ) return - 1 ;
272 //find correct start position [alignment]
273 for ( ii
= 0 ; ii
< 2 ;++ ii
){
274 for ( i
= ii
; i
<* size
- 3 ; i
+= 2 )
275 if ( BitStream
[ i
]== BitStream
[ i
+ 1 ])
285 for ( i
= bestRun
; i
< * size
- 3 ; i
+= 2 ){
286 if ( BitStream
[ i
] == 1 && ( BitStream
[ i
+ 1 ] == 0 )){
287 BitStream
[ bitnum
++]= invert
;
288 } else if (( BitStream
[ i
] == 0 ) && BitStream
[ i
+ 1 ] == 1 ){
289 BitStream
[ bitnum
++]= invert
^ 1 ;
291 BitStream
[ bitnum
++]= 7 ;
293 if ( bitnum
> MaxBits
) break ;
299 uint32_t manchesterEncode2Bytes ( uint16_t datain
) {
302 for ( uint8_t i
= 0 ; i
< 16 ; i
++) {
303 curBit
= ( datain
>> ( 15 - i
) & 1 );
304 output
|= ( 1 <<((( 15 - i
)* 2 )+ curBit
));
310 //encode binary data into binary manchester
311 int ManchesterEncode ( uint8_t * BitStream
, size_t size
)
313 size_t modIdx
= 20000 , i
= 0 ;
314 if ( size
> modIdx
) return - 1 ;
315 for ( size_t idx
= 0 ; idx
< size
; idx
++){
316 BitStream
[ idx
+ modIdx
++] = BitStream
[ idx
];
317 BitStream
[ idx
+ modIdx
++] = BitStream
[ idx
]^ 1 ;
319 for (; i
<( size
* 2 ); i
++){
320 BitStream
[ i
] = BitStream
[ i
+ 20000 ];
326 //take 01 or 10 = 1 and 11 or 00 = 0
327 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
328 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
329 int BiphaseRawDecode ( uint8_t * BitStream
, size_t * size
, int offset
, int invert
)
334 uint16_t MaxBits
= 512 ;
335 //if not enough samples - error
336 if (* size
< 51 ) return - 1 ;
337 //check for phase change faults - skip one sample if faulty
338 uint8_t offsetA
= 1 , offsetB
= 1 ;
340 if ( BitStream
[ i
+ 1 ]== BitStream
[ i
+ 2 ]) offsetA
= 0 ;
341 if ( BitStream
[ i
+ 2 ]== BitStream
[ i
+ 3 ]) offsetB
= 0 ;
343 if (! offsetA
&& offsetB
) offset
++;
344 for ( i
= offset
; i
<* size
- 3 ; i
+= 2 ){
345 //check for phase error
346 if ( BitStream
[ i
+ 1 ]== BitStream
[ i
+ 2 ]) {
347 BitStream
[ bitnum
++]= 7 ;
350 if (( BitStream
[ i
]== 1 && BitStream
[ i
+ 1 ]== 0 ) || ( BitStream
[ i
]== 0 && BitStream
[ i
+ 1 ]== 1 )){
351 BitStream
[ bitnum
++]= 1 ^ invert
;
352 } else if (( BitStream
[ i
]== 0 && BitStream
[ i
+ 1 ]== 0 ) || ( BitStream
[ i
]== 1 && BitStream
[ i
+ 1 ]== 1 )){
353 BitStream
[ bitnum
++]= invert
;
355 BitStream
[ bitnum
++]= 7 ;
358 if ( bitnum
> MaxBits
) break ;
365 // demod gProxIIDemod
366 // error returns as -x
367 // success returns start position in BitStream
368 // BitStream must contain previously askrawdemod and biphasedemoded data
369 int gProxII_Demod ( uint8_t BitStream
[], size_t * size
)
372 uint8_t preamble
[] = { 1 , 1 , 1 , 1 , 1 , 0 };
374 uint8_t errChk
= preambleSearch ( BitStream
, preamble
, sizeof ( preamble
), size
, & startIdx
);
375 if ( errChk
== 0 ) return - 3 ; //preamble not found
376 if (* size
!= 96 ) return - 2 ; //should have found 96 bits
377 //check first 6 spacer bits to verify format
378 if (! BitStream
[ startIdx
+ 5 ] && ! BitStream
[ startIdx
+ 10 ] && ! BitStream
[ startIdx
+ 15 ] && ! BitStream
[ startIdx
+ 20 ] && ! BitStream
[ startIdx
+ 25 ] && ! BitStream
[ startIdx
+ 30 ]){
379 //confirmed proper separator bits found
380 //return start position
381 return ( int ) startIdx
;
386 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
387 size_t fsk_wave_demod ( uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
389 size_t last_transition
= 0 ;
392 if ( fchigh
== 0 ) fchigh
= 10 ;
393 if ( fclow
== 0 ) fclow
= 8 ;
394 //set the threshold close to 0 (graph) or 128 std to avoid static
395 uint8_t threshold_value
= 123 ;
396 size_t preLastSample
= 0 ;
397 size_t LastSample
= 0 ;
398 size_t currSample
= 0 ;
399 // sync to first lo-hi transition, and threshold
401 // Need to threshold first sample
402 // skip 160 samples to allow antenna/samples to settle
403 if ( dest
[ 160 ] < threshold_value
) dest
[ 0 ] = 0 ;
407 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
408 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
409 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
410 for ( idx
= 161 ; idx
< size
- 20 ; idx
++) {
411 // threshold current value
413 if ( dest
[ idx
] < threshold_value
) dest
[ idx
] = 0 ;
416 // Check for 0->1 transition
417 if ( dest
[ idx
- 1 ] < dest
[ idx
]) { // 0 -> 1 transition
418 preLastSample
= LastSample
;
419 LastSample
= currSample
;
420 currSample
= idx
- last_transition
;
421 if ( currSample
< ( fclow
- 2 )){ //0-5 = garbage noise (or 0-3)
422 //do nothing with extra garbage
423 } else if ( currSample
< ( fchigh
- 1 )) { //6-8 = 8 sample waves or 3-6 = 5
424 if ( LastSample
> ( fchigh
- 2 ) && ( preLastSample
< ( fchigh
- 1 ) || preLastSample
== 0 )){
425 dest
[ numBits
- 1 ]= 1 ; //correct previous 9 wave surrounded by 8 waves
429 } else if ( currSample
> ( fchigh
) && ! numBits
) { //12 + and first bit = garbage
430 //do nothing with beginning garbage
431 } else if ( currSample
== ( fclow
+ 1 ) && LastSample
== ( fclow
- 1 )) { // had a 7 then a 9 should be two 8's
433 } else { //9+ = 10 sample waves
436 last_transition
= idx
;
439 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
442 //translate 11111100000 to 10
443 size_t aggregate_bits ( uint8_t * dest
, size_t size
, uint8_t rfLen
,
444 uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
446 uint8_t lastval
= dest
[ 0 ];
450 for ( idx
= 1 ; idx
< size
; idx
++) {
452 if ( dest
[ idx
]== lastval
) continue ;
454 //if lastval was 1, we have a 1->0 crossing
455 if ( dest
[ idx
- 1 ]== 1 ) {
456 n
= ( n
* fclow
+ rfLen
/ 2 ) / rfLen
;
457 } else { // 0->1 crossing
458 n
= ( n
* fchigh
+ rfLen
/ 2 ) / rfLen
;
462 memset ( dest
+ numBits
, dest
[ idx
- 1 ]^ invert
, n
);
467 // if valid extra bits at the end were all the same frequency - add them in
468 if ( n
> rfLen
/ fchigh
) {
469 if ( dest
[ idx
- 2 ]== 1 ) {
470 n
= ( n
* fclow
+ rfLen
/ 2 ) / rfLen
;
472 n
= ( n
* fchigh
+ rfLen
/ 2 ) / rfLen
;
474 memset ( dest
+ numBits
, dest
[ idx
- 1 ]^ invert
, n
);
480 //by marshmellow (from holiman's base)
481 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
482 int fskdemod ( uint8_t * dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
485 size
= fsk_wave_demod ( dest
, size
, fchigh
, fclow
);
486 size
= aggregate_bits ( dest
, size
, rfLen
, invert
, fchigh
, fclow
);
490 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
491 int HIDdemodFSK ( uint8_t * dest
, size_t * size
, uint32_t * hi2
, uint32_t * hi
, uint32_t * lo
)
493 if ( justNoise ( dest
, * size
)) return - 1 ;
495 size_t numStart
= 0 , size2
=* size
, startIdx
= 0 ;
497 * size
= fskdemod ( dest
, size2
, 50 , 1 , 10 , 8 ); //fsk2a
498 if (* size
< 96 * 2 ) return - 2 ;
499 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
500 uint8_t preamble
[] = { 0 , 0 , 0 , 1 , 1 , 1 , 0 , 1 };
501 // find bitstring in array
502 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
503 if ( errChk
== 0 ) return - 3 ; //preamble not found
505 numStart
= startIdx
+ sizeof ( preamble
);
506 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
507 for ( size_t idx
= numStart
; ( idx
- numStart
) < * size
- sizeof ( preamble
); idx
+= 2 ){
508 if ( dest
[ idx
] == dest
[ idx
+ 1 ]){
509 return - 4 ; //not manchester data
511 * hi2
= (* hi2
<< 1 )|(* hi
>> 31 );
512 * hi
= (* hi
<< 1 )|(* lo
>> 31 );
513 //Then, shift in a 0 or one into low
514 if ( dest
[ idx
] && ! dest
[ idx
+ 1 ]) // 1 0
519 return ( int ) startIdx
;
522 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
523 int ParadoxdemodFSK ( uint8_t * dest
, size_t * size
, uint32_t * hi2
, uint32_t * hi
, uint32_t * lo
)
525 if ( justNoise ( dest
, * size
)) return - 1 ;
527 size_t numStart
= 0 , size2
=* size
, startIdx
= 0 ;
529 * size
= fskdemod ( dest
, size2
, 50 , 1 , 10 , 8 ); //fsk2a
530 if (* size
< 96 ) return - 2 ;
532 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
533 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 1 , 1 , 1 , 1 };
535 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
536 if ( errChk
== 0 ) return - 3 ; //preamble not found
538 numStart
= startIdx
+ sizeof ( preamble
);
539 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
540 for ( size_t idx
= numStart
; ( idx
- numStart
) < * size
- sizeof ( preamble
); idx
+= 2 ){
541 if ( dest
[ idx
] == dest
[ idx
+ 1 ])
542 return - 4 ; //not manchester data
543 * hi2
= (* hi2
<< 1 )|(* hi
>> 31 );
544 * hi
= (* hi
<< 1 )|(* lo
>> 31 );
545 //Then, shift in a 0 or one into low
546 if ( dest
[ idx
] && ! dest
[ idx
+ 1 ]) // 1 0
551 return ( int ) startIdx
;
554 uint32_t bytebits_to_byte ( uint8_t * src
, size_t numbits
)
557 for ( int i
= 0 ; i
< numbits
; i
++)
559 num
= ( num
<< 1 ) | (* src
);
565 //least significant bit first
566 uint32_t bytebits_to_byteLSBF ( uint8_t * src
, size_t numbits
)
569 for ( int i
= 0 ; i
< numbits
; i
++)
571 num
= ( num
<< 1 ) | *( src
+ ( numbits
-( i
+ 1 )));
576 int IOdemodFSK ( uint8_t * dest
, size_t size
)
578 if ( justNoise ( dest
, size
)) return - 1 ;
579 //make sure buffer has data
580 if ( size
< 66 * 64 ) return - 2 ;
582 size
= fskdemod ( dest
, size
, 64 , 1 , 10 , 8 ); // FSK2a RF/64
583 if ( size
< 65 ) return - 3 ; //did we get a good demod?
585 //0 10 20 30 40 50 60
587 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
588 //-----------------------------------------------------------------------------
589 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
591 //XSF(version)facility:codeone+codetwo
594 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
595 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), & size
, & startIdx
);
596 if ( errChk
== 0 ) return - 4 ; //preamble not found
598 if (! dest
[ startIdx
+ 8 ] && dest
[ startIdx
+ 17 ]== 1 && dest
[ startIdx
+ 26 ]== 1 && dest
[ startIdx
+ 35 ]== 1 && dest
[ startIdx
+ 44 ]== 1 && dest
[ startIdx
+ 53 ]== 1 ){
599 //confirmed proper separator bits found
600 //return start position
601 return ( int ) startIdx
;
607 // find viking preamble 0xF200 in already demoded data
608 int VikingDemod_AM ( uint8_t * dest
, size_t * size
) {
609 //make sure buffer has data
610 if (* size
< 64 * 2 ) return - 2 ;
613 uint8_t preamble
[] = { 1 , 1 , 1 , 1 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
614 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
615 if ( errChk
== 0 ) return - 4 ; //preamble not found
616 uint32_t checkCalc
= bytebits_to_byte ( dest
+ startIdx
, 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 8 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 16 , 8 )
617 ^ bytebits_to_byte ( dest
+ startIdx
+ 24 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 32 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 40 , 8 )
618 ^ bytebits_to_byte ( dest
+ startIdx
+ 48 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 56 , 8 );
619 if ( checkCalc
!= 0xA8 ) return - 5 ;
620 if (* size
!= 64 ) return - 6 ;
621 //return start position
622 return ( int ) startIdx
;
626 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
627 // Parity Type (1 for odd; 0 for even; 2 Always 1's), and binary Length (length to run)
628 size_t removeParity ( uint8_t * BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
)
630 uint32_t parityWd
= 0 ;
631 size_t j
= 0 , bitCnt
= 0 ;
632 for ( int word
= 0 ; word
< ( bLen
); word
+= pLen
){
633 for ( int bit
= 0 ; bit
< pLen
; bit
++){
634 parityWd
= ( parityWd
<< 1 ) | BitStream
[ startIdx
+ word
+ bit
];
635 BitStream
[ j
++] = ( BitStream
[ startIdx
+ word
+ bit
]);
637 j
--; // overwrite parity with next data
638 // if parity fails then return 0
639 if ( pType
== 2 ) { // then marker bit which should be a 1
640 if (! BitStream
[ j
]) return 0 ;
642 if ( parityTest ( parityWd
, pLen
, pType
) == 0 ) return 0 ;
647 // if we got here then all the parities passed
648 //return ID start index and size
652 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
653 // BitStream must contain previously askrawdemod and biphasedemoded data
654 int FDXBdemodBI ( uint8_t * dest
, size_t * size
)
656 //make sure buffer has enough data
657 if (* size
< 128 ) return - 1 ;
660 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
662 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
663 if ( errChk
== 0 ) return - 2 ; //preamble not found
664 return ( int ) startIdx
;
668 // FSK Demod then try to locate an AWID ID
669 int AWIDdemodFSK ( uint8_t * dest
, size_t * size
)
671 //make sure buffer has enough data
672 if (* size
< 96 * 50 ) return - 1 ;
674 if ( justNoise ( dest
, * size
)) return - 2 ;
677 * size
= fskdemod ( dest
, * size
, 50 , 1 , 10 , 8 ); // fsk2a RF/50
678 if (* size
< 96 ) return - 3 ; //did we get a good demod?
680 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
682 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
683 if ( errChk
== 0 ) return - 4 ; //preamble not found
684 if (* size
!= 96 ) return - 5 ;
685 return ( int ) startIdx
;
689 // FSK Demod then try to locate a Farpointe Data (pyramid) ID
690 int PyramiddemodFSK ( uint8_t * dest
, size_t * size
)
692 //make sure buffer has data
693 if (* size
< 128 * 50 ) return - 5 ;
695 //test samples are not just noise
696 if ( justNoise ( dest
, * size
)) return - 1 ;
699 * size
= fskdemod ( dest
, * size
, 50 , 1 , 10 , 8 ); // fsk2a RF/50
700 if (* size
< 128 ) return - 2 ; //did we get a good demod?
702 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
704 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
705 if ( errChk
== 0 ) return - 4 ; //preamble not found
706 if (* size
!= 128 ) return - 3 ;
707 return ( int ) startIdx
;
711 // to detect a wave that has heavily clipped (clean) samples
712 uint8_t DetectCleanAskWave ( uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
)
714 bool allArePeaks
= true ;
716 size_t loopEnd
= 512 + 160 ;
717 if ( loopEnd
> size
) loopEnd
= size
;
718 for ( size_t i
= 160 ; i
< loopEnd
; i
++){
719 if ( dest
[ i
]> low
&& dest
[ i
]< high
)
725 if ( cntPeaks
> 300 ) return true ;
730 // to help detect clocks on heavily clipped samples
731 // based on count of low to low
732 int DetectStrongAskClock ( uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
)
734 uint8_t fndClk
[] = { 8 , 16 , 32 , 40 , 50 , 64 , 128 };
738 // get to first full low to prime loop and skip incomplete first pulse
739 while (( dest
[ i
] < high
) && ( i
< size
))
741 while (( dest
[ i
] > low
) && ( i
< size
))
744 // loop through all samples
746 // measure from low to low
747 while (( dest
[ i
] > low
) && ( i
< size
))
750 while (( dest
[ i
] < high
) && ( i
< size
))
752 while (( dest
[ i
] > low
) && ( i
< size
))
754 //get minimum measured distance
755 if ( i
- startwave
< minClk
&& i
< size
)
756 minClk
= i
- startwave
;
759 //prnt("minClk: %d",minClk);
760 for ( uint8_t clkCnt
= 0 ; clkCnt
< 7 ; clkCnt
++) {
761 if ( minClk
>= fndClk
[ clkCnt
]-( fndClk
[ clkCnt
]/ 8 ) && minClk
<= fndClk
[ clkCnt
]+ 1 )
762 return fndClk
[ clkCnt
];
768 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
769 // maybe somehow adjust peak trimming value based on samples to fix?
770 // return start index of best starting position for that clock and return clock (by reference)
771 int DetectASKClock ( uint8_t dest
[], size_t size
, int * clock
, int maxErr
)
774 uint8_t clk
[] = { 255 , 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 };
776 uint8_t loopCnt
= 255 ; //don't need to loop through entire array...
777 if ( size
<= loopCnt
+ 60 ) return - 1 ; //not enough samples
778 size
-= 60 ; //sometimes there is a strange end wave - filter out this....
779 //if we already have a valid clock
782 if ( clk
[ i
] == * clock
) clockFnd
= i
;
783 //clock found but continue to find best startpos
785 //get high and low peak
787 if ( getHiLo ( dest
, loopCnt
, & peak
, & low
, 75 , 75 ) < 1 ) return - 1 ;
789 //test for large clean peaks
791 if ( DetectCleanAskWave ( dest
, size
, peak
, low
)== 1 ){
792 int ans
= DetectStrongAskClock ( dest
, size
, peak
, low
);
793 for ( i
= clkEnd
- 1 ; i
> 0 ; i
--){
797 return 0 ; // for strong waves i don't use the 'best start position' yet...
798 //break; //clock found but continue to find best startpos [not yet]
804 uint8_t clkCnt
, tol
= 0 ;
805 uint16_t bestErr
[]={ 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 };
806 uint8_t bestStart
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
808 size_t arrLoc
, loopEnd
;
816 //test each valid clock from smallest to greatest to see which lines up
817 for (; clkCnt
< clkEnd
; clkCnt
++){
818 if ( clk
[ clkCnt
] <= 32 ){
823 //if no errors allowed - keep start within the first clock
824 if (! maxErr
&& size
> clk
[ clkCnt
]* 2 + tol
&& clk
[ clkCnt
]< 128 ) loopCnt
= clk
[ clkCnt
]* 2 ;
825 bestErr
[ clkCnt
]= 1000 ;
826 //try lining up the peaks by moving starting point (try first few clocks)
827 for ( ii
= 0 ; ii
< loopCnt
; ii
++){
828 if ( dest
[ ii
] < peak
&& dest
[ ii
] > low
) continue ;
831 // now that we have the first one lined up test rest of wave array
832 loopEnd
= (( size
- ii
- tol
) / clk
[ clkCnt
]) - 1 ;
833 for ( i
= 0 ; i
< loopEnd
; ++ i
){
834 arrLoc
= ii
+ ( i
* clk
[ clkCnt
]);
835 if ( dest
[ arrLoc
] >= peak
|| dest
[ arrLoc
] <= low
){
836 } else if ( dest
[ arrLoc
- tol
] >= peak
|| dest
[ arrLoc
- tol
] <= low
){
837 } else if ( dest
[ arrLoc
+ tol
] >= peak
|| dest
[ arrLoc
+ tol
] <= low
){
838 } else { //error no peak detected
842 //if we found no errors then we can stop here and a low clock (common clocks)
843 // this is correct one - return this clock
844 //prnt("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
845 if ( errCnt
== 0 && clkCnt
< 7 ) {
846 if (! clockFnd
) * clock
= clk
[ clkCnt
];
849 //if we found errors see if it is lowest so far and save it as best run
850 if ( errCnt
< bestErr
[ clkCnt
]){
851 bestErr
[ clkCnt
]= errCnt
;
852 bestStart
[ clkCnt
]= ii
;
858 for ( iii
= 1 ; iii
< clkEnd
; ++ iii
){
859 if ( bestErr
[ iii
] < bestErr
[ best
]){
860 if ( bestErr
[ iii
] == 0 ) bestErr
[ iii
]= 1 ;
861 // current best bit to error ratio vs new bit to error ratio
862 if ( ( size
/ clk
[ best
])/ bestErr
[ best
] < ( size
/ clk
[ iii
])/ bestErr
[ iii
] ){
867 //if (bestErr[best] > maxErr) return -1;
868 if (! clockFnd
) * clock
= clk
[ best
];
869 return bestStart
[ best
];
873 //detect psk clock by reading each phase shift
874 // a phase shift is determined by measuring the sample length of each wave
875 int DetectPSKClock ( uint8_t dest
[], size_t size
, int clock
)
877 uint8_t clk
[]={ 255 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 }; //255 is not a valid clock
878 uint16_t loopCnt
= 4096 ; //don't need to loop through entire array...
879 if ( size
== 0 ) return 0 ;
880 if ( size
< loopCnt
) loopCnt
= size
- 20 ;
882 //if we already have a valid clock quit
885 if ( clk
[ i
] == clock
) return clock
;
887 size_t waveStart
= 0 , waveEnd
= 0 , firstFullWave
= 0 , lastClkBit
= 0 ;
888 uint8_t clkCnt
, fc
= 0 , fullWaveLen
= 0 , tol
= 1 ;
889 uint16_t peakcnt
= 0 , errCnt
= 0 , waveLenCnt
= 0 ;
890 uint16_t bestErr
[]={ 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 };
891 uint16_t peaksdet
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
892 fc
= countFC ( dest
, size
, 0 );
893 if ( fc
!= 2 && fc
!= 4 && fc
!= 8 ) return - 1 ;
894 //prnt("DEBUG: FC: %d",fc);
896 //find first full wave
897 for ( i
= 160 ; i
< loopCnt
; i
++){
898 if ( dest
[ i
] < dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
899 if ( waveStart
== 0 ) {
901 //prnt("DEBUG: waveStart: %d",waveStart);
904 //prnt("DEBUG: waveEnd: %d",waveEnd);
905 waveLenCnt
= waveEnd
- waveStart
;
906 if ( waveLenCnt
> fc
){
907 firstFullWave
= waveStart
;
908 fullWaveLen
= waveLenCnt
;
915 //prnt("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
917 //test each valid clock from greatest to smallest to see which lines up
918 for ( clkCnt
= 7 ; clkCnt
>= 1 ; clkCnt
--){
919 lastClkBit
= firstFullWave
; //set end of wave as clock align
923 //prnt("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
925 for ( i
= firstFullWave
+ fullWaveLen
- 1 ; i
< loopCnt
- 2 ; i
++){
926 //top edge of wave = start of new wave
927 if ( dest
[ i
] < dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
928 if ( waveStart
== 0 ) {
933 waveLenCnt
= waveEnd
- waveStart
;
934 if ( waveLenCnt
> fc
){
935 //if this wave is a phase shift
936 //prnt("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc);
937 if ( i
+ 1 >= lastClkBit
+ clk
[ clkCnt
] - tol
){ //should be a clock bit
939 lastClkBit
+= clk
[ clkCnt
];
940 } else if ( i
< lastClkBit
+ 8 ){
941 //noise after a phase shift - ignore
942 } else { //phase shift before supposed to based on clock
945 } else if ( i
+ 1 > lastClkBit
+ clk
[ clkCnt
] + tol
+ fc
){
946 lastClkBit
+= clk
[ clkCnt
]; //no phase shift but clock bit
955 if ( errCnt
<= bestErr
[ clkCnt
]) bestErr
[ clkCnt
]= errCnt
;
956 if ( peakcnt
> peaksdet
[ clkCnt
]) peaksdet
[ clkCnt
]= peakcnt
;
958 //all tested with errors
959 //return the highest clk with the most peaks found
961 for ( i
= 7 ; i
>= 1 ; i
--){
962 if ( peaksdet
[ i
] > peaksdet
[ best
]) {
965 //prnt("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
970 int DetectStrongNRZClk ( uint8_t * dest
, size_t size
, int peak
, int low
){
971 //find shortest transition from high to low
973 size_t transition1
= 0 ;
974 int lowestTransition
= 255 ;
975 bool lastWasHigh
= false ;
977 //find first valid beginning of a high or low wave
978 while (( dest
[ i
] >= peak
|| dest
[ i
] <= low
) && ( i
< size
))
980 while (( dest
[ i
] < peak
&& dest
[ i
] > low
) && ( i
< size
))
982 lastWasHigh
= ( dest
[ i
] >= peak
);
984 if ( i
== size
) return 0 ;
987 for (; i
< size
; i
++) {
988 if (( dest
[ i
] >= peak
&& ! lastWasHigh
) || ( dest
[ i
] <= low
&& lastWasHigh
)) {
989 lastWasHigh
= ( dest
[ i
] >= peak
);
990 if ( i
- transition1
< lowestTransition
) lowestTransition
= i
- transition1
;
994 //prnt("DEBUG: LowestTrs: %d",lowestTransition);
995 if ( lowestTransition
== 255 ) lowestTransition
= 0 ;
996 return lowestTransition
;
1000 //detect nrz clock by reading #peaks vs no peaks(or errors)
1001 int DetectNRZClock ( uint8_t dest
[], size_t size
, int clock
)
1004 uint8_t clk
[]={ 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 };
1005 size_t loopCnt
= 4096 ; //don't need to loop through entire array...
1006 if ( size
== 0 ) return 0 ;
1007 if ( size
< loopCnt
) loopCnt
= size
- 20 ;
1008 //if we already have a valid clock quit
1010 if ( clk
[ i
] == clock
) return clock
;
1012 //get high and low peak
1014 if ( getHiLo ( dest
, loopCnt
, & peak
, & low
, 75 , 75 ) < 1 ) return 0 ;
1016 int lowestTransition
= DetectStrongNRZClk ( dest
, size
- 20 , peak
, low
);
1020 uint16_t smplCnt
= 0 ;
1021 int16_t peakcnt
= 0 ;
1022 int16_t peaksdet
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1023 uint16_t maxPeak
= 255 ;
1024 bool firstpeak
= false ;
1025 //test for large clipped waves
1026 for ( i
= 0 ; i
< loopCnt
; i
++){
1027 if ( dest
[ i
] >= peak
|| dest
[ i
] <= low
){
1028 if (! firstpeak
) continue ;
1033 if ( maxPeak
> smplCnt
){
1035 //prnt("maxPk: %d",maxPeak);
1038 //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt);
1043 bool errBitHigh
= 0 ;
1045 uint8_t ignoreCnt
= 0 ;
1046 uint8_t ignoreWindow
= 4 ;
1047 bool lastPeakHigh
= 0 ;
1050 //test each valid clock from smallest to greatest to see which lines up
1051 for ( clkCnt
= 0 ; clkCnt
< 8 ; ++ clkCnt
){
1052 //ignore clocks smaller than smallest peak
1053 if ( clk
[ clkCnt
] < maxPeak
- ( clk
[ clkCnt
]/ 4 )) continue ;
1054 //try lining up the peaks by moving starting point (try first 256)
1055 for ( ii
= 20 ; ii
< loopCnt
; ++ ii
){
1056 if (( dest
[ ii
] >= peak
) || ( dest
[ ii
] <= low
)){
1060 lastBit
= ii
- clk
[ clkCnt
];
1061 //loop through to see if this start location works
1062 for ( i
= ii
; i
< size
- 20 ; ++ i
) {
1063 //if we are at a clock bit
1064 if (( i
>= lastBit
+ clk
[ clkCnt
] - tol
) && ( i
<= lastBit
+ clk
[ clkCnt
] + tol
)) {
1066 if ( dest
[ i
] >= peak
|| dest
[ i
] <= low
) {
1067 //if same peak don't count it
1068 if (( dest
[ i
] >= peak
&& ! lastPeakHigh
) || ( dest
[ i
] <= low
&& lastPeakHigh
)) {
1071 lastPeakHigh
= ( dest
[ i
] >= peak
);
1074 ignoreCnt
= ignoreWindow
;
1075 lastBit
+= clk
[ clkCnt
];
1076 } else if ( i
== lastBit
+ clk
[ clkCnt
] + tol
) {
1077 lastBit
+= clk
[ clkCnt
];
1079 //else if not a clock bit and no peaks
1080 } else if ( dest
[ i
] < peak
&& dest
[ i
] > low
){
1083 if ( errBitHigh
== true ) peakcnt
--;
1088 // else if not a clock bit but we have a peak
1089 } else if (( dest
[ i
]>= peak
|| dest
[ i
]<= low
) && (! bitHigh
)) {
1090 //error bar found no clock...
1094 if ( peakcnt
> peaksdet
[ clkCnt
]) {
1095 peaksdet
[ clkCnt
]= peakcnt
;
1102 for ( iii
= 7 ; iii
> 0 ; iii
--){
1103 if (( peaksdet
[ iii
] >= ( peaksdet
[ best
]- 1 )) && ( peaksdet
[ iii
] <= peaksdet
[ best
]+ 1 ) && lowestTransition
) {
1104 if ( clk
[ iii
] > ( lowestTransition
- ( clk
[ iii
]/ 8 )) && clk
[ iii
] < ( lowestTransition
+ ( clk
[ iii
]/ 8 ))) {
1107 } else if ( peaksdet
[ iii
] > peaksdet
[ best
]){
1110 //prnt("DEBUG: Clk: %d, peaks: %d, maxPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],maxPeak, clk[best], lowestTransition);
1117 // convert psk1 demod to psk2 demod
1118 // only transition waves are 1s
1119 void psk1TOpsk2 ( uint8_t * BitStream
, size_t size
)
1122 uint8_t lastBit
= BitStream
[ 0 ];
1123 for (; i
< size
; i
++){
1124 if ( BitStream
[ i
]== 7 ){
1126 } else if ( lastBit
!= BitStream
[ i
]){
1127 lastBit
= BitStream
[ i
];
1137 // convert psk2 demod to psk1 demod
1138 // from only transition waves are 1s to phase shifts change bit
1139 void psk2TOpsk1 ( uint8_t * BitStream
, size_t size
)
1142 for ( size_t i
= 0 ; i
< size
; i
++){
1143 if ( BitStream
[ i
]== 1 ){
1151 // redesigned by marshmellow adjusted from existing decode functions
1152 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1153 int indala26decode ( uint8_t * bitStream
, size_t * size
, uint8_t * invert
)
1155 //26 bit 40134 format (don't know other formats)
1156 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
1157 uint8_t preamble_i
[] = { 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 0 };
1158 size_t startidx
= 0 ;
1159 if (! preambleSearch ( bitStream
, preamble
, sizeof ( preamble
), size
, & startidx
)){
1160 // if didn't find preamble try again inverting
1161 if (! preambleSearch ( bitStream
, preamble_i
, sizeof ( preamble_i
), size
, & startidx
)) return - 1 ;
1164 if (* size
!= 64 && * size
!= 224 ) return - 2 ;
1166 for ( size_t i
= startidx
; i
< * size
; i
++)
1169 return ( int ) startidx
;
1172 // by marshmellow - demodulate NRZ wave
1173 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1174 int nrzRawDemod ( uint8_t * dest
, size_t * size
, int * clk
, int * invert
){
1175 if ( justNoise ( dest
, * size
)) return - 1 ;
1176 * clk
= DetectNRZClock ( dest
, * size
, * clk
);
1177 if (* clk
== 0 ) return - 2 ;
1178 size_t i
, gLen
= 4096 ;
1179 if ( gLen
>* size
) gLen
= * size
- 20 ;
1181 if ( getHiLo ( dest
, gLen
, & high
, & low
, 75 , 75 ) < 1 ) return - 3 ; //25% fuzz on high 25% fuzz on low
1184 //convert wave samples to 1's and 0's
1185 for ( i
= 20 ; i
< * size
- 20 ; i
++){
1186 if ( dest
[ i
] >= high
) bit
= 1 ;
1187 if ( dest
[ i
] <= low
) bit
= 0 ;
1190 //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit)
1193 for ( i
= 21 ; i
< * size
- 20 ; i
++) {
1194 //if transition detected or large number of same bits - store the passed bits
1195 if ( dest
[ i
] != dest
[ i
- 1 ] || ( i
- lastBit
) == ( 10 * * clk
)) {
1196 memset ( dest
+ numBits
, dest
[ i
- 1 ] ^ * invert
, ( i
- lastBit
+ (* clk
/ 4 )) / * clk
);
1197 numBits
+= ( i
- lastBit
+ (* clk
/ 4 )) / * clk
;
1206 //detects the bit clock for FSK given the high and low Field Clocks
1207 uint8_t detectFSKClk ( uint8_t * BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
)
1209 uint8_t clk
[] = { 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 0 };
1210 uint16_t rfLens
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1211 uint8_t rfCnts
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1212 uint8_t rfLensFnd
= 0 ;
1213 uint8_t lastFCcnt
= 0 ;
1214 uint16_t fcCounter
= 0 ;
1215 uint16_t rfCounter
= 0 ;
1216 uint8_t firstBitFnd
= 0 ;
1218 if ( size
== 0 ) return 0 ;
1220 uint8_t fcTol
= (( fcHigh
* 100 - fcLow
* 100 )/ 2 + 50 )/ 100 ; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2);
1225 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1226 // prime i to first peak / up transition
1227 for ( i
= 160 ; i
< size
- 20 ; i
++)
1228 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
]>= BitStream
[ i
+ 1 ])
1231 for (; i
< size
- 20 ; i
++){
1235 if ( BitStream
[ i
] <= BitStream
[ i
- 1 ] || BitStream
[ i
] < BitStream
[ i
+ 1 ])
1238 // if we got less than the small fc + tolerance then set it to the small fc
1239 if ( fcCounter
< fcLow
+ fcTol
)
1241 else //set it to the large fc
1244 //look for bit clock (rf/xx)
1245 if (( fcCounter
< lastFCcnt
|| fcCounter
> lastFCcnt
)){
1246 //not the same size as the last wave - start of new bit sequence
1247 if ( firstBitFnd
> 1 ){ //skip first wave change - probably not a complete bit
1248 for ( int ii
= 0 ; ii
< 15 ; ii
++){
1249 if ( rfLens
[ ii
] >= ( rfCounter
- 4 ) && rfLens
[ ii
] <= ( rfCounter
+ 4 )){
1255 if ( rfCounter
> 0 && rfLensFnd
< 15 ){
1256 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1257 rfCnts
[ rfLensFnd
]++;
1258 rfLens
[ rfLensFnd
++] = rfCounter
;
1264 lastFCcnt
= fcCounter
;
1268 uint8_t rfHighest
= 15 , rfHighest2
= 15 , rfHighest3
= 15 ;
1270 for ( i
= 0 ; i
< 15 ; i
++){
1271 //prnt("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1272 //get highest 2 RF values (might need to get more values to compare or compare all?)
1273 if ( rfCnts
[ i
]> rfCnts
[ rfHighest
]){
1274 rfHighest3
= rfHighest2
;
1275 rfHighest2
= rfHighest
;
1277 } else if ( rfCnts
[ i
]> rfCnts
[ rfHighest2
]){
1278 rfHighest3
= rfHighest2
;
1280 } else if ( rfCnts
[ i
]> rfCnts
[ rfHighest3
]){
1284 // set allowed clock remainder tolerance to be 1 large field clock length+1
1285 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1286 uint8_t tol1
= fcHigh
+ 1 ;
1288 //prnt("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1290 // loop to find the highest clock that has a remainder less than the tolerance
1291 // compare samples counted divided by
1292 // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less)
1294 for (; ii
>= 2 ; ii
--){
1295 if ( rfLens
[ rfHighest
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest
] % clk
[ ii
] > clk
[ ii
]- tol1
){
1296 if ( rfLens
[ rfHighest2
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest2
] % clk
[ ii
] > clk
[ ii
]- tol1
){
1297 if ( rfLens
[ rfHighest3
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest3
] % clk
[ ii
] > clk
[ ii
]- tol1
){
1304 if ( ii
< 0 ) return 0 ; // oops we went too far
1310 //countFC is to detect the field clock lengths.
1311 //counts and returns the 2 most common wave lengths
1312 //mainly used for FSK field clock detection
1313 uint16_t countFC ( uint8_t * BitStream
, size_t size
, uint8_t fskAdj
)
1315 uint8_t fcLens
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1316 uint16_t fcCnts
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1317 uint8_t fcLensFnd
= 0 ;
1318 uint8_t lastFCcnt
= 0 ;
1319 uint8_t fcCounter
= 0 ;
1321 if ( size
== 0 ) return 0 ;
1323 // prime i to first up transition
1324 for ( i
= 160 ; i
< size
- 20 ; i
++)
1325 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
] >= BitStream
[ i
+ 1 ])
1328 for (; i
< size
- 20 ; i
++){
1329 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
] >= BitStream
[ i
+ 1 ]){
1330 // new up transition
1333 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1334 if ( lastFCcnt
== 5 && fcCounter
== 9 ) fcCounter
--;
1335 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
1336 if (( fcCounter
== 9 ) || fcCounter
== 4 ) fcCounter
++;
1337 // save last field clock count (fc/xx)
1338 lastFCcnt
= fcCounter
;
1340 // find which fcLens to save it to:
1341 for ( int ii
= 0 ; ii
< 15 ; ii
++){
1342 if ( fcLens
[ ii
]== fcCounter
){
1348 if ( fcCounter
> 0 && fcLensFnd
< 15 ){
1350 fcCnts
[ fcLensFnd
]++;
1351 fcLens
[ fcLensFnd
++]= fcCounter
;
1360 uint8_t best1
= 14 , best2
= 14 , best3
= 14 ;
1362 // go through fclens and find which ones are bigest 2
1363 for ( i
= 0 ; i
< 15 ; i
++){
1364 //prnt("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]);
1365 // get the 3 best FC values
1366 if ( fcCnts
[ i
]> maxCnt1
) {
1371 } else if ( fcCnts
[ i
]> fcCnts
[ best2
]){
1374 } else if ( fcCnts
[ i
]> fcCnts
[ best3
]){
1378 if ( fcLens
[ best1
]== 0 ) return 0 ;
1379 uint8_t fcH
= 0 , fcL
= 0 ;
1380 if ( fcLens
[ best1
]> fcLens
[ best2
]){
1387 //prnt("DEBUG: dd %d > %d",(size-180)/fcH/3,fcCnts[best1]+fcCnts[best2]);
1388 if (( size
- 180 )/ fcH
/ 3 > fcCnts
[ best1
]+ fcCnts
[ best2
]) return 0 ; //lots of waves not psk or fsk
1390 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1392 uint16_t fcs
= ((( uint16_t ) fcH
)<< 8 ) | fcL
;
1393 //prnt("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
1394 if ( fskAdj
) return fcs
;
1395 return fcLens
[ best1
];
1398 //by marshmellow - demodulate PSK1 wave
1399 //uses wave lengths (# Samples)
1400 int pskRawDemod ( uint8_t dest
[], size_t * size
, int * clock
, int * invert
)
1402 if ( size
== 0 ) return - 1 ;
1403 uint16_t loopCnt
= 4096 ; //don't need to loop through entire array...
1404 if (* size
< loopCnt
) loopCnt
= * size
;
1407 uint8_t curPhase
= * invert
;
1408 size_t i
, waveStart
= 1 , waveEnd
= 0 , firstFullWave
= 0 , lastClkBit
= 0 ;
1409 uint8_t fc
= 0 , fullWaveLen
= 0 , tol
= 1 ;
1410 uint16_t errCnt
= 0 , waveLenCnt
= 0 ;
1411 fc
= countFC ( dest
, * size
, 0 );
1412 if ( fc
!= 2 && fc
!= 4 && fc
!= 8 ) return - 1 ;
1413 //PrintAndLog("DEBUG: FC: %d",fc);
1414 * clock
= DetectPSKClock ( dest
, * size
, * clock
);
1415 if (* clock
== 0 ) return - 1 ;
1416 int avgWaveVal
= 0 , lastAvgWaveVal
= 0 ;
1417 //find first phase shift
1418 for ( i
= 0 ; i
< loopCnt
; i
++){
1419 if ( dest
[ i
]+ fc
< dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
1421 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1422 waveLenCnt
= waveEnd
- waveStart
;
1423 if ( waveLenCnt
> fc
&& waveStart
> fc
&& !( waveLenCnt
> fc
+ 2 )){ //not first peak and is a large wave but not out of whack
1424 lastAvgWaveVal
= avgWaveVal
/( waveLenCnt
);
1425 firstFullWave
= waveStart
;
1426 fullWaveLen
= waveLenCnt
;
1427 //if average wave value is > graph 0 then it is an up wave or a 1
1428 if ( lastAvgWaveVal
> 123 ) curPhase
^= 1 ; //fudge graph 0 a little 123 vs 128
1434 avgWaveVal
+= dest
[ i
+ 2 ];
1436 if ( firstFullWave
== 0 ) {
1437 // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
1438 // so skip a little to ensure we are past any Start Signal
1439 firstFullWave
= 160 ;
1440 memset ( dest
, curPhase
, firstFullWave
/ * clock
);
1442 memset ( dest
, curPhase
^ 1 , firstFullWave
/ * clock
);
1445 numBits
+= ( firstFullWave
/ * clock
);
1446 //set start of wave as clock align
1447 lastClkBit
= firstFullWave
;
1448 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1449 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1451 dest
[ numBits
++] = curPhase
; //set first read bit
1452 for ( i
= firstFullWave
+ fullWaveLen
- 1 ; i
< * size
- 3 ; i
++){
1453 //top edge of wave = start of new wave
1454 if ( dest
[ i
]+ fc
< dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
1455 if ( waveStart
== 0 ) {
1458 avgWaveVal
= dest
[ i
+ 1 ];
1461 waveLenCnt
= waveEnd
- waveStart
;
1462 lastAvgWaveVal
= avgWaveVal
/ waveLenCnt
;
1463 if ( waveLenCnt
> fc
){
1464 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1465 //this wave is a phase shift
1466 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1467 if ( i
+ 1 >= lastClkBit
+ * clock
- tol
){ //should be a clock bit
1469 dest
[ numBits
++] = curPhase
;
1470 lastClkBit
+= * clock
;
1471 } else if ( i
< lastClkBit
+ 10 + fc
){
1472 //noise after a phase shift - ignore
1473 } else { //phase shift before supposed to based on clock
1475 dest
[ numBits
++] = 7 ;
1477 } else if ( i
+ 1 > lastClkBit
+ * clock
+ tol
+ fc
){
1478 lastClkBit
+= * clock
; //no phase shift but clock bit
1479 dest
[ numBits
++] = curPhase
;
1485 avgWaveVal
+= dest
[ i
+ 1 ];