]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
880e2c2b04ebfe20cd6d212f82d20837db9e5eb4
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 - by marshmellow, holiman, iceman and
9 // many others who came before
12 // LF Demod functions are placed here to allow the flexability to use client or
13 // device side. Most BUT NOT ALL of these functions are currenlty safe for
14 // device side use currently. (DetectST for example...)
16 // There are likely many improvements to the code that could be made, please
17 // make suggestions...
19 // we tried to include author comments so any questions could be directed to
22 // There are 4 main sections of code below:
24 // for general utilities used by multiple other functions
25 // Clock / Bitrate Detection Section:
26 // for clock detection functions for each modulation
27 // Modulation Demods &/or Decoding Section:
28 // for main general modulation demodulating and encoding decoding code.
29 // Tag format detection section:
30 // for detection of specific tag formats within demodulated data
33 //-----------------------------------------------------------------------------
35 #include <string.h> // for memset, memcmp and size_t
37 #include <stdint.h> // for uint_32+
38 #include <stdbool.h> // for bool
39 #include "parity.h" // for parity test
41 //**********************************************************************************************
42 //---------------------------------Utilities Section--------------------------------------------
43 //**********************************************************************************************
44 #define LOWEST_DEFAULT_CLOCK 32
45 #define FSK_PSK_THRESHOLD 123
47 //to allow debug print calls when used not on device
48 void dummy ( char * fmt
, ...){}
51 #include "cmdparser.h"
53 #define prnt PrintAndLog
55 uint8_t g_debugMode
= 0 ;
59 uint8_t justNoise ( uint8_t * BitStream
, size_t size
) {
60 //test samples are not just noise
61 uint8_t justNoise1
= 1 ;
62 for ( size_t idx
= 0 ; idx
< size
&& justNoise1
; idx
++){
63 justNoise1
= BitStream
[ idx
] < FSK_PSK_THRESHOLD
;
69 //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
70 int getHiLo ( uint8_t * BitStream
, size_t size
, int * high
, int * low
, uint8_t fuzzHi
, uint8_t fuzzLo
) {
73 // get high and low thresholds
74 for ( size_t i
= 0 ; i
< size
; i
++){
75 if ( BitStream
[ i
] > * high
) * high
= BitStream
[ i
];
76 if ( BitStream
[ i
] < * low
) * low
= BitStream
[ i
];
78 if (* high
< FSK_PSK_THRESHOLD
) return - 1 ; // just noise
79 * high
= ((* high
- 128 )* fuzzHi
+ 12800 )/ 100 ;
80 * low
= ((* low
- 128 )* fuzzLo
+ 12800 )/ 100 ;
85 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
86 // returns 1 if passed
87 bool parityTest ( uint32_t bits
, uint8_t bitLen
, uint8_t pType
) {
88 return oddparity32 ( bits
) ^ pType
;
92 // takes a array of binary values, start position, length of bits per parity (includes parity bit - MAX 32),
93 // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run)
94 size_t removeParity ( uint8_t * BitStream
, size_t startIdx
, uint8_t pLen
, uint8_t pType
, size_t bLen
) {
95 uint32_t parityWd
= 0 ;
97 for ( int word
= 0 ; word
< ( bLen
); word
+= pLen
) {
98 for ( int bit
= 0 ; bit
< pLen
; bit
++) {
99 if ( word
+ bit
>= bLen
) break ;
100 parityWd
= ( parityWd
<< 1 ) | BitStream
[ startIdx
+ word
+ bit
];
101 BitStream
[ bitCnt
++] = ( BitStream
[ startIdx
+ word
+ bit
]);
103 if ( word
+ pLen
> bLen
) break ;
105 bitCnt
--; // overwrite parity with next data
106 // if parity fails then return 0
108 case 3 : if ( BitStream
[ bitCnt
]== 1 ) { return 0 ;} break ; //should be 0 spacer bit
109 case 2 : if ( BitStream
[ bitCnt
]== 0 ) { return 0 ;} break ; //should be 1 spacer bit
110 default : if ( parityTest ( parityWd
, pLen
, pType
) == 0 ) { return 0 ;} break ; //test parity
114 // if we got here then all the parities passed
120 // takes a array of binary values, length of bits per parity (includes parity bit),
121 // Parity Type (1 for odd; 0 for even; 2 Always 1's; 3 Always 0's), and binary Length (length to run)
122 // Make sure *dest is long enough to store original sourceLen + #_of_parities_to_be_added
123 size_t addParity ( uint8_t * BitSource
, uint8_t * dest
, uint8_t sourceLen
, uint8_t pLen
, uint8_t pType
) {
124 uint32_t parityWd
= 0 ;
125 size_t j
= 0 , bitCnt
= 0 ;
126 for ( int word
= 0 ; word
< sourceLen
; word
+= pLen
- 1 ) {
127 for ( int bit
= 0 ; bit
< pLen
- 1 ; bit
++){
128 parityWd
= ( parityWd
<< 1 ) | BitSource
[ word
+ bit
];
129 dest
[ j
++] = ( BitSource
[ word
+ bit
]);
131 // if parity fails then return 0
133 case 3 : dest
[ j
++]= 0 ; break ; // marker bit which should be a 0
134 case 2 : dest
[ j
++]= 1 ; break ; // marker bit which should be a 1
136 dest
[ j
++] = parityTest ( parityWd
, pLen
- 1 , pType
) ^ 1 ;
142 // if we got here then all the parities passed
143 //return ID start index and size
147 uint32_t bytebits_to_byte ( uint8_t * src
, size_t numbits
) {
149 for ( int i
= 0 ; i
< numbits
; i
++)
151 num
= ( num
<< 1 ) | (* src
);
157 //least significant bit first
158 uint32_t bytebits_to_byteLSBF ( uint8_t * src
, size_t numbits
) {
160 for ( int i
= 0 ; i
< numbits
; i
++)
162 num
= ( num
<< 1 ) | *( src
+ ( numbits
-( i
+ 1 )));
167 // search for given preamble in given BitStream and return success=1 or fail=0 and startIndex (where it was found) and length if not fineone
168 // fineone does not look for a repeating preamble for em4x05/4x69 sends preamble once, so look for it once in the first pLen bits
169 bool preambleSearchEx ( uint8_t * BitStream
, uint8_t * preamble
, size_t pLen
, size_t * size
, size_t * startIdx
, bool findone
) {
170 // Sanity check. If preamble length is bigger than bitstream length.
171 if ( * size
<= pLen
) return false ;
173 uint8_t foundCnt
= 0 ;
174 for ( size_t idx
= 0 ; idx
< * size
- pLen
; idx
++) {
175 if ( memcmp ( BitStream
+ idx
, preamble
, pLen
) == 0 ) {
179 if ( g_debugMode
) prnt ( "DEBUG: preamble found at %u" , idx
);
181 if ( findone
) return true ;
182 } else if ( foundCnt
== 2 ) {
183 * size
= idx
- * startIdx
;
192 //search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
193 uint8_t preambleSearch ( uint8_t * BitStream
, uint8_t * preamble
, size_t pLen
, size_t * size
, size_t * startIdx
) {
194 return ( preambleSearchEx ( BitStream
, preamble
, pLen
, size
, startIdx
, false )) ? 1 : 0 ;
197 // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.
198 size_t findModStart ( uint8_t dest
[], size_t size
, uint8_t expWaveSize
) {
200 size_t waveSizeCnt
= 0 ;
201 uint8_t thresholdCnt
= 0 ;
202 bool isAboveThreshold
= dest
[ i
++] >= FSK_PSK_THRESHOLD
;
203 for (; i
< size
- 20 ; i
++ ) {
204 if ( dest
[ i
] < FSK_PSK_THRESHOLD
&& isAboveThreshold
) {
206 if ( thresholdCnt
> 2 && waveSizeCnt
< expWaveSize
+ 1 ) break ;
207 isAboveThreshold
= false ;
209 } else if ( dest
[ i
] >= FSK_PSK_THRESHOLD
&& ! isAboveThreshold
) {
211 if ( thresholdCnt
> 2 && waveSizeCnt
< expWaveSize
+ 1 ) break ;
212 isAboveThreshold
= true ;
217 if ( thresholdCnt
> 10 ) break ;
219 if ( g_debugMode
== 2 ) prnt ( "DEBUG: threshold Count reached at %u, count: %u" , i
, thresholdCnt
);
223 int getClosestClock ( int testclk
) {
224 uint8_t fndClk
[] = { 8 , 16 , 32 , 40 , 50 , 64 , 128 };
226 for ( uint8_t clkCnt
= 0 ; clkCnt
< 7 ; clkCnt
++)
227 if ( testclk
>= fndClk
[ clkCnt
]-( fndClk
[ clkCnt
]/ 8 ) && testclk
<= fndClk
[ clkCnt
]+ 1 )
228 return fndClk
[ clkCnt
];
233 void getNextLow ( uint8_t samples
[], size_t size
, int low
, size_t * i
) {
234 while (( samples
[* i
] > low
) && (* i
< size
))
238 void getNextHigh ( uint8_t samples
[], size_t size
, int high
, size_t * i
) {
239 while (( samples
[* i
] < high
) && (* i
< size
))
243 // load wave counters
244 bool loadWaveCounters ( uint8_t samples
[], size_t size
, int lowToLowWaveLen
[], int highToLowWaveLen
[], int * waveCnt
, int * skip
, int * minClk
, int * high
, int * low
) {
245 size_t i
= 0 , firstLow
, firstHigh
;
246 size_t testsize
= ( size
< 512 ) ? size
: 512 ;
248 if ( getHiLo ( samples
, testsize
, high
, low
, 80 , 80 ) == - 1 ) {
249 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: just noise detected - quitting" );
250 return false ; //just noise
253 // get to first full low to prime loop and skip incomplete first pulse
254 getNextHigh ( samples
, size
, * high
, & i
);
255 getNextLow ( samples
, size
, * low
, & i
);
258 // populate tmpbuff buffer with pulse lengths
260 // measure from low to low
262 //find first high point for this wave
263 getNextHigh ( samples
, size
, * high
, & i
);
266 getNextLow ( samples
, size
, * low
, & i
);
268 if (* waveCnt
>= ( size
/ LOWEST_DEFAULT_CLOCK
))
271 highToLowWaveLen
[* waveCnt
] = i
- firstHigh
; //first high to first low
272 lowToLowWaveLen
[* waveCnt
] = i
- firstLow
;
274 if ( i
- firstLow
< * minClk
&& i
< size
) {
275 * minClk
= i
- firstLow
;
281 size_t pskFindFirstPhaseShift ( uint8_t samples
[], size_t size
, uint8_t * curPhase
, size_t waveStart
, uint16_t fc
, uint16_t * fullWaveLen
) {
282 uint16_t loopCnt
= ( size
+ 3 < 4096 ) ? size
: 4096 ; //don't need to loop through entire array...
284 uint16_t avgWaveVal
= 0 , lastAvgWaveVal
= 0 ;
285 size_t i
= waveStart
, waveEnd
, waveLenCnt
, firstFullWave
;
286 for (; i
< loopCnt
; i
++) {
287 // find peak // was "samples[i] + fc" but why? must have been used to weed out some wave error... removed..
288 if ( samples
[ i
] < samples
[ i
+ 1 ] && samples
[ i
+ 1 ] >= samples
[ i
+ 2 ]){
290 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: waveEnd: %u, waveStart: %u" , waveEnd
, waveStart
);
291 waveLenCnt
= waveEnd
- waveStart
;
292 if ( waveLenCnt
> fc
&& waveStart
> fc
&& !( waveLenCnt
> fc
+ 8 )){ //not first peak and is a large wave but not out of whack
293 lastAvgWaveVal
= avgWaveVal
/( waveLenCnt
);
294 firstFullWave
= waveStart
;
295 * fullWaveLen
= waveLenCnt
;
296 //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
297 if ( lastAvgWaveVal
> FSK_PSK_THRESHOLD
) * curPhase
^= 1 ;
298 return firstFullWave
;
303 avgWaveVal
+= samples
[ i
+ 2 ];
309 //amplify based on ask edge detection - not accurate enough to use all the time
310 void askAmp ( uint8_t * BitStream
, size_t size
) {
312 for ( size_t i
= 1 ; i
< size
; i
++){
313 if ( BitStream
[ i
]- BitStream
[ i
- 1 ]>= 30 ) //large jump up
315 else if ( BitStream
[ i
- 1 ]- BitStream
[ i
]>= 20 ) //large jump down
318 BitStream
[ i
- 1 ] = Last
;
323 uint32_t manchesterEncode2Bytes ( uint16_t datain
) {
326 for ( uint8_t i
= 0 ; i
< 16 ; i
++) {
327 curBit
= ( datain
>> ( 15 - i
) & 1 );
328 output
|= ( 1 <<((( 15 - i
)* 2 )+ curBit
));
334 //encode binary data into binary manchester
335 //NOTE: BitStream must have triple the size of "size" available in memory to do the swap
336 int ManchesterEncode ( uint8_t * BitStream
, size_t size
) {
337 //allow up to 4K out (means BitStream must be at least 2048+4096 to handle the swap)
338 size
= ( size
> 2048 ) ? 2048 : size
;
339 size_t modIdx
= size
;
341 for ( size_t idx
= 0 ; idx
< size
; idx
++){
342 BitStream
[ idx
+ modIdx
++] = BitStream
[ idx
];
343 BitStream
[ idx
+ modIdx
++] = BitStream
[ idx
]^ 1 ;
345 for ( i
= 0 ; i
<( size
* 2 ); i
++){
346 BitStream
[ i
] = BitStream
[ i
+ size
];
352 // to detect a wave that has heavily clipped (clean) samples
353 uint8_t DetectCleanAskWave ( uint8_t dest
[], size_t size
, uint8_t high
, uint8_t low
) {
354 bool allArePeaks
= true ;
356 size_t loopEnd
= 512 + 160 ;
357 if ( loopEnd
> size
) loopEnd
= size
;
358 for ( size_t i
= 160 ; i
< loopEnd
; i
++){
359 if ( dest
[ i
]> low
&& dest
[ i
]< high
)
365 if ( cntPeaks
> 300 ) return true ;
370 //**********************************************************************************************
371 //-------------------Clock / Bitrate Detection Section------------------------------------------
372 //**********************************************************************************************
375 // to help detect clocks on heavily clipped samples
376 // based on count of low to low
377 int DetectStrongAskClock ( uint8_t dest
[], size_t size
, int high
, int low
, int * clock
) {
381 int shortestWaveIdx
= 0 ;
382 // get to first full low to prime loop and skip incomplete first pulse
383 getNextHigh ( dest
, size
, high
, & i
);
384 getNextLow ( dest
, size
, low
, & i
);
386 // loop through all samples
388 // measure from low to low
391 getNextHigh ( dest
, size
, high
, & i
);
392 getNextLow ( dest
, size
, low
, & i
);
393 //get minimum measured distance
394 if ( i
- startwave
< minClk
&& i
< size
) {
395 minClk
= i
- startwave
;
396 shortestWaveIdx
= startwave
;
400 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: DetectStrongAskClock smallest wave: %d" , minClk
);
401 * clock
= getClosestClock ( minClk
);
405 return shortestWaveIdx
;
409 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
410 // maybe somehow adjust peak trimming value based on samples to fix?
411 // return start index of best starting position for that clock and return clock (by reference)
412 int DetectASKClock ( uint8_t dest
[], size_t size
, int * clock
, int maxErr
) {
414 uint8_t clk
[] = { 255 , 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 };
416 uint8_t loopCnt
= 255 ; //don't need to loop through entire array...
417 if ( size
<= loopCnt
+ 60 ) return - 1 ; //not enough samples
418 size
-= 60 ; //sometimes there is a strange end wave - filter out this....
419 //if we already have a valid clock
422 if ( clk
[ i
] == * clock
) clockFnd
= i
;
423 //clock found but continue to find best startpos
425 //get high and low peak
427 if ( getHiLo ( dest
, loopCnt
, & peak
, & low
, 75 , 75 ) < 1 ) return - 1 ;
429 //test for large clean peaks
431 if ( DetectCleanAskWave ( dest
, size
, peak
, low
)== 1 ){
432 int ans
= DetectStrongAskClock ( dest
, size
, peak
, low
, clock
);
433 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: detectaskclk Clean Ask Wave Detected: clk %i, ShortestWave: %i" , clock
, ans
);
435 return ans
; //return shortest wave start position
440 uint8_t clkCnt
, tol
= 0 ;
441 uint16_t bestErr
[]={ 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 };
442 uint8_t bestStart
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
444 size_t arrLoc
, loopEnd
;
452 //test each valid clock from smallest to greatest to see which lines up
453 for (; clkCnt
< clkEnd
; clkCnt
++){
454 if ( clk
[ clkCnt
] <= 32 ){
459 //if no errors allowed - keep start within the first clock
460 if (! maxErr
&& size
> clk
[ clkCnt
]* 2 + tol
&& clk
[ clkCnt
]< 128 ) loopCnt
= clk
[ clkCnt
]* 2 ;
461 bestErr
[ clkCnt
]= 1000 ;
462 //try lining up the peaks by moving starting point (try first few clocks)
463 for ( ii
= 0 ; ii
< loopCnt
; ii
++){
464 if ( dest
[ ii
] < peak
&& dest
[ ii
] > low
) continue ;
467 // now that we have the first one lined up test rest of wave array
468 loopEnd
= (( size
- ii
- tol
) / clk
[ clkCnt
]) - 1 ;
469 for ( i
= 0 ; i
< loopEnd
; ++ i
){
470 arrLoc
= ii
+ ( i
* clk
[ clkCnt
]);
471 if ( dest
[ arrLoc
] >= peak
|| dest
[ arrLoc
] <= low
){
472 } else if ( dest
[ arrLoc
- tol
] >= peak
|| dest
[ arrLoc
- tol
] <= low
){
473 } else if ( dest
[ arrLoc
+ tol
] >= peak
|| dest
[ arrLoc
+ tol
] <= low
){
474 } else { //error no peak detected
478 //if we found no errors then we can stop here and a low clock (common clocks)
479 // this is correct one - return this clock
480 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: clk %d, err %d, startpos %d, endpos %d" , clk
[ clkCnt
], errCnt
, ii
, i
);
481 if ( errCnt
== 0 && clkCnt
< 7 ) {
482 if (! clockFnd
) * clock
= clk
[ clkCnt
];
485 //if we found errors see if it is lowest so far and save it as best run
486 if ( errCnt
< bestErr
[ clkCnt
]){
487 bestErr
[ clkCnt
]= errCnt
;
488 bestStart
[ clkCnt
]= ii
;
494 for ( iii
= 1 ; iii
< clkEnd
; ++ iii
){
495 if ( bestErr
[ iii
] < bestErr
[ best
]){
496 if ( bestErr
[ iii
] == 0 ) bestErr
[ iii
]= 1 ;
497 // current best bit to error ratio vs new bit to error ratio
498 if ( ( size
/ clk
[ best
])/ bestErr
[ best
] < ( size
/ clk
[ iii
])/ bestErr
[ iii
] ){
502 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d" , clk
[ iii
], bestErr
[ iii
], clk
[ best
], bestStart
[ best
]);
504 if (! clockFnd
) * clock
= clk
[ best
];
505 return bestStart
[ best
];
508 int DetectStrongNRZClk ( uint8_t * dest
, size_t size
, int peak
, int low
, bool * strong
) {
509 //find shortest transition from high to low
512 size_t transition1
= 0 ;
513 int lowestTransition
= 255 ;
514 bool lastWasHigh
= false ;
515 size_t transitionSampleCount
= 0 ;
516 //find first valid beginning of a high or low wave
517 while (( dest
[ i
] >= peak
|| dest
[ i
] <= low
) && ( i
< size
))
519 while (( dest
[ i
] < peak
&& dest
[ i
] > low
) && ( i
< size
))
521 lastWasHigh
= ( dest
[ i
] >= peak
);
523 if ( i
== size
) return 0 ;
526 for (; i
< size
; i
++) {
527 if (( dest
[ i
] >= peak
&& ! lastWasHigh
) || ( dest
[ i
] <= low
&& lastWasHigh
)) {
528 lastWasHigh
= ( dest
[ i
] >= peak
);
529 if ( i
- transition1
< lowestTransition
) lowestTransition
= i
- transition1
;
531 } else if ( dest
[ i
] < peak
&& dest
[ i
] > low
) {
532 transitionSampleCount
++;
535 if ( lowestTransition
== 255 ) lowestTransition
= 0 ;
536 if ( g_debugMode
== 2 ) prnt ( "DEBUG NRZ: detectstrongNRZclk smallest wave: %d" , lowestTransition
);
537 // if less than 10% of the samples were not peaks (or 90% were peaks) then we have a strong wave
538 if ( transitionSampleCount
/ size
< 10 ) {
540 lowestTransition
= getClosestClock ( lowestTransition
);
542 return lowestTransition
;
546 //detect nrz clock by reading #peaks vs no peaks(or errors)
547 int DetectNRZClock ( uint8_t dest
[], size_t size
, int clock
, size_t * clockStartIdx
) {
549 uint8_t clk
[]={ 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 };
550 size_t loopCnt
= 4096 ; //don't need to loop through entire array...
551 if ( size
== 0 ) return 0 ;
552 if ( size
< loopCnt
) loopCnt
= size
- 20 ;
553 //if we already have a valid clock quit
555 if ( clk
[ i
] == clock
) return clock
;
557 //get high and low peak
559 if ( getHiLo ( dest
, loopCnt
, & peak
, & low
, 90 , 90 ) < 1 ) return 0 ;
562 int lowestTransition
= DetectStrongNRZClk ( dest
, size
- 20 , peak
, low
, & strong
);
563 if ( strong
) return lowestTransition
;
567 uint16_t smplCnt
= 0 ;
569 int16_t peaksdet
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
570 uint16_t minPeak
= 255 ;
571 bool firstpeak
= true ;
572 //test for large clipped waves - ignore first peak
573 for ( i
= 0 ; i
< loopCnt
; i
++) {
574 if ( dest
[ i
] >= peak
|| dest
[ i
] <= low
) {
575 if ( firstpeak
) continue ;
580 if ( minPeak
> smplCnt
&& smplCnt
> 7 ) minPeak
= smplCnt
;
582 if ( g_debugMode
== 2 ) prnt ( "DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d" , minPeak
, smplCnt
, peakcnt
);
587 if ( minPeak
< 8 ) return 0 ;
590 uint8_t ignoreCnt
= 0 ;
591 uint8_t ignoreWindow
= 4 ;
592 bool lastPeakHigh
= 0 ;
594 size_t bestStart
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
596 //test each valid clock from smallest to greatest to see which lines up
597 for ( clkCnt
= 0 ; clkCnt
< 8 ; ++ clkCnt
) {
598 //ignore clocks smaller than smallest peak
599 if ( clk
[ clkCnt
] < minPeak
- ( clk
[ clkCnt
]/ 4 )) continue ;
600 //try lining up the peaks by moving starting point (try first 256)
601 for ( ii
= 20 ; ii
< loopCnt
; ++ ii
) {
602 if (( dest
[ ii
] >= peak
) || ( dest
[ ii
] <= low
)) {
606 lastBit
= ii
- clk
[ clkCnt
];
607 //loop through to see if this start location works
608 for ( i
= ii
; i
< size
- 20 ; ++ i
) {
609 //if we are at a clock bit
610 if (( i
>= lastBit
+ clk
[ clkCnt
] - tol
) && ( i
<= lastBit
+ clk
[ clkCnt
] + tol
)) {
612 if ( dest
[ i
] >= peak
|| dest
[ i
] <= low
) {
613 //if same peak don't count it
614 if (( dest
[ i
] >= peak
&& ! lastPeakHigh
) || ( dest
[ i
] <= low
&& lastPeakHigh
)) {
617 lastPeakHigh
= ( dest
[ i
] >= peak
);
620 ignoreCnt
= ignoreWindow
;
621 lastBit
+= clk
[ clkCnt
];
622 } else if ( i
== lastBit
+ clk
[ clkCnt
] + tol
) {
623 lastBit
+= clk
[ clkCnt
];
625 //else if not a clock bit and no peaks
626 } else if ( dest
[ i
] < peak
&& dest
[ i
] > low
) {
629 if ( errBitHigh
== true ) peakcnt
--;
634 // else if not a clock bit but we have a peak
635 } else if (( dest
[ i
]>= peak
|| dest
[ i
]<= low
) && (! bitHigh
)) {
636 //error bar found no clock...
640 if ( peakcnt
> peaksdet
[ clkCnt
]) {
641 bestStart
[ clkCnt
]= ii
;
642 peaksdet
[ clkCnt
]= peakcnt
;
649 for ( iii
= 7 ; iii
> 0 ; iii
--) {
650 if (( peaksdet
[ iii
] >= ( peaksdet
[ best
]- 1 )) && ( peaksdet
[ iii
] <= peaksdet
[ best
]+ 1 ) && lowestTransition
) {
651 if ( clk
[ iii
] > ( lowestTransition
- ( clk
[ iii
]/ 8 )) && clk
[ iii
] < ( lowestTransition
+ ( clk
[ iii
]/ 8 ))) {
654 } else if ( peaksdet
[ iii
] > peaksdet
[ best
]) {
657 if ( g_debugMode
== 2 ) prnt ( "DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d" , clk
[ iii
], peaksdet
[ iii
], minPeak
, clk
[ best
], lowestTransition
);
659 * clockStartIdx
= bestStart
[ best
];
664 //countFC is to detect the field clock lengths.
665 //counts and returns the 2 most common wave lengths
666 //mainly used for FSK field clock detection
667 uint16_t countFC ( uint8_t * BitStream
, size_t size
, uint8_t fskAdj
) {
668 uint8_t fcLens
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
669 uint16_t fcCnts
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
670 uint8_t fcLensFnd
= 0 ;
671 uint8_t lastFCcnt
= 0 ;
672 uint8_t fcCounter
= 0 ;
674 if ( size
< 180 ) return 0 ;
676 // prime i to first up transition
677 for ( i
= 160 ; i
< size
- 20 ; i
++)
678 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
] >= BitStream
[ i
+ 1 ])
681 for (; i
< size
- 20 ; i
++){
682 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
] >= BitStream
[ i
+ 1 ]){
686 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
687 if ( lastFCcnt
== 5 && fcCounter
== 9 ) fcCounter
--;
688 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
689 if (( fcCounter
== 9 ) || fcCounter
== 4 ) fcCounter
++;
690 // save last field clock count (fc/xx)
691 lastFCcnt
= fcCounter
;
693 // find which fcLens to save it to:
694 for ( int ii
= 0 ; ii
< 15 ; ii
++){
695 if ( fcLens
[ ii
]== fcCounter
){
701 if ( fcCounter
> 0 && fcLensFnd
< 15 ){
704 fcLens
[ fcLensFnd
++]= fcCounter
;
713 uint8_t best1
= 14 , best2
= 14 , best3
= 14 ;
715 // go through fclens and find which ones are bigest 2
716 for ( i
= 0 ; i
< 15 ; i
++){
717 // get the 3 best FC values
718 if ( fcCnts
[ i
]> maxCnt1
) {
723 } else if ( fcCnts
[ i
]> fcCnts
[ best2
]){
726 } else if ( fcCnts
[ i
]> fcCnts
[ best3
]){
729 if ( g_debugMode
== 2 ) prnt ( "DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u" , fcLens
[ i
], fcCnts
[ i
], fcLens
[ best1
], fcLens
[ best2
]);
730 if ( fcLens
[ i
]== 0 ) break ;
732 if ( fcLens
[ best1
]== 0 ) return 0 ;
733 uint8_t fcH
= 0 , fcL
= 0 ;
734 if ( fcLens
[ best1
]> fcLens
[ best2
]){
741 if (( size
- 180 )/ fcH
/ 3 > fcCnts
[ best1
]+ fcCnts
[ best2
]) {
742 if ( g_debugMode
== 2 ) prnt ( "DEBUG countfc: fc is too large: %u > %u. Not psk or fsk" ,( size
- 180 )/ fcH
/ 3 , fcCnts
[ best1
]+ fcCnts
[ best2
]);
743 return 0 ; //lots of waves not psk or fsk
745 // TODO: take top 3 answers and compare to known Field clocks to get top 2
747 uint16_t fcs
= ((( uint16_t ) fcH
)<< 8 ) | fcL
;
748 if ( fskAdj
) return fcs
;
749 return ( uint16_t ) fcLens
[ best2
] << 8 | fcLens
[ best1
];
753 //detect psk clock by reading each phase shift
754 // a phase shift is determined by measuring the sample length of each wave
755 int DetectPSKClock ( uint8_t dest
[], size_t size
, int clock
, size_t * firstPhaseShift
, uint8_t * curPhase
, uint8_t * fc
) {
756 uint8_t clk
[]={ 255 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 255 }; //255 is not a valid clock
757 uint16_t loopCnt
= 4096 ; //don't need to loop through entire array...
758 if ( size
== 0 ) return 0 ;
759 if ( size
+ 3 < loopCnt
) loopCnt
= size
- 20 ;
761 uint16_t fcs
= countFC ( dest
, size
, 0 );
763 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: FC: %d, FC2: %d" ,* fc
, fcs
>> 8 );
764 if (( fcs
>> 8 ) == 10 && * fc
== 8 ) return 0 ;
765 if (* fc
!= 2 && * fc
!= 4 && * fc
!= 8 ) return 0 ;
767 //if we already have a valid clock quit
770 if ( clk
[ i
] == clock
) return clock
;
772 size_t waveStart
= 0 , waveEnd
= 0 , firstFullWave
= 0 , lastClkBit
= 0 ;
774 uint8_t clkCnt
, tol
= 1 ;
775 uint16_t peakcnt
= 0 , errCnt
= 0 , waveLenCnt
= 0 , fullWaveLen
= 0 ;
776 uint16_t bestErr
[]={ 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 , 1000 };
777 uint16_t peaksdet
[]={ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
779 //find start of modulating data in trace
780 i
= findModStart ( dest
, size
, * fc
);
782 firstFullWave
= pskFindFirstPhaseShift ( dest
, size
, curPhase
, i
, * fc
, & fullWaveLen
);
783 if ( firstFullWave
== 0 ) {
784 // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
785 // so skip a little to ensure we are past any Start Signal
790 * firstPhaseShift
= firstFullWave
;
791 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: firstFullWave: %d, waveLen: %d" , firstFullWave
, fullWaveLen
);
792 //test each valid clock from greatest to smallest to see which lines up
793 for ( clkCnt
= 7 ; clkCnt
>= 1 ; clkCnt
--) {
795 lastClkBit
= firstFullWave
; //set end of wave as clock align
799 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: clk: %d, lastClkBit: %d" , clk
[ clkCnt
], lastClkBit
);
801 for ( i
= firstFullWave
+ fullWaveLen
- 1 ; i
< loopCnt
- 2 ; i
++){
802 //top edge of wave = start of new wave
803 if ( dest
[ i
] < dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]){
804 if ( waveStart
== 0 ) {
809 waveLenCnt
= waveEnd
- waveStart
;
810 if ( waveLenCnt
> * fc
){
811 //if this wave is a phase shift
812 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d" , waveStart
, waveLenCnt
, lastClkBit
+ clk
[ clkCnt
]- tol
, i
+ 1 ,* fc
);
813 if ( i
+ 1 >= lastClkBit
+ clk
[ clkCnt
] - tol
){ //should be a clock bit
815 lastClkBit
+= clk
[ clkCnt
];
816 } else if ( i
< lastClkBit
+ 8 ){
817 //noise after a phase shift - ignore
818 } else { //phase shift before supposed to based on clock
821 } else if ( i
+ 1 > lastClkBit
+ clk
[ clkCnt
] + tol
+ * fc
){
822 lastClkBit
+= clk
[ clkCnt
]; //no phase shift but clock bit
831 if ( errCnt
<= bestErr
[ clkCnt
]) bestErr
[ clkCnt
]= errCnt
;
832 if ( peakcnt
> peaksdet
[ clkCnt
]) peaksdet
[ clkCnt
]= peakcnt
;
834 //all tested with errors
835 //return the highest clk with the most peaks found
837 for ( i
= 7 ; i
>= 1 ; i
--){
838 if ( peaksdet
[ i
] > peaksdet
[ best
]) {
841 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d" , clk
[ i
], peaksdet
[ i
], bestErr
[ i
], clk
[ best
]);
847 //detects the bit clock for FSK given the high and low Field Clocks
848 uint8_t detectFSKClk ( uint8_t * BitStream
, size_t size
, uint8_t fcHigh
, uint8_t fcLow
, int * firstClockEdge
) {
849 uint8_t clk
[] = { 8 , 16 , 32 , 40 , 50 , 64 , 100 , 128 , 0 };
850 uint16_t rfLens
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
851 uint8_t rfCnts
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
852 uint8_t rfLensFnd
= 0 ;
853 uint8_t lastFCcnt
= 0 ;
854 uint16_t fcCounter
= 0 ;
855 uint16_t rfCounter
= 0 ;
856 uint8_t firstBitFnd
= 0 ;
858 if ( size
== 0 ) return 0 ;
860 uint8_t fcTol
= (( fcHigh
* 100 - fcLow
* 100 )/ 2 + 50 )/ 100 ; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2);
865 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
866 // prime i to first peak / up transition
867 for ( i
= 160 ; i
< size
- 20 ; i
++)
868 if ( BitStream
[ i
] > BitStream
[ i
- 1 ] && BitStream
[ i
]>= BitStream
[ i
+ 1 ])
871 for (; i
< size
- 20 ; i
++){
875 if ( BitStream
[ i
] <= BitStream
[ i
- 1 ] || BitStream
[ i
] < BitStream
[ i
+ 1 ])
878 // if we got less than the small fc + tolerance then set it to the small fc
879 // if it is inbetween set it to the last counter
880 if ( fcCounter
< fcHigh
&& fcCounter
> fcLow
)
881 fcCounter
= lastFCcnt
;
882 else if ( fcCounter
< fcLow
+ fcTol
)
884 else //set it to the large fc
887 //look for bit clock (rf/xx)
888 if (( fcCounter
< lastFCcnt
|| fcCounter
> lastFCcnt
)){
889 //not the same size as the last wave - start of new bit sequence
890 if ( firstBitFnd
> 1 ){ //skip first wave change - probably not a complete bit
891 for ( int ii
= 0 ; ii
< 15 ; ii
++){
892 if ( rfLens
[ ii
] >= ( rfCounter
- 4 ) && rfLens
[ ii
] <= ( rfCounter
+ 4 )){
898 if ( rfCounter
> 0 && rfLensFnd
< 15 ){
899 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
901 rfLens
[ rfLensFnd
++] = rfCounter
;
912 uint8_t rfHighest
= 15 , rfHighest2
= 15 , rfHighest3
= 15 ;
914 for ( i
= 0 ; i
< 15 ; i
++){
915 //get highest 2 RF values (might need to get more values to compare or compare all?)
916 if ( rfCnts
[ i
]> rfCnts
[ rfHighest
]){
917 rfHighest3
= rfHighest2
;
918 rfHighest2
= rfHighest
;
920 } else if ( rfCnts
[ i
]> rfCnts
[ rfHighest2
]){
921 rfHighest3
= rfHighest2
;
923 } else if ( rfCnts
[ i
]> rfCnts
[ rfHighest3
]){
926 if ( g_debugMode
== 2 ) prnt ( "DEBUG FSK: RF %d, cnts %d" , rfLens
[ i
], rfCnts
[ i
]);
928 // set allowed clock remainder tolerance to be 1 large field clock length+1
929 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
930 uint8_t tol1
= fcHigh
+ 1 ;
932 if ( g_debugMode
== 2 ) prnt ( "DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d" , rfLens
[ rfHighest
], rfLens
[ rfHighest2
], rfLens
[ rfHighest3
]);
934 // loop to find the highest clock that has a remainder less than the tolerance
935 // compare samples counted divided by
936 // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less)
939 if ( rfLens
[ rfHighest
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest
] % clk
[ ii
] > clk
[ ii
]- tol1
){
940 if ( rfLens
[ rfHighest2
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest2
] % clk
[ ii
] > clk
[ ii
]- tol1
){
941 if ( rfLens
[ rfHighest3
] % clk
[ ii
] < tol1
|| rfLens
[ rfHighest3
] % clk
[ ii
] > clk
[ ii
]- tol1
){
942 if ( g_debugMode
== 2 ) prnt ( "DEBUG FSK: clk %d divides into the 3 most rf values within tolerance" , clk
[ ii
]);
949 if ( ii
< 2 ) return 0 ; // oops we went too far
954 //**********************************************************************************************
955 //--------------------Modulation Demods &/or Decoding Section-----------------------------------
956 //**********************************************************************************************
958 // look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index...
959 bool findST ( int * stStopLoc
, int * stStartIdx
, int lowToLowWaveLen
[], int highToLowWaveLen
[], int clk
, int tol
, int buffSize
, size_t * i
) {
960 if ( buffSize
< * i
+ 4 ) return false ;
962 for (; * i
< buffSize
- 4 ; * i
+= 1 ) {
963 * stStartIdx
+= lowToLowWaveLen
[* i
]; //caution part of this wave may be data and part may be ST.... to be accounted for in main function for now...
964 if ( lowToLowWaveLen
[* i
] >= clk
* 1 - tol
&& lowToLowWaveLen
[* i
] <= ( clk
* 2 )+ tol
&& highToLowWaveLen
[* i
] < clk
+ tol
) { //1 to 2 clocks depending on 2 bits prior
965 if ( lowToLowWaveLen
[* i
+ 1 ] >= clk
* 2 - tol
&& lowToLowWaveLen
[* i
+ 1 ] <= clk
* 2 + tol
&& highToLowWaveLen
[* i
+ 1 ] > clk
* 3 / 2 - tol
) { //2 clocks and wave size is 1 1/2
966 if ( lowToLowWaveLen
[* i
+ 2 ] >= ( clk
* 3 )/ 2 - tol
&& lowToLowWaveLen
[* i
+ 2 ] <= clk
* 2 + tol
&& highToLowWaveLen
[* i
+ 2 ] > clk
- tol
) { //1 1/2 to 2 clocks and at least one full clock wave
967 if ( lowToLowWaveLen
[* i
+ 3 ] >= clk
* 1 - tol
&& lowToLowWaveLen
[* i
+ 3 ] <= clk
* 2 + tol
) { //1 to 2 clocks for end of ST + first bit
978 //attempt to identify a Sequence Terminator in ASK modulated raw wave
979 bool DetectST ( uint8_t buffer
[], size_t * size
, int * foundclock
, size_t * ststart
, size_t * stend
) {
980 size_t bufsize
= * size
;
981 //need to loop through all samples and identify our clock, look for the ST pattern
984 int j
= 0 , high
, low
, skip
= 0 , start
= 0 , end
= 0 , minClk
= 255 ;
986 //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
987 int tmpbuff
[ bufsize
/ LOWEST_DEFAULT_CLOCK
]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured
988 int waveLen
[ bufsize
/ LOWEST_DEFAULT_CLOCK
]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed...
989 //size_t testsize = (bufsize < 512) ? bufsize : 512;
992 memset ( tmpbuff
, 0 , sizeof ( tmpbuff
));
993 memset ( waveLen
, 0 , sizeof ( waveLen
));
995 if (! loadWaveCounters ( buffer
, bufsize
, tmpbuff
, waveLen
, & j
, & skip
, & minClk
, & high
, & low
)) return false ;
996 // set clock - might be able to get this externally and remove this work...
997 clk
= getClosestClock ( minClk
);
998 // clock not found - ERROR
1000 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: clock not found - quitting" );
1006 if (! findST (& start
, & skip
, tmpbuff
, waveLen
, clk
, tol
, j
, & i
)) {
1007 // first ST not found - ERROR
1008 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: first STT not found - quitting" );
1011 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: first STT found at wave: %i, skip: %i, j=%i" , start
, skip
, j
);
1013 if ( waveLen
[ i
+ 2 ] > clk
* 1 + tol
)
1018 // skip over the remainder of ST
1019 skip
+= clk
* 7 / 2 ; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point
1021 // now do it again to find the end
1025 if (! findST (& dummy1
, & end
, tmpbuff
, waveLen
, clk
, tol
, j
, & i
)) {
1026 //didn't find second ST - ERROR
1027 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: second STT not found - quitting" );
1031 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: start of data: %d end of data: %d, datalen: %d, clk: %d, bits: %d, phaseoff: %d" , skip
, end
, end
- skip
, clk
, ( end
- skip
)/ clk
, phaseoff
);
1032 //now begin to trim out ST so we can use normal demod cmds
1034 size_t datalen
= end
- start
;
1035 // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock
1036 if ( clk
- ( datalen
% clk
) <= clk
/ 8 ) {
1037 // padd the amount off - could be problematic... but shouldn't happen often
1038 datalen
+= clk
- ( datalen
% clk
);
1039 } else if ( ( datalen
% clk
) <= clk
/ 8 ) {
1040 // padd the amount off - could be problematic... but shouldn't happen often
1041 datalen
-= datalen
% clk
;
1043 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting" , datalen
, clk
, datalen
% clk
);
1046 // if datalen is less than one t55xx block - ERROR
1047 if ( datalen
/ clk
< 8 * 4 ) {
1048 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: datalen is less than 1 full t55xx block - quitting" );
1051 size_t dataloc
= start
;
1052 if ( buffer
[ dataloc
-( clk
* 4 )-( clk
/ 4 )] <= low
&& buffer
[ dataloc
] <= low
&& buffer
[ dataloc
-( clk
* 4 )] >= high
) {
1053 //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start
1054 for ( i
= 0 ; i
<= ( clk
/ 4 ); ++ i
) {
1055 if ( buffer
[ dataloc
- ( clk
* 4 ) - i
] <= low
) {
1064 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: Starting STT trim - start: %d, datalen: %d " , dataloc
, datalen
);
1065 bool firstrun
= true ;
1066 // warning - overwriting buffer given with raw wave data with ST removed...
1067 while ( dataloc
< bufsize
-( clk
/ 2 ) ) {
1068 //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part)
1069 if ( buffer
[ dataloc
]< high
&& buffer
[ dataloc
]> low
&& buffer
[ dataloc
+ clk
/ 4 ]< high
&& buffer
[ dataloc
+ clk
/ 4 ]> low
) {
1070 for ( i
= 0 ; i
< clk
/ 2 - tol
; ++ i
) {
1071 buffer
[ dataloc
+ i
] = high
+ 5 ;
1073 } //test for small spike outlier (high between two lows) in the case of very strong waves
1074 if ( buffer
[ dataloc
] > low
&& buffer
[ dataloc
+ clk
/ 4 ] <= low
) {
1075 for ( i
= 0 ; i
< clk
/ 4 ; ++ i
) {
1076 buffer
[ dataloc
+ i
] = buffer
[ dataloc
+ clk
/ 4 ];
1081 * ststart
= dataloc
-( clk
* 4 );
1084 for ( i
= 0 ; i
< datalen
; ++ i
) {
1085 if ( i
+ newloc
< bufsize
) {
1086 if ( i
+ newloc
< dataloc
)
1087 buffer
[ i
+ newloc
] = buffer
[ dataloc
];
1093 //skip next ST - we just assume it will be there from now on...
1094 if ( g_debugMode
== 2 ) prnt ( "DEBUG STT: skipping STT at %d to %d" , dataloc
, dataloc
+( clk
* 4 ));
1102 //take 11 10 01 11 00 and make 01100 ... miller decoding
1103 //check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
1104 //decodes miller encoded binary
1105 //NOTE askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!
1106 int millerRawDecode ( uint8_t * BitStream
, size_t * size
, int invert
) {
1107 if (* size
< 16 ) return - 1 ;
1108 uint16_t MaxBits
= 512 , errCnt
= 0 ;
1110 uint8_t alignCnt
= 0 , curBit
= BitStream
[ 0 ], alignedIdx
= 0 ;
1111 uint8_t halfClkErr
= 0 ;
1112 //find alignment, needs 4 1s or 0s to properly align
1113 for ( i
= 1 ; i
< * size
- 1 ; i
++) {
1114 alignCnt
= ( BitStream
[ i
] == curBit
) ? alignCnt
+ 1 : 0 ;
1115 curBit
= BitStream
[ i
];
1116 if ( alignCnt
== 4 ) break ;
1118 // for now error if alignment not found. later add option to run it with multiple offsets...
1119 if ( alignCnt
!= 4 ) {
1120 if ( g_debugMode
) prnt ( "ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it" );
1123 alignedIdx
= ( i
- 1 ) % 2 ;
1124 for ( i
= alignedIdx
; i
< * size
- 3 ; i
+= 2 ) {
1125 halfClkErr
= ( uint8_t )(( halfClkErr
<< 1 | BitStream
[ i
]) & 0xFF );
1126 if ( ( halfClkErr
& 0x7 ) == 5 || ( halfClkErr
& 0x7 ) == 2 || ( i
> 2 && ( halfClkErr
& 0x7 ) == 0 ) || ( halfClkErr
& 0x1F ) == 0x1F ) {
1128 BitStream
[ bitCnt
++] = 7 ;
1131 BitStream
[ bitCnt
++] = BitStream
[ i
] ^ BitStream
[ i
+ 1 ] ^ invert
;
1133 if ( bitCnt
> MaxBits
) break ;
1140 //take 01 or 10 = 1 and 11 or 00 = 0
1141 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
1142 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
1143 int BiphaseRawDecode ( uint8_t * BitStream
, size_t * size
, int * offset
, int invert
) {
1144 uint16_t bitnum
= 0 ;
1145 uint16_t errCnt
= 0 ;
1147 uint16_t MaxBits
= 512 ;
1148 //if not enough samples - error
1149 if (* size
< 51 ) return - 1 ;
1150 //check for phase change faults - skip one sample if faulty
1151 uint8_t offsetA
= 1 , offsetB
= 1 ;
1153 if ( BitStream
[ i
+ 1 ]== BitStream
[ i
+ 2 ]) offsetA
= 0 ;
1154 if ( BitStream
[ i
+ 2 ]== BitStream
[ i
+ 3 ]) offsetB
= 0 ;
1156 if (! offsetA
&& offsetB
) * offset
+= 1 ;
1157 for ( i
=* offset
; i
<* size
- 3 ; i
+= 2 ){
1158 //check for phase error
1159 if ( BitStream
[ i
+ 1 ]== BitStream
[ i
+ 2 ]) {
1160 BitStream
[ bitnum
++]= 7 ;
1163 if (( BitStream
[ i
]== 1 && BitStream
[ i
+ 1 ]== 0 ) || ( BitStream
[ i
]== 0 && BitStream
[ i
+ 1 ]== 1 )){
1164 BitStream
[ bitnum
++]= 1 ^ invert
;
1165 } else if (( BitStream
[ i
]== 0 && BitStream
[ i
+ 1 ]== 0 ) || ( BitStream
[ i
]== 1 && BitStream
[ i
+ 1 ]== 1 )){
1166 BitStream
[ bitnum
++]= invert
;
1168 BitStream
[ bitnum
++]= 7 ;
1171 if ( bitnum
> MaxBits
) break ;
1178 //take 10 and 01 and manchester decode
1179 //run through 2 times and take least errCnt
1180 int manrawdecode ( uint8_t * BitStream
, size_t * size
, uint8_t invert
, uint8_t * alignPos
) {
1181 uint16_t bitnum
= 0 , MaxBits
= 512 , errCnt
= 0 ;
1183 uint16_t bestErr
= 1000 , bestRun
= 0 ;
1184 if (* size
< 16 ) return - 1 ;
1185 //find correct start position [alignment]
1186 for ( ii
= 0 ; ii
< 2 ;++ ii
){
1187 for ( i
= ii
; i
<* size
- 3 ; i
+= 2 )
1188 if ( BitStream
[ i
]== BitStream
[ i
+ 1 ])
1191 if ( bestErr
> errCnt
){
1199 for ( i
= bestRun
; i
< * size
- 3 ; i
+= 2 ){
1200 if ( BitStream
[ i
] == 1 && ( BitStream
[ i
+ 1 ] == 0 )){
1201 BitStream
[ bitnum
++]= invert
;
1202 } else if (( BitStream
[ i
] == 0 ) && BitStream
[ i
+ 1 ] == 1 ){
1203 BitStream
[ bitnum
++]= invert
^ 1 ;
1205 BitStream
[ bitnum
++]= 7 ;
1207 if ( bitnum
> MaxBits
) break ;
1214 //demodulates strong heavily clipped samples
1215 int cleanAskRawDemod ( uint8_t * BinStream
, size_t * size
, int clk
, int invert
, int high
, int low
, int * startIdx
)
1218 size_t bitCnt
= 0 , smplCnt
= 1 , errCnt
= 0 ;
1219 bool waveHigh
= ( BinStream
[ 0 ] >= high
);
1220 for ( size_t i
= 1 ; i
< * size
; i
++){
1221 if ( BinStream
[ i
] >= high
&& waveHigh
){
1223 } else if ( BinStream
[ i
] <= low
&& ! waveHigh
){
1225 } else { //transition
1226 if (( BinStream
[ i
] >= high
&& ! waveHigh
) || ( BinStream
[ i
] <= low
&& waveHigh
)){
1227 if ( smplCnt
> clk
-( clk
/ 4 )- 1 ) { //full clock
1228 if ( smplCnt
> clk
+ ( clk
/ 4 )+ 1 ) { //too many samples
1230 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: Modulation Error at: %u" , i
);
1231 BinStream
[ bitCnt
++] = 7 ;
1232 } else if ( waveHigh
) {
1233 BinStream
[ bitCnt
++] = invert
;
1234 BinStream
[ bitCnt
++] = invert
;
1235 } else if (! waveHigh
) {
1236 BinStream
[ bitCnt
++] = invert
^ 1 ;
1237 BinStream
[ bitCnt
++] = invert
^ 1 ;
1239 if (* startIdx
== 0 ) * startIdx
= i
- clk
;
1240 waveHigh
= ! waveHigh
;
1242 } else if ( smplCnt
> ( clk
/ 2 ) - ( clk
/ 4 )- 1 ) { //half clock
1244 BinStream
[ bitCnt
++] = invert
;
1245 } else if (! waveHigh
) {
1246 BinStream
[ bitCnt
++] = invert
^ 1 ;
1248 if (* startIdx
== 0 ) * startIdx
= i
-( clk
/ 2 );
1249 waveHigh
= ! waveHigh
;
1253 //transition bit oops
1255 } else { //haven't hit new high or new low yet
1265 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
1266 int askdemod_ext ( uint8_t * BinStream
, size_t * size
, int * clk
, int * invert
, int maxErr
, uint8_t amp
, uint8_t askType
, int * startIdx
) {
1267 if (* size
== 0 ) return - 1 ;
1268 int start
= DetectASKClock ( BinStream
, * size
, clk
, maxErr
); //clock default
1269 if (* clk
== 0 || start
< 0 ) return - 3 ;
1270 if (* invert
!= 1 ) * invert
= 0 ;
1271 if ( amp
== 1 ) askAmp ( BinStream
, * size
);
1272 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: clk %d, beststart %d, amp %d" , * clk
, start
, amp
);
1274 //start pos from detect ask clock is 1/2 clock offset
1275 // NOTE: can be negative (demod assumes rest of wave was there)
1276 * startIdx
= start
- (* clk
/ 2 );
1277 uint8_t initLoopMax
= 255 ;
1278 if ( initLoopMax
> * size
) initLoopMax
= * size
;
1279 // Detect high and lows
1280 //25% clip in case highs and lows aren't clipped [marshmellow]
1282 if ( getHiLo ( BinStream
, initLoopMax
, & high
, & low
, 75 , 75 ) < 1 )
1283 return - 2 ; //just noise
1286 // if clean clipped waves detected run alternate demod
1287 if ( DetectCleanAskWave ( BinStream
, * size
, high
, low
)) {
1288 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: Clean Wave Detected - using clean wave demod" );
1289 errCnt
= cleanAskRawDemod ( BinStream
, size
, * clk
, * invert
, high
, low
, startIdx
);
1290 if ( askType
) { //askman
1291 uint8_t alignPos
= 0 ;
1292 errCnt
= manrawdecode ( BinStream
, size
, 0 , & alignPos
);
1293 * startIdx
+= * clk
/ 2 * alignPos
;
1294 if ( g_debugMode
) prnt ( "DEBUG ASK CLEAN: startIdx %i, alignPos %u" , * startIdx
, alignPos
);
1300 if ( g_debugMode
) prnt ( "DEBUG ASK WEAK: startIdx %i" , * startIdx
);
1301 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: Weak Wave Detected - using weak wave demod" );
1303 int lastBit
; //set first clock check - can go negative
1304 size_t i
, bitnum
= 0 ; //output counter
1306 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
1307 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
1308 size_t MaxBits
= 3072 ; //max bits to collect
1309 lastBit
= start
- * clk
;
1311 for ( i
= start
; i
< * size
; ++ i
) {
1312 if ( i
- lastBit
>= * clk
- tol
){
1313 if ( BinStream
[ i
] >= high
) {
1314 BinStream
[ bitnum
++] = * invert
;
1315 } else if ( BinStream
[ i
] <= low
) {
1316 BinStream
[ bitnum
++] = * invert
^ 1 ;
1317 } else if ( i
- lastBit
>= * clk
+ tol
) {
1319 if ( g_debugMode
== 2 ) prnt ( "DEBUG ASK: Modulation Error at: %u" , i
);
1320 BinStream
[ bitnum
++]= 7 ;
1323 } else { //in tolerance - looking for peak
1328 } else if ( i
- lastBit
>= (* clk
/ 2 - tol
) && ! midBit
&& ! askType
){
1329 if ( BinStream
[ i
] >= high
) {
1330 BinStream
[ bitnum
++] = * invert
;
1331 } else if ( BinStream
[ i
] <= low
) {
1332 BinStream
[ bitnum
++] = * invert
^ 1 ;
1333 } else if ( i
- lastBit
>= * clk
/ 2 + tol
) {
1334 BinStream
[ bitnum
] = BinStream
[ bitnum
- 1 ];
1336 } else { //in tolerance - looking for peak
1341 if ( bitnum
>= MaxBits
) break ;
1347 int askdemod ( uint8_t * BinStream
, size_t * size
, int * clk
, int * invert
, int maxErr
, uint8_t amp
, uint8_t askType
) {
1349 return askdemod_ext ( BinStream
, size
, clk
, invert
, maxErr
, amp
, askType
, & start
);
1352 // by marshmellow - demodulate NRZ wave - requires a read with strong signal
1353 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1354 int nrzRawDemod ( uint8_t * dest
, size_t * size
, int * clk
, int * invert
, int * startIdx
) {
1355 if ( justNoise ( dest
, * size
)) return - 1 ;
1356 size_t clkStartIdx
= 0 ;
1357 * clk
= DetectNRZClock ( dest
, * size
, * clk
, & clkStartIdx
);
1358 if (* clk
== 0 ) return - 2 ;
1359 size_t i
, gLen
= 4096 ;
1360 if ( gLen
>* size
) gLen
= * size
- 20 ;
1362 if ( getHiLo ( dest
, gLen
, & high
, & low
, 75 , 75 ) < 1 ) return - 3 ; //25% fuzz on high 25% fuzz on low
1365 //convert wave samples to 1's and 0's
1366 for ( i
= 20 ; i
< * size
- 20 ; i
++){
1367 if ( dest
[ i
] >= high
) bit
= 1 ;
1368 if ( dest
[ i
] <= low
) bit
= 0 ;
1371 //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit)
1374 for ( i
= 21 ; i
< * size
- 20 ; i
++) {
1375 //if transition detected or large number of same bits - store the passed bits
1376 if ( dest
[ i
] != dest
[ i
- 1 ] || ( i
- lastBit
) == ( 10 * * clk
)) {
1377 memset ( dest
+ numBits
, dest
[ i
- 1 ] ^ * invert
, ( i
- lastBit
+ (* clk
/ 4 )) / * clk
);
1378 numBits
+= ( i
- lastBit
+ (* clk
/ 4 )) / * clk
;
1380 * startIdx
= i
- ( numBits
* * clk
);
1381 if ( g_debugMode
== 2 ) prnt ( "DEBUG NRZ: startIdx %i" , * startIdx
);
1390 //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq])
1391 size_t fsk_wave_demod ( uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
, int * startIdx
) {
1392 size_t last_transition
= 0 ;
1394 if ( fchigh
== 0 ) fchigh
= 10 ;
1395 if ( fclow
== 0 ) fclow
= 8 ;
1396 //set the threshold close to 0 (graph) or 128 std to avoid static
1397 size_t preLastSample
= 0 ;
1398 size_t LastSample
= 0 ;
1399 size_t currSample
= 0 ;
1400 if ( size
< 1024 ) return 0 ; // not enough samples
1402 //find start of modulating data in trace
1403 idx
= findModStart ( dest
, size
, fchigh
);
1404 // Need to threshold first sample
1405 if ( dest
[ idx
] < FSK_PSK_THRESHOLD
) dest
[ 0 ] = 0 ;
1408 last_transition
= idx
;
1411 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
1412 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with anywhere
1413 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
1414 // (could also be fc/5 && fc/7 for fsk1 = 4-9)
1415 for (; idx
< size
; idx
++) {
1416 // threshold current value
1417 if ( dest
[ idx
] < FSK_PSK_THRESHOLD
) dest
[ idx
] = 0 ;
1420 // Check for 0->1 transition
1421 if ( dest
[ idx
- 1 ] < dest
[ idx
]) {
1422 preLastSample
= LastSample
;
1423 LastSample
= currSample
;
1424 currSample
= idx
- last_transition
;
1425 if ( currSample
< ( fclow
- 2 )) { //0-5 = garbage noise (or 0-3)
1426 //do nothing with extra garbage
1427 } else if ( currSample
< ( fchigh
- 1 )) { //6-8 = 8 sample waves (or 3-6 = 5)
1428 //correct previous 9 wave surrounded by 8 waves (or 6 surrounded by 5)
1429 if ( numBits
> 1 && LastSample
> ( fchigh
- 2 ) && ( preLastSample
< ( fchigh
- 1 ))){
1433 if ( numBits
> 0 && * startIdx
== 0 ) * startIdx
= idx
- fclow
;
1434 } else if ( currSample
> ( fchigh
+ 1 ) && numBits
< 3 ) { //12 + and first two bit = unusable garbage
1435 //do nothing with beginning garbage and reset.. should be rare..
1437 } else if ( currSample
== ( fclow
+ 1 ) && LastSample
== ( fclow
- 1 )) { // had a 7 then a 9 should be two 8's (or 4 then a 6 should be two 5's)
1439 if ( numBits
> 0 && * startIdx
== 0 ) * startIdx
= idx
- fclow
;
1440 } else { //9+ = 10 sample waves (or 6+ = 7)
1442 if ( numBits
> 0 && * startIdx
== 0 ) * startIdx
= idx
- fchigh
;
1444 last_transition
= idx
;
1447 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
1450 //translate 11111100000 to 10
1451 //rfLen = clock, fchigh = larger field clock, fclow = smaller field clock
1452 size_t aggregate_bits ( uint8_t * dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
, int * startIdx
) {
1453 uint8_t lastval
= dest
[ 0 ];
1457 for ( idx
= 1 ; idx
< size
; idx
++) {
1459 if ( dest
[ idx
]== lastval
) continue ; //skip until we hit a transition
1461 //find out how many bits (n) we collected (use 1/2 clk tolerance)
1462 //if lastval was 1, we have a 1->0 crossing
1463 if ( dest
[ idx
- 1 ]== 1 ) {
1464 n
= ( n
* fclow
+ rfLen
/ 2 ) / rfLen
;
1465 } else { // 0->1 crossing
1466 n
= ( n
* fchigh
+ rfLen
/ 2 ) / rfLen
;
1470 //first transition - save startidx
1472 if ( lastval
== 1 ) { //high to low
1473 * startIdx
+= ( fclow
* idx
) - ( n
* rfLen
);
1474 if ( g_debugMode
== 2 ) prnt ( "DEBUG FSK: startIdx %i, fclow*idx %i, n*rflen %u" , * startIdx
, fclow
*( idx
), n
* rfLen
);
1476 * startIdx
+= ( fchigh
* idx
) - ( n
* rfLen
);
1477 if ( g_debugMode
== 2 ) prnt ( "DEBUG FSK: startIdx %i, fchigh*idx %i, n*rflen %u" , * startIdx
, fchigh
*( idx
), n
* rfLen
);
1481 //add to our destination the bits we collected
1482 memset ( dest
+ numBits
, dest
[ idx
- 1 ]^ invert
, n
);
1487 // if valid extra bits at the end were all the same frequency - add them in
1488 if ( n
> rfLen
/ fchigh
) {
1489 if ( dest
[ idx
- 2 ]== 1 ) {
1490 n
= ( n
* fclow
+ rfLen
/ 2 ) / rfLen
;
1492 n
= ( n
* fchigh
+ rfLen
/ 2 ) / rfLen
;
1494 memset ( dest
+ numBits
, dest
[ idx
- 1 ]^ invert
, n
);
1500 //by marshmellow (from holiman's base)
1501 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
1502 int fskdemod ( uint8_t * dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
, int * startIdx
) {
1503 if ( justNoise ( dest
, size
)) return 0 ;
1505 size
= fsk_wave_demod ( dest
, size
, fchigh
, fclow
, startIdx
);
1506 size
= aggregate_bits ( dest
, size
, rfLen
, invert
, fchigh
, fclow
, startIdx
);
1511 // convert psk1 demod to psk2 demod
1512 // only transition waves are 1s
1513 void psk1TOpsk2 ( uint8_t * BitStream
, size_t size
) {
1515 uint8_t lastBit
= BitStream
[ 0 ];
1516 for (; i
< size
; i
++){
1517 if ( BitStream
[ i
]== 7 ){
1519 } else if ( lastBit
!= BitStream
[ i
]){
1520 lastBit
= BitStream
[ i
];
1530 // convert psk2 demod to psk1 demod
1531 // from only transition waves are 1s to phase shifts change bit
1532 void psk2TOpsk1 ( uint8_t * BitStream
, size_t size
) {
1534 for ( size_t i
= 0 ; i
< size
; i
++){
1535 if ( BitStream
[ i
]== 1 ){
1543 //by marshmellow - demodulate PSK1 wave
1544 //uses wave lengths (# Samples)
1545 int pskRawDemod_ext ( uint8_t dest
[], size_t * size
, int * clock
, int * invert
, int * startIdx
) {
1546 if (* size
< 170 ) return - 1 ;
1548 uint8_t curPhase
= * invert
;
1550 size_t i
= 0 , numBits
= 0 , waveStart
= 1 , waveEnd
= 0 , firstFullWave
= 0 , lastClkBit
= 0 ;
1551 uint16_t fullWaveLen
= 0 , waveLenCnt
= 0 , avgWaveVal
;
1552 uint16_t errCnt
= 0 , errCnt2
= 0 ;
1554 * clock
= DetectPSKClock ( dest
, * size
, * clock
, & firstFullWave
, & curPhase
, & fc
);
1555 if (* clock
<= 0 ) return - 1 ;
1556 //if clock detect found firstfullwave...
1557 uint16_t tol
= fc
/ 2 ;
1558 if ( firstFullWave
== 0 ) {
1559 //find start of modulating data in trace
1560 i
= findModStart ( dest
, * size
, fc
);
1561 //find first phase shift
1562 firstFullWave
= pskFindFirstPhaseShift ( dest
, * size
, & curPhase
, i
, fc
, & fullWaveLen
);
1563 if ( firstFullWave
== 0 ) {
1564 // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
1565 // so skip a little to ensure we are past any Start Signal
1566 firstFullWave
= 160 ;
1567 memset ( dest
, curPhase
, firstFullWave
/ * clock
);
1569 memset ( dest
, curPhase
^ 1 , firstFullWave
/ * clock
);
1572 memset ( dest
, curPhase
^ 1 , firstFullWave
/ * clock
);
1575 numBits
+= ( firstFullWave
/ * clock
);
1576 * startIdx
= firstFullWave
- (* clock
* numBits
)+ 2 ;
1577 //set start of wave as clock align
1578 lastClkBit
= firstFullWave
;
1579 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: firstFullWave: %u, waveLen: %u, startIdx %i" , firstFullWave
, fullWaveLen
, * startIdx
);
1580 if ( g_debugMode
== 2 ) prnt ( "DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u" , * clock
, lastClkBit
,( unsigned int ) fc
);
1582 dest
[ numBits
++] = curPhase
; //set first read bit
1583 for ( i
= firstFullWave
+ fullWaveLen
- 1 ; i
< * size
- 3 ; i
++) {
1584 //top edge of wave = start of new wave
1585 if ( dest
[ i
]+ fc
< dest
[ i
+ 1 ] && dest
[ i
+ 1 ] >= dest
[ i
+ 2 ]) {
1586 if ( waveStart
== 0 ) {
1589 avgWaveVal
= dest
[ i
+ 1 ];
1592 waveLenCnt
= waveEnd
- waveStart
;
1593 if ( waveLenCnt
> fc
) {
1594 //this wave is a phase shift
1595 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1596 if ( i
+ 1 >= lastClkBit
+ * clock
- tol
) { //should be a clock bit
1598 dest
[ numBits
++] = curPhase
;
1599 lastClkBit
+= * clock
;
1600 } else if ( i
< lastClkBit
+ 10 + fc
) {
1601 //noise after a phase shift - ignore
1602 } else { //phase shift before supposed to based on clock
1604 dest
[ numBits
++] = 7 ;
1606 } else if ( i
+ 1 > lastClkBit
+ * clock
+ tol
+ fc
) {
1607 lastClkBit
+= * clock
; //no phase shift but clock bit
1608 dest
[ numBits
++] = curPhase
;
1609 } else if ( waveLenCnt
< fc
- 1 ) { //wave is smaller than field clock (shouldn't happen often)
1611 if ( errCnt2
> 101 ) return errCnt2
;
1612 avgWaveVal
+= dest
[ i
+ 1 ];
1619 avgWaveVal
+= dest
[ i
+ 1 ];
1625 int pskRawDemod ( uint8_t dest
[], size_t * size
, int * clock
, int * invert
) {
1627 return pskRawDemod_ext ( dest
, size
, clock
, invert
, & startIdx
);
1630 //**********************************************************************************************
1631 //-----------------Tag format detection section-------------------------------------------------
1632 //**********************************************************************************************
1635 // FSK Demod then try to locate an AWID ID
1636 int AWIDdemodFSK ( uint8_t * dest
, size_t * size
, int * waveStartIdx
) {
1637 //make sure buffer has enough data
1638 if (* size
< 96 * 50 ) return - 1 ;
1641 * size
= fskdemod ( dest
, * size
, 50 , 1 , 10 , 8 , waveStartIdx
); // fsk2a RF/50
1642 if (* size
< 96 ) return - 3 ; //did we get a good demod?
1644 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
1645 size_t startIdx
= 0 ;
1646 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1647 if ( errChk
== 0 ) return - 4 ; //preamble not found
1648 if (* size
!= 96 ) return - 5 ;
1649 return ( int ) startIdx
;
1653 //takes 1s and 0s and searches for EM410x format - output EM ID
1654 uint8_t Em410xDecode ( uint8_t * BitStream
, size_t * size
, size_t * startIdx
, uint32_t * hi
, uint64_t * lo
)
1657 if (* size
< 64 ) return 0 ;
1658 if ( BitStream
[ 1 ]> 1 ) return 0 ; //allow only 1s and 0s
1660 // 111111111 bit pattern represent start of frame
1661 // include 0 in front to help get start pos
1662 uint8_t preamble
[] = { 0 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 };
1664 uint8_t FmtLen
= 10 ; // sets of 4 bits = end data
1666 errChk
= preambleSearch ( BitStream
, preamble
, sizeof ( preamble
), size
, startIdx
);
1667 if ( errChk
== 0 || (* size
!= 64 && * size
!= 128 ) ) return 0 ;
1668 if (* size
== 128 ) FmtLen
= 22 ; // 22 sets of 4 bits
1670 //skip last 4bit parity row for simplicity
1671 * size
= removeParity ( BitStream
, * startIdx
+ sizeof ( preamble
), 5 , 0 , FmtLen
* 5 );
1672 if (* size
== 40 ) { // std em410x format
1674 * lo
= (( uint64_t )( bytebits_to_byte ( BitStream
, 8 )) << 32 ) | ( bytebits_to_byte ( BitStream
+ 8 , 32 ));
1675 } else if (* size
== 88 ) { // long em format
1676 * hi
= ( bytebits_to_byte ( BitStream
, 24 ));
1677 * lo
= (( uint64_t )( bytebits_to_byte ( BitStream
+ 24 , 32 )) << 32 ) | ( bytebits_to_byte ( BitStream
+ 24 + 32 , 32 ));
1679 if ( g_debugMode
) prnt ( "Error removing parity: %u" , * size
);
1685 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
1686 // BitStream must contain previously askrawdemod and biphasedemoded data
1687 int FDXBdemodBI ( uint8_t * dest
, size_t * size
) {
1688 //make sure buffer has enough data
1689 if (* size
< 128 ) return - 1 ;
1691 size_t startIdx
= 0 ;
1692 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
1694 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1695 if ( errChk
== 0 ) return - 2 ; //preamble not found
1696 if (* size
!= 128 ) return - 3 ; //wrong size for fdxb
1697 //return start position
1698 return ( int ) startIdx
;
1702 // demod gProxIIDemod
1703 // error returns as -x
1704 // success returns start position in BitStream
1705 // BitStream must contain previously askrawdemod and biphasedemoded data
1706 int gProxII_Demod ( uint8_t BitStream
[], size_t * size
) {
1708 uint8_t preamble
[] = { 1 , 1 , 1 , 1 , 1 , 0 };
1710 uint8_t errChk
= preambleSearch ( BitStream
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1711 if ( errChk
== 0 ) return - 3 ; //preamble not found
1712 if (* size
!= 96 ) return - 2 ; //should have found 96 bits
1713 //check first 6 spacer bits to verify format
1714 if (! BitStream
[ startIdx
+ 5 ] && ! BitStream
[ startIdx
+ 10 ] && ! BitStream
[ startIdx
+ 15 ] && ! BitStream
[ startIdx
+ 20 ] && ! BitStream
[ startIdx
+ 25 ] && ! BitStream
[ startIdx
+ 30 ]){
1715 //confirmed proper separator bits found
1716 //return start position
1717 return ( int ) startIdx
;
1719 return - 5 ; //spacer bits not found - not a valid gproxII
1722 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
1723 int HIDdemodFSK ( uint8_t * dest
, size_t * size
, uint32_t * hi2
, uint32_t * hi
, uint32_t * lo
, int * waveStartIdx
) {
1724 size_t numStart
= 0 , size2
=* size
, startIdx
= 0 ;
1725 // FSK demodulator fsk2a so invert and fc/10/8
1726 * size
= fskdemod ( dest
, size2
, 50 , 1 , 10 , 8 , waveStartIdx
);
1727 if (* size
< 96 * 2 ) return - 2 ;
1728 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
1729 uint8_t preamble
[] = { 0 , 0 , 0 , 1 , 1 , 1 , 0 , 1 };
1730 // find bitstring in array
1731 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1732 if ( errChk
== 0 ) return - 3 ; //preamble not found
1734 numStart
= startIdx
+ sizeof ( preamble
);
1735 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
1736 for ( size_t idx
= numStart
; ( idx
- numStart
) < * size
- sizeof ( preamble
); idx
+= 2 ){
1737 if ( dest
[ idx
] == dest
[ idx
+ 1 ]){
1738 return - 4 ; //not manchester data
1740 * hi2
= (* hi2
<< 1 )|(* hi
>> 31 );
1741 * hi
= (* hi
<< 1 )|(* lo
>> 31 );
1742 //Then, shift in a 0 or one into low
1743 if ( dest
[ idx
] && ! dest
[ idx
+ 1 ]) // 1 0
1748 return ( int ) startIdx
;
1751 int IOdemodFSK ( uint8_t * dest
, size_t size
, int * waveStartIdx
) {
1752 //make sure buffer has data
1753 if ( size
< 66 * 64 ) return - 2 ;
1754 // FSK demodulator RF/64, fsk2a so invert, and fc/10/8
1755 size
= fskdemod ( dest
, size
, 64 , 1 , 10 , 8 , waveStartIdx
);
1756 if ( size
< 65 ) return - 3 ; //did we get a good demod?
1758 //0 10 20 30 40 50 60
1760 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
1761 //-----------------------------------------------------------------------------
1762 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
1764 //XSF(version)facility:codeone+codetwo
1766 size_t startIdx
= 0 ;
1767 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
1768 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), & size
, & startIdx
);
1769 if ( errChk
== 0 ) return - 4 ; //preamble not found
1771 if (! dest
[ startIdx
+ 8 ] && dest
[ startIdx
+ 17 ]== 1 && dest
[ startIdx
+ 26 ]== 1 && dest
[ startIdx
+ 35 ]== 1 && dest
[ startIdx
+ 44 ]== 1 && dest
[ startIdx
+ 53 ]== 1 ){
1772 //confirmed proper separator bits found
1773 //return start position
1774 return ( int ) startIdx
;
1779 // redesigned by marshmellow adjusted from existing decode functions
1780 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1781 int indala26decode ( uint8_t * bitStream
, size_t * size
, uint8_t * invert
) {
1782 //26 bit 40134 format (don't know other formats)
1783 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 };
1784 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 };
1785 size_t startidx
= 0 ;
1786 if (! preambleSearch ( bitStream
, preamble
, sizeof ( preamble
), size
, & startidx
)){
1787 // if didn't find preamble try again inverting
1788 if (! preambleSearch ( bitStream
, preamble_i
, sizeof ( preamble_i
), size
, & startidx
)) return - 1 ;
1791 if (* size
!= 64 && * size
!= 224 ) return - 2 ;
1793 for ( size_t i
= startidx
; i
< * size
+ startidx
; i
++)
1796 return ( int ) startidx
;
1799 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
1800 int ParadoxdemodFSK ( uint8_t * dest
, size_t * size
, uint32_t * hi2
, uint32_t * hi
, uint32_t * lo
, int * waveStartIdx
) {
1801 size_t numStart
= 0 , size2
=* size
, startIdx
= 0 ;
1803 * size
= fskdemod ( dest
, size2
, 50 , 1 , 10 , 8 , waveStartIdx
); //fsk2a
1804 if (* size
< 96 ) return - 2 ;
1806 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
1807 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 1 , 1 , 1 , 1 };
1809 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1810 if ( errChk
== 0 ) return - 3 ; //preamble not found
1812 numStart
= startIdx
+ sizeof ( preamble
);
1813 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
1814 for ( size_t idx
= numStart
; ( idx
- numStart
) < * size
- sizeof ( preamble
); idx
+= 2 ){
1815 if ( dest
[ idx
] == dest
[ idx
+ 1 ])
1816 return - 4 ; //not manchester data
1817 * hi2
= (* hi2
<< 1 )|(* hi
>> 31 );
1818 * hi
= (* hi
<< 1 )|(* lo
>> 31 );
1819 //Then, shift in a 0 or one into low
1820 if ( dest
[ idx
] && ! dest
[ idx
+ 1 ]) // 1 0
1825 return ( int ) startIdx
;
1828 // find presco preamble 0x10D in already demoded data
1829 int PrescoDemod ( uint8_t * dest
, size_t * size
) {
1830 //make sure buffer has data
1831 if (* size
< 64 * 2 ) return - 2 ;
1833 size_t startIdx
= 0 ;
1834 uint8_t preamble
[] = { 1 , 0 , 0 , 0 , 0 , 1 , 1 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
1835 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1836 if ( errChk
== 0 ) return - 4 ; //preamble not found
1837 //return start position
1838 return ( int ) startIdx
;
1842 // FSK Demod then try to locate a Farpointe Data (pyramid) ID
1843 int PyramiddemodFSK ( uint8_t * dest
, size_t * size
, int * waveStartIdx
) {
1844 //make sure buffer has data
1845 if (* size
< 128 * 50 ) return - 5 ;
1848 * size
= fskdemod ( dest
, * size
, 50 , 1 , 10 , 8 , waveStartIdx
); // fsk2a RF/50
1849 if (* size
< 128 ) return - 2 ; //did we get a good demod?
1851 uint8_t preamble
[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 };
1852 size_t startIdx
= 0 ;
1853 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1854 if ( errChk
== 0 ) return - 4 ; //preamble not found
1855 if (* size
!= 128 ) return - 3 ;
1856 return ( int ) startIdx
;
1860 // find viking preamble 0xF200 in already demoded data
1861 int VikingDemod_AM ( uint8_t * dest
, size_t * size
) {
1862 //make sure buffer has data
1863 if (* size
< 64 * 2 ) return - 2 ;
1865 size_t startIdx
= 0 ;
1866 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 };
1867 uint8_t errChk
= preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
);
1868 if ( errChk
== 0 ) return - 4 ; //preamble not found
1869 uint32_t checkCalc
= bytebits_to_byte ( dest
+ startIdx
, 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 8 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 16 , 8 )
1870 ^ bytebits_to_byte ( dest
+ startIdx
+ 24 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 32 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 40 , 8 )
1871 ^ bytebits_to_byte ( dest
+ startIdx
+ 48 , 8 ) ^ bytebits_to_byte ( dest
+ startIdx
+ 56 , 8 );
1872 if ( checkCalc
!= 0xA8 ) return - 5 ;
1873 if (* size
!= 64 ) return - 6 ;
1874 //return start position
1875 return ( int ) startIdx
;
1879 // find Visa2000 preamble in already demoded data
1880 int Visa2kDemod_AM ( uint8_t * dest
, size_t * size
) {
1881 if (* size
< 96 ) return - 1 ; //make sure buffer has data
1882 size_t startIdx
= 0 ;
1883 uint8_t preamble
[] = { 0 , 1 , 0 , 1 , 0 , 1 , 1 , 0 , 0 , 1 , 0 , 0 , 1 , 0 , 0 , 1 , 0 , 1 , 0 , 1 , 0 , 0 , 1 , 1 , 0 , 0 , 1 , 1 , 0 , 0 , 1 , 0 };
1884 if ( preambleSearch ( dest
, preamble
, sizeof ( preamble
), size
, & startIdx
) == 0 )
1885 return - 2 ; //preamble not found
1886 if (* size
!= 96 ) return - 3 ; //wrong demoded size
1887 //return start position
1888 return ( int ) startIdx
;