]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
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 commands
9 //-----------------------------------------------------------------------------
17 //takes 1s and 0s and searches for EM410x format - output EM ID
18 uint64_t Em410xDecode(uint8_t *BitStream
, uint32_t BitLen
)
20 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
21 // otherwise could be a void with no arguments
23 int high
= 0, low
= 128;
26 uint32_t initLoopMax
= 65;
28 if (initLoopMax
> BitLen
)
31 for (; i
< initLoopMax
; ++i
) //65 samples should be plenty to find high and low values
33 if (BitStream
[i
] > high
)
35 else if (BitStream
[i
] < low
)
39 if (((high
!=1)||(low
!=0))){ //allow only 1s and 0s
43 uint8_t parityTest
= 0;
44 // 111111111 bit pattern represent start of frame
45 uint8_t frame_marker_mask
[] = {1,1,1,1,1,1,1,1,1};
49 while( (idx
+ 64) < BitLen
) {
53 // search for a start of frame marker
54 if ( memcmp(BitStream
+idx
, frame_marker_mask
, sizeof(frame_marker_mask
)) == 0) {
56 idx
+= 9;//sizeof(frame_marker_mask);
57 for ( i
= 0; i
< 10; ++i
){
58 for( j
= 0; j
< 5; ++j
){
59 parityTest
+= BitStream
[(i
*5) + j
+ idx
];
61 if (parityTest
== ( (parityTest
>> 1) << 1)){
63 for (j
= 0; j
< 4; ++j
){
64 lo
= ( lo
<< 1LL)|( BitStream
[( i
* 5 ) + j
+ idx
]);
70 if (resetCnt
> 5) return 0;
72 goto restart
;//continue;
75 //skip last 5 bit parity test for simplicity.
85 //takes 2 arguments - clock and invert both as integers
86 //attempts to demodulate ask while decoding manchester
87 //prints binary found and saves in graphbuffer for further commands
88 int askmandemod(uint8_t *BinStream
, uint32_t *BitLen
, int *clk
, int *invert
)
91 int high
= 0, low
= 128;
92 *clk
= DetectASKClock(BinStream
, (size_t)*BitLen
, *clk
); //clock default
94 if (*clk
< 8 ) *clk
= 64;
95 if (*clk
< 32 ) *clk
= 32;
96 if (*invert
!= 1) *invert
= 0;
98 uint32_t initLoopMax
= 200;
99 if (initLoopMax
> *BitLen
)
100 initLoopMax
= *BitLen
;
102 // Detect high and lows
103 // 200 samples should be enough to find high and low values
104 for (i
= 0; i
< initLoopMax
; ++i
) {
105 if (BinStream
[i
] > high
)
107 else if (BinStream
[i
] < low
)
115 //25% fuzz in case highs and lows aren't clipped [marshmellow]
116 high
= (int)(high
* .75);
117 low
= (int)(low
+128 * .25);
119 int lastBit
= 0; // set first clock check
120 uint32_t bitnum
= 0; // output counter
122 // clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
123 //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely
124 int tol
= ( *clk
== 32 ) ? 1 : 0;
127 uint32_t gLen
= *BitLen
;
129 if (gLen
> 3000) gLen
= 3000;
132 uint32_t bestStart
= *BitLen
;
133 uint32_t bestErrCnt
= (*BitLen
/1000);
134 uint32_t maxErr
= bestErrCnt
;
136 //loop to find first wave that works
137 for (j
=0; j
< gLen
; ++j
){
139 if ((BinStream
[j
] >= high
)||(BinStream
[j
] <= low
)){
143 //loop through to see if this start location works
144 for (i
= j
; i
< *BitLen
; ++i
) {
145 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
147 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
148 //low found and we are expecting a bar
151 //mid value found or no bar supposed to be here
152 if ((i
-lastBit
) > (*clk
+ tol
)){
153 //should have hit a high or low based on clock!!
156 lastBit
+= *clk
;//skip over until hit too many errors
157 if (errCnt
> maxErr
) break; //allow 1 error for every 1000 samples else start over
160 if ((i
-j
) >(400 * *clk
)) break; //got plenty of bits
162 //we got more than 64 good bits and not all errors
163 if ((((i
-j
)/ *clk
) > (64 + errCnt
)) && (errCnt
< maxErr
)) {
168 break; //great read - finish
170 if (errCnt
< bestErrCnt
){ //set this as new best run
177 if (bestErrCnt
< maxErr
){
178 //best run is good enough set to best run and set overwrite BinStream
180 lastBit
= bestStart
- *clk
;
182 for (i
= j
; i
< *BitLen
; ++i
) {
183 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
185 BinStream
[bitnum
] = *invert
;
187 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
188 //low found and we are expecting a bar
190 BinStream
[bitnum
] = 1 - *invert
;
193 //mid value found or no bar supposed to be here
194 if ((i
-lastBit
) > (*clk
+tol
)){
195 //should have hit a high or low based on clock!!
197 BinStream
[bitnum
] = 77;
200 lastBit
+= *clk
;//skip over error
203 if (bitnum
>= 400) break;
215 //take 10 and 01 and manchester decode
216 //run through 2 times and take least errCnt
217 int manrawdecode(uint8_t * bits
, int *bitlen
)
228 for ( i
= i
+ j
; i
< *bitlen
-2; i
+= 2){
229 if ( bits
[i
]==1 && (bits
[i
+1]==0)){
230 } else if ((bits
[i
]==0)&& bits
[i
+1]==1){
234 if(bitnum
> 300) break;
236 if (bestErr
> errCnt
){
246 for ( i
= i
+j
; i
< *bitlen
-2; i
+= 2){
247 if ( bits
[i
] == 1 && bits
[i
+ 1] == 0 ){
249 } else if ( bits
[i
] == 0 && bits
[i
+ 1] == 1 ){
254 if ( bitnum
> 300 ) break;
263 //take 01 or 10 = 0 and 11 or 00 = 1
264 int BiphaseRawDecode(uint8_t * bits
, int *bitlen
, int offset
)
270 for (; i
< *bitlen
-2; i
+= 2 ){
271 if ( (bits
[i
]==1 && bits
[i
+1]==0)||
272 (bits
[i
]==0 && bits
[i
+1]==1)){
274 } else if ( (bits
[i
]==0 && bits
[i
+1]==0)||
275 (bits
[i
]==1 && bits
[i
+1]==1)){
281 if ( bitnum
> 250) break;
288 //takes 2 arguments - clock and invert both as integers
289 //attempts to demodulate ask only
290 //prints binary found and saves in graphbuffer for further commands
291 int askrawdemod(uint8_t *BinStream
, int *bitLen
, int *clk
, int *invert
)
294 uint32_t initLoopMax
= 200;
295 int high
= 0, low
= 128;
296 uint8_t BitStream
[502] = {0x00};
298 *clk
= DetectASKClock(BinStream
, *bitLen
, *clk
); //clock default
300 if (*clk
< 8) *clk
= 64;
301 if (*clk
< 32) *clk
= 32;
302 if (*invert
!= 1) *invert
= 0;
304 if (initLoopMax
> *bitLen
)
305 initLoopMax
= *bitLen
;
307 // Detect high and lows
308 for (i
= 0; i
< initLoopMax
; ++i
) //200 samples should be plenty to find high and low values
310 if (BinStream
[i
] > high
)
312 else if (BinStream
[i
] < low
)
321 //25% fuzz in case highs and lows aren't clipped [marshmellow]
322 high
= (int)(high
* .75);
323 low
= (int)(low
+128 * .25);
325 int lastBit
= 0; //set first clock check
326 uint32_t bitnum
= 0; //output counter
328 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
329 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
331 uint32_t gLen
= *bitLen
;
332 if (gLen
> 500) gLen
= 500;
336 uint32_t bestStart
= *bitLen
;
337 uint32_t bestErrCnt
= (*bitLen
/ 1000);
338 uint32_t errCntLimit
= bestErrCnt
;
341 //loop to find first wave that works
342 for (j
= 0; j
< gLen
; ++j
){
344 if ((BinStream
[j
] >= high
)||(BinStream
[j
] <= low
)){
346 //loop through to see if this start location works
347 for (i
= j
; i
< *bitLen
; ++i
) {
348 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
350 BitStream
[bitnum
] = *invert
;
353 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
354 //low found and we are expecting a bar
356 BitStream
[bitnum
] = 1-*invert
;
359 } else if ((BinStream
[i
]<=low
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
362 BitStream
[bitnum
] = 1 - *invert
;
364 } else if ((BinStream
[i
]>=high
)&&(midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
367 BitStream
[bitnum
] = *invert
;
369 } else if ((i
-lastBit
)>((*clk
/2)+tol
)&&(midBit
==0)){
372 BitStream
[bitnum
] = BitStream
[bitnum
-1];
375 //mid value found or no bar supposed to be here
377 if (( i
- lastBit
) > ( *clk
+ tol
)){
378 //should have hit a high or low based on clock!!
381 BitStream
[bitnum
] = 77;
386 lastBit
+= *clk
;//skip over until hit too many errors
387 if (errCnt
> errCntLimit
){ //allow 1 error for every 1000 samples else start over
389 bitnum
= 0;//start over
394 if (bitnum
> 500) break;
396 //we got more than 64 good bits and not all errors
398 if ((bitnum
> (64 + errCnt
)) && (errCnt
< errCntLimit
)) {
400 //great read - finish
401 if (errCnt
== 0) break;
403 //if current run == bestErrCnt run (after exhausted testing) then finish
404 if (bestStart
== j
) break;
406 //set this as new best run
407 if (errCnt
< bestErrCnt
){
413 if (j
>= gLen
){ //exhausted test
414 //if there was a ok test go back to that one and re-run the best run (then dump after that run)
415 if (bestErrCnt
< errCntLimit
)
421 for (i
= 0; i
< bitnum
; ++i
){
422 BinStream
[i
] = BitStream
[i
];
430 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
431 size_t fsk_wave_demod(uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
433 uint32_t last_transition
= 0;
437 if (fchigh
== 0) fchigh
= 10;
438 if (fclow
== 0) fclow
= 8;
440 // we do care about the actual theshold value as sometimes near the center of the
441 // wave we may get static that changes direction of wave for one value
442 // if our value is too low it might affect the read. and if our tag or
443 // antenna is weak a setting too high might not see anything. [marshmellow]
447 // Find high from first 100 samples
448 for ( idx
= 1; idx
< 100; idx
++ ){
449 if ( maxVal
< dest
[idx
])
453 // set close to the top of the wave threshold with 25% margin for error
454 // less likely to get a false transition up there.
455 // (but have to be careful not to go too high and miss some short waves)
456 uint8_t threshold_value
= (uint8_t)(maxVal
* .75);
458 // sync to first lo-hi transition, and threshold
459 // Need to threshold first sample
461 dest
[0] = (dest
[0] < threshold_value
) ? 0 : 1;
465 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
466 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
467 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
468 for(idx
= 1; idx
< size
; idx
++) {
470 // threshold current value
471 dest
[idx
] = (dest
[idx
] < threshold_value
) ? 0 : 1;
473 // Check for 0->1 transition
474 if (dest
[idx
-1] < dest
[idx
]) { // 0 -> 1 transition
475 if ( ( idx
- last_transition
) <( fclow
- 2 ) ) { //0-5 = garbage noise
476 //do nothing with extra garbage
477 } else if ((idx
- last_transition
) < ( fchigh
- 1 )) { //6-8 = 8 waves
479 } else { //9+ = 10 waves
482 last_transition
= idx
;
486 //it returns the number of bytes, but each byte represents a bit: 1 or 0
490 uint32_t myround2(float f
)
492 if (f
>= 2000) return 2000;//something bad happened
493 return (uint32_t) (f
+ (float)0.5);
496 //translate 11111100000 to 10
497 size_t aggregate_bits(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t maxConsequtiveBits
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
499 uint8_t lastval
= dest
[0];
504 for( idx
= 1; idx
< size
; idx
++) {
506 if (dest
[idx
] == lastval
) {
510 //if lastval was 1, we have a 1->0 crossing
511 if ( dest
[idx
-1] == 1 ) {
512 n
= myround2( (float)( n
+ 1 ) / ((float)(rfLen
)/(float)fclow
));
513 } else { // 0->1 crossing
514 n
= myround2( (float)( n
+ 1 ) / ((float)(rfLen
-2)/(float)fchigh
)); //-2 for fudge factor
518 if(n
< maxConsequtiveBits
) //Consecutive
520 if(invert
== 0){ //invert bits
521 memset(dest
+numBits
, dest
[idx
-1] , n
);
523 memset(dest
+numBits
, dest
[idx
-1]^1 , n
);
533 //by marshmellow (from holiman's base)
534 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
535 int fskdemod(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
538 size
= fsk_wave_demod(dest
, size
, fchigh
, fclow
);
540 size
= aggregate_bits(dest
, size
, rfLen
, 192, invert
, fchigh
, fclow
);
544 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
545 int HIDdemodFSK(uint8_t *dest
, size_t size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
551 size
= fskdemod(dest
, size
, 50, 0, 10, 8);
553 // final loop, go over previously decoded manchester data and decode into usable tag ID
554 // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
555 uint8_t frame_marker_mask
[] = {1,1,1,0,0,0};
557 uint8_t mask_len
= sizeof frame_marker_mask
/ sizeof frame_marker_mask
[0];
560 while( idx
+ mask_len
< size
) {
561 // search for a start of frame marker
562 if ( memcmp(dest
+idx
, frame_marker_mask
, sizeof(frame_marker_mask
)) == 0)
563 { // frame marker found
565 while(dest
[idx
] != dest
[idx
+1] && idx
< size
-2)
567 // Keep going until next frame marker (or error)
568 // Shift in a bit. Start by shifting high registers
569 *hi2
= ( *hi2
<< 1 ) | ( *hi
>> 31 );
570 *hi
= ( *hi
<< 1 ) | ( *lo
>> 31 );
571 //Then, shift in a 0 or one into low
572 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
573 *lo
= ( *lo
<< 1 ) | 0;
575 *lo
= ( *lo
<< 1 ) | 1;
579 // Hopefully, we read a tag and hit upon the next frame marker
580 if(idx
+ mask_len
< size
)
582 if ( memcmp(dest
+idx
, frame_marker_mask
, sizeof(frame_marker_mask
)) == 0)
589 *hi2
= *hi
= *lo
= 0;
598 uint32_t bytebits_to_byte(uint8_t *src
, int numbits
)
600 //HACK: potential overflow in numbits is larger then uint32 bits.
603 for(int i
= 0 ; i
< numbits
; ++i
) {
604 num
= (num
<< 1) | (*src
);
610 int IOdemodFSK(uint8_t *dest
, size_t size
)
612 //make sure buffer has data
613 if (size
< 100) return -1;
618 //test samples are not just noise
619 for (; idx
< 65; ++idx
){
620 if (testMax
< dest
[idx
])
625 if (testMax
< 20) return -2;
628 size
= fskdemod(dest
, size
, 64, 1, 10, 8); // RF/64 and invert
630 //did we get a good demod?
631 if (size
< 65) return -3;
634 //0 10 20 30 40 50 60
636 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
637 //-----------------------------------------------------------------------------
638 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
640 //XSF(version)facility:codeone+codetwo
643 uint8_t mask
[] = {0,0,0,0,0,0,0,0,0,1};
645 for( idx
= 0; idx
< (size
- 65); ++idx
) {
646 if ( memcmp(dest
+ idx
, mask
, sizeof(mask
))==0) {
654 //confirmed proper separator bits found
655 //return start position
664 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
665 // maybe somehow adjust peak trimming value based on samples to fix?
666 int DetectASKClock(uint8_t dest
[], size_t size
, int clock
)
669 int clk
[] = {16,32,40,50,64,100,128,256};
670 uint8_t clkLen
= sizeof clk
/ sizeof clk
[0];
672 //if we already have a valid clock quit
673 for (; i
< clkLen
; ++i
)
683 //get high and low peak
684 for ( i
= 0; i
< loopCnt
; ++i
){
691 peak
= (int)(peak
* .75);
692 low
= (int)(low
+128 * .25);
694 int ii
, cnt
, bestErr
, tol
= 0;
696 memset(errCnt
, 0x00, clkLen
);
698 int tmpIndex
, tmphigh
, tmplow
;
700 //test each valid clock from smallest to greatest to see which lines up
701 for( cnt
= 0; cnt
< clkLen
; ++cnt
){
703 tol
= (clk
[cnt
] == 32) ? 1 : 0;
705 tmpIndex
= tmphigh
= tmplow
= 0;
707 //try lining up the peaks by moving starting point (try first 256)
708 for (ii
=0; ii
< loopCnt
; ++ii
){
710 // not a peak? continue
711 if ( (dest
[ii
] < peak
) && (dest
[ii
] > low
))
716 // now that we have the first one lined up test rest of wave array
717 for ( i
= 0; i
< ((int)(size
/ clk
[cnt
]) - 1); ++i
){
719 tmpIndex
= ii
+ (i
* clk
[cnt
] );
720 tmplow
= dest
[ tmpIndex
- tol
];
721 tmphigh
= dest
[ tmpIndex
+ tol
];
723 if ( dest
[tmpIndex
] >= peak
|| dest
[tmpIndex
] <= low
) {
725 else if ( tmplow
>= peak
|| tmplow
<= low
){
727 else if ( tmphigh
>= peak
|| tmphigh
<= low
){
730 errCnt
[cnt
]++; //error no peak detected
733 //if we found no errors this is correct one - return this clock
734 if ( errCnt
[cnt
] == 0 )
737 if ( errCnt
[cnt
] < bestErr
)
738 bestErr
= errCnt
[cnt
];
740 // save the least error.
741 errCnt
[cnt
] = bestErr
;
743 // find best clock which has lowest number of errors
744 int j
= 0, bestIndex
= 0;
745 for (; j
< clkLen
; ++j
){
746 if ( errCnt
[j
] < errCnt
[bestIndex
] )
749 return clk
[bestIndex
];