]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
144783198e1684a8be432a5abdcc5f5d296c5147
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 //-----------------------------------------------------------------------------
16 //takes 1s and 0s and searches for EM410x format - output EM ID
17 uint64_t Em410xDecode(uint8_t *BitStream
,uint32_t BitLen
)
19 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
20 // otherwise could be a void with no arguments
23 uint64_t lo
=0; //hi=0,
26 uint32_t initLoopMax
= 65;
27 if (initLoopMax
>BitLen
) initLoopMax
=BitLen
;
29 for (;i
< initLoopMax
; ++i
) //65 samples should be plenty to find high and low values
31 if (BitStream
[i
] > high
)
33 else if (BitStream
[i
] < low
)
36 if (((high
!=1)||(low
!=0))){ //allow only 1s and 0s
37 // PrintAndLog("no data found");
41 // 111111111 bit pattern represent start of frame
42 uint8_t frame_marker_mask
[] = {1,1,1,1,1,1,1,1,1};
46 while( (idx
+ 64) < BitLen
) {
48 // search for a start of frame marker
49 if ( memcmp(BitStream
+idx
, frame_marker_mask
, sizeof(frame_marker_mask
)) == 0)
50 { // frame marker found
51 idx
+=9;//sizeof(frame_marker_mask);
53 for(ii
=0; ii
<5; ++ii
){
54 parityTest
+= BitStream
[(i
*5)+ii
+idx
];
56 if (parityTest
== ((parityTest
>>1)<<1)){
58 for (ii
=0; ii
<4;++ii
){
59 //hi = (hi<<1)|(lo>>31);
60 lo
=(lo
<<1LL)|(BitStream
[(i
*5)+ii
+idx
]);
62 //PrintAndLog("DEBUG: EM parity passed parity val: %d, i:%d, ii:%d,idx:%d, Buffer: %d%d%d%d%d,lo: %d",parityTest,i,ii,idx,BitStream[idx+ii+(i*5)-5],BitStream[idx+ii+(i*5)-4],BitStream[idx+ii+(i*5)-3],BitStream[idx+ii+(i*5)-2],BitStream[idx+ii+(i*5)-1],lo);
63 }else {//parity failed
64 //PrintAndLog("DEBUG: EM parity failed parity val: %d, i:%d, ii:%d,idx:%d, Buffer: %d%d%d%d%d",parityTest,i,ii,idx,BitStream[idx+ii+(i*5)-5],BitStream[idx+ii+(i*5)-4],BitStream[idx+ii+(i*5)-3],BitStream[idx+ii+(i*5)-2],BitStream[idx+ii+(i*5)-1]);
67 if (resetCnt
>5)return 0;
69 goto restart
;//continue;
72 //skip last 5 bit parity test for simplicity.
82 //takes 2 arguments - clock and invert both as integers
83 //attempts to demodulate ask while decoding manchester
84 //prints binary found and saves in graphbuffer for further commands
85 int askmandemod(uint8_t * BinStream
,uint32_t *BitLen
,int *clk
, int *invert
)
88 int high
= 0, low
= 128;
89 *clk
=DetectASKClock(BinStream
,(size_t)*BitLen
,*clk
); //clock default
93 if (*invert
!= 0 && *invert
!= 1) *invert
=0;
94 uint32_t initLoopMax
= 200;
95 if (initLoopMax
>*BitLen
) initLoopMax
=*BitLen
;
96 // Detect high and lows
97 for (i
= 0; i
< initLoopMax
; ++i
) //200 samples should be enough to find high and low values
99 if (BinStream
[i
] > high
)
101 else if (BinStream
[i
] < low
)
104 if ((high
< 158) ){ //throw away static
105 //PrintAndLog("no data found");
108 //25% fuzz in case highs and lows aren't clipped [marshmellow]
109 high
=(int)((high
-128)*.75)+128;
110 low
= (int)((low
-128)*.75)+128;
112 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
113 int lastBit
= 0; //set first clock check
114 uint32_t bitnum
= 0; //output counter
115 int tol
= 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
116 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
118 uint32_t gLen
= *BitLen
;
119 if (gLen
> 3000) gLen
=3000;
121 uint32_t bestStart
= *BitLen
;
122 uint32_t bestErrCnt
= (*BitLen
/1000);
123 uint32_t maxErr
= (*BitLen
/1000);
124 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
125 //loop to find first wave that works
126 for (iii
=0; iii
< gLen
; ++iii
){
127 if ((BinStream
[iii
]>=high
)||(BinStream
[iii
]<=low
)){
130 //loop through to see if this start location works
131 for (i
= iii
; i
< *BitLen
; ++i
) {
132 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
134 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
135 //low found and we are expecting a bar
138 //mid value found or no bar supposed to be here
139 if ((i
-lastBit
)>(*clk
+tol
)){
140 //should have hit a high or low based on clock!!
143 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
146 lastBit
+=*clk
;//skip over until hit too many errors
147 if (errCnt
>(maxErr
)) break; //allow 1 error for every 1000 samples else start over
150 if ((i
-iii
) >(400 * *clk
)) break; //got plenty of bits
152 //we got more than 64 good bits and not all errors
153 if ((((i
-iii
)/ *clk
) > (64+errCnt
)) && (errCnt
<maxErr
)) {
158 break; //great read - finish
160 if (errCnt
<bestErrCnt
){ //set this as new best run
167 if (bestErrCnt
<maxErr
){
168 //best run is good enough set to best run and set overwrite BinStream
170 lastBit
=bestStart
-*clk
;
172 for (i
= iii
; i
< *BitLen
; ++i
) {
173 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
175 BinStream
[bitnum
] = *invert
;
177 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
178 //low found and we are expecting a bar
180 BinStream
[bitnum
] = 1-*invert
;
183 //mid value found or no bar supposed to be here
184 if ((i
-lastBit
)>(*clk
+tol
)){
185 //should have hit a high or low based on clock!!
188 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
190 BinStream
[bitnum
]=77;
194 lastBit
+=*clk
;//skip over error
197 if (bitnum
>=400) break;
209 //take 10 and 01 and manchester decode
210 //run through 2 times and take least errCnt
211 int manrawdecode(uint8_t * BitStream
, int *bitLen
)
219 for (ii
=1;ii
<3;++ii
){
221 for (i
=i
+ii
;i
<*bitLen
-2;i
+=2){
222 if(BitStream
[i
]==1 && (BitStream
[i
+1]==0)){
223 } else if((BitStream
[i
]==0)&& BitStream
[i
+1]==1){
227 if(bitnum
>300) break;
239 for (i
=i
+ii
;i
<*bitLen
-2;i
+=2){
240 if(BitStream
[i
]==1 && (BitStream
[i
+1]==0)){
241 BitStream
[bitnum
++]=0;
242 } else if((BitStream
[i
]==0)&& BitStream
[i
+1]==1){
243 BitStream
[bitnum
++]=1;
245 BitStream
[bitnum
++]=77;
248 if(bitnum
>300) break;
257 //take 01 or 10 = 0 and 11 or 00 = 1
258 int BiphaseRawDecode(uint8_t * BitStream
, int *bitLen
, int offset
)
264 for (;i
<*bitLen
-2;i
+=2){
265 if((BitStream
[i
]==1 && BitStream
[i
+1]==0)||(BitStream
[i
]==0 && BitStream
[i
+1]==1)){
266 BitStream
[bitnum
++]=1;
267 } else if((BitStream
[i
]==0 && BitStream
[i
+1]==0)||(BitStream
[i
]==1 && BitStream
[i
+1]==1)){
268 BitStream
[bitnum
++]=0;
270 BitStream
[bitnum
++]=77;
273 if(bitnum
>250) break;
280 //takes 2 arguments - clock and invert both as integers
281 //attempts to demodulate ask only
282 //prints binary found and saves in graphbuffer for further commands
283 int askrawdemod(uint8_t *BinStream
, int *bitLen
,int *clk
, int *invert
)
286 // int invert=0; //invert default
287 int high
= 0, low
= 128;
288 *clk
=DetectASKClock(BinStream
,*bitLen
,*clk
); //clock default
289 uint8_t BitStream
[502] = {0};
291 if (*clk
<8) *clk
=64;
292 if (*clk
<32) *clk
=32;
293 if (*invert
!= 0 && *invert
!= 1) *invert
=0;
294 uint32_t initLoopMax
= 200;
295 if (initLoopMax
>*bitLen
) initLoopMax
=*bitLen
;
296 // Detect high and lows
297 for (i
= 0; i
< initLoopMax
; ++i
) //200 samples should be plenty to find high and low values
299 if (BinStream
[i
] > high
)
301 else if (BinStream
[i
] < low
)
304 if ((high
< 158)){ //throw away static
305 // PrintAndLog("no data found");
308 //25% fuzz in case highs and lows aren't clipped [marshmellow]
309 high
=(int)((high
-128)*.75)+128;
310 low
= (int)((low
-128)*.75)+128;
312 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
313 int lastBit
= 0; //set first clock check
314 uint32_t bitnum
= 0; //output counter
315 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
316 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
318 uint32_t gLen
= *bitLen
;
319 if (gLen
> 500) gLen
=500;
321 uint32_t bestStart
= *bitLen
;
322 uint32_t bestErrCnt
= (*bitLen
/1000);
324 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
325 //loop to find first wave that works
326 for (iii
=0; iii
< gLen
; ++iii
){
327 if ((BinStream
[iii
]>=high
)||(BinStream
[iii
]<=low
)){
329 //loop through to see if this start location works
330 for (i
= iii
; i
< *bitLen
; ++i
) {
331 if ((BinStream
[i
] >= high
) && ((i
-lastBit
)>(*clk
-tol
))){
333 BitStream
[bitnum
] = *invert
;
336 } else if ((BinStream
[i
] <= low
) && ((i
-lastBit
)>(*clk
-tol
))){
337 //low found and we are expecting a bar
339 BitStream
[bitnum
] = 1-*invert
;
342 } else if ((BinStream
[i
]<=low
) && (midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
345 BitStream
[bitnum
]= 1-*invert
;
347 } else if ((BinStream
[i
]>=high
)&&(midBit
==0) && ((i
-lastBit
)>((*clk
/2)-tol
))){
350 BitStream
[bitnum
]= *invert
;
352 } else if ((i
-lastBit
)>((*clk
/2)+tol
)&&(midBit
==0)){
355 BitStream
[bitnum
]= BitStream
[bitnum
-1];
358 //mid value found or no bar supposed to be here
360 if ((i
-lastBit
)>(*clk
+tol
)){
361 //should have hit a high or low based on clock!!
363 //PrintAndLog("DEBUG - no wave in expected area - location: %d, expected: %d-%d, lastBit: %d - resetting search",i,(lastBit+(clk-((int)(tol)))),(lastBit+(clk+((int)(tol)))),lastBit);
365 BitStream
[bitnum
]=77;
371 lastBit
+=*clk
;//skip over until hit too many errors
372 if (errCnt
>((*bitLen
/1000))){ //allow 1 error for every 1000 samples else start over
374 bitnum
=0;//start over
379 if (bitnum
>500) break;
381 //we got more than 64 good bits and not all errors
382 if ((bitnum
> (64+errCnt
)) && (errCnt
<(*bitLen
/1000))) {
384 if (errCnt
==0) break; //great read - finish
385 if (bestStart
== iii
) break; //if current run == bestErrCnt run (after exhausted testing) then finish
386 if (errCnt
<bestErrCnt
){ //set this as new best run
392 if (iii
>=gLen
){ //exhausted test
393 //if there was a ok test go back to that one and re-run the best run (then dump after that run)
394 if (bestErrCnt
< (*bitLen
/1000)) iii
=bestStart
;
399 // PrintAndLog("Data start pos:%d, lastBit:%d, stop pos:%d, numBits:%d",iii,lastBit,i,bitnum);
400 //move BitStream back to BinStream
402 for (i
=0; i
< bitnum
; ++i
){
403 BinStream
[i
]=BitStream
[i
];
406 // RepaintGraphWindow();
409 // PrintAndLog("# Errors during Demoding (shown as 77 in bit stream): %d",errCnt);
411 // PrintAndLog("ASK decoded bitstream:");
412 // Now output the bitstream to the scrollback by line of 16 bits
413 // printBitStream2(BitStream,bitnum);
415 //errCnt=manrawdemod(BitStream,bitnum);
417 // Em410xDecode(Cmd);
421 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
422 size_t fsk_wave_demod(uint8_t * dest
, size_t size
, uint8_t fchigh
, uint8_t fclow
)
424 uint32_t last_transition
= 0;
427 if (fchigh
==0) fchigh
=10;
428 if (fclow
==0) fclow
=8;
429 // we do care about the actual theshold value as sometimes near the center of the
430 // wave we may get static that changes direction of wave for one value
431 // if our value is too low it might affect the read. and if our tag or
432 // antenna is weak a setting too high might not see anything. [marshmellow]
433 if (size
<100) return 0;
434 for(idx
=1; idx
<100; idx
++){
435 if(maxVal
<dest
[idx
]) maxVal
= dest
[idx
];
437 // set close to the top of the wave threshold with 25% margin for error
438 // less likely to get a false transition up there.
439 // (but have to be careful not to go too high and miss some short waves)
440 uint8_t threshold_value
= (uint8_t)(((maxVal
-128)*.75)+128);
442 //uint8_t threshold_value = 127;
444 // sync to first lo-hi transition, and threshold
446 // Need to threshold first sample
448 if(dest
[0] < threshold_value
) dest
[0] = 0;
452 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
453 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
454 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
455 for(idx
= 1; idx
< size
; idx
++) {
456 // threshold current value
458 if (dest
[idx
] < threshold_value
) dest
[idx
] = 0;
461 // Check for 0->1 transition
462 if (dest
[idx
-1] < dest
[idx
]) { // 0 -> 1 transition
463 if ((idx
-last_transition
)<(fclow
-2)){ //0-5 = garbage noise
464 //do nothing with extra garbage
465 } else if ((idx
-last_transition
) < (fchigh
-1)) { //6-8 = 8 waves
467 } else { //9+ = 10 waves
470 last_transition
= idx
;
474 return numBits
; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
477 uint32_t myround2(float f
)
479 if (f
>= 2000) return 2000;//something bad happened
480 return (uint32_t) (f
+ (float)0.5);
483 //translate 11111100000 to 10
484 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
)// uint8_t h2l_crossing_value,uint8_t l2h_crossing_value,
486 uint8_t lastval
=dest
[0];
491 for( idx
=1; idx
< size
; idx
++) {
493 if (dest
[idx
]==lastval
) {
497 //if lastval was 1, we have a 1->0 crossing
498 if ( dest
[idx
-1]==1 ) {
499 n
=myround2((float)(n
+1)/((float)(rfLen
)/(float)fclow
));
500 //n=(n+1) / h2l_crossing_value;
501 } else {// 0->1 crossing
502 n
=myround2((float)(n
+1)/((float)(rfLen
-2)/(float)fchigh
)); //-2 for fudge factor
503 //n=(n+1) / l2h_crossing_value;
507 if(n
< maxConsequtiveBits
) //Consecutive
509 if(invert
==0){ //invert bits
510 memset(dest
+numBits
, dest
[idx
-1] , n
);
512 memset(dest
+numBits
, dest
[idx
-1]^1 , n
);
521 //by marshmellow (from holiman's base)
522 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
523 int fskdemod(uint8_t *dest
, size_t size
, uint8_t rfLen
, uint8_t invert
, uint8_t fchigh
, uint8_t fclow
)
526 size
= fsk_wave_demod(dest
, size
, fchigh
, fclow
);
527 size
= aggregate_bits(dest
, size
,rfLen
,192,invert
,fchigh
,fclow
);
530 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
531 int HIDdemodFSK(uint8_t *dest
, size_t size
, uint32_t *hi2
, uint32_t *hi
, uint32_t *lo
)
534 size_t idx
=0; //, found=0; //size=0,
536 size
= fskdemod(dest
, size
,50,0,10,8);
538 // final loop, go over previously decoded manchester data and decode into usable tag ID
539 // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
540 uint8_t frame_marker_mask
[] = {1,1,1,0,0,0};
544 while( idx
+ sizeof(frame_marker_mask
) < size
) {
545 // search for a start of frame marker
546 if ( memcmp(dest
+idx
, frame_marker_mask
, sizeof(frame_marker_mask
)) == 0)
547 { // frame marker found
548 idx
+=sizeof(frame_marker_mask
);
549 while(dest
[idx
] != dest
[idx
+1] && idx
< size
-2)
551 // Keep going until next frame marker (or error)
552 // Shift in a bit. Start by shifting high registers
553 *hi2
= (*hi2
<<1)|(*hi
>>31);
554 *hi
= (*hi
<<1)|(*lo
>>31);
555 //Then, shift in a 0 or one into low
556 if (dest
[idx
] && !dest
[idx
+1]) // 1 0
563 // Hopefully, we read a tag and hit upon the next frame marker
564 if(idx
+ sizeof(frame_marker_mask
) < size
)
566 if ( memcmp(dest
+idx
, frame_marker_mask
, sizeof(frame_marker_mask
)) == 0)
573 *hi2
= *hi
= *lo
= 0;
582 uint32_t bytebits_to_byte(uint8_t* src
, int numbits
)
585 for(int i
= 0 ; i
< numbits
; i
++)
587 num
= (num
<< 1) | (*src
);
593 int IOdemodFSK(uint8_t *dest
, size_t size
)
595 static const uint8_t THRESHOLD
= 140;
597 //make sure buffer has data
598 if (size
< 66) return -1;
599 //test samples are not just noise
600 uint8_t justNoise
= 1;
601 for(idx
=0;idx
< size
&& justNoise
;idx
++){
602 justNoise
= dest
[idx
] < THRESHOLD
;
604 if(justNoise
) return 0;
607 size
= fskdemod(dest
, size
,64,1,10,8); // RF/64 and invert
608 if (size
< 65) return -1; //did we get a good demod?
610 //0 10 20 30 40 50 60
612 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
613 //-----------------------------------------------------------------------------
614 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
616 //XSF(version)facility:codeone+codetwo
618 uint8_t mask
[] = {0,0,0,0,0,0,0,0,0,1};
619 for( idx
=0; idx
< (size
- 65); idx
++) {
620 if ( memcmp(dest
+ idx
, mask
, sizeof(mask
))==0) {
622 if (!dest
[idx
+8] && dest
[idx
+17]==1 && dest
[idx
+26]==1 && dest
[idx
+35]==1 && dest
[idx
+44]==1 && dest
[idx
+53]==1){
623 //confirmed proper separator bits found
624 //return start position
633 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
634 // maybe somehow adjust peak trimming value based on samples to fix?
635 int DetectASKClock(uint8_t dest
[], size_t size
, int clock
)
640 int clk
[]={16,32,40,50,64,100,128,256};
641 int loopCnt
= 256; //don't need to loop through entire array...
642 if (size
<loopCnt
) loopCnt
= size
;
644 //if we already have a valid clock quit
646 if (clk
[i
]==clock
) return clock
;
648 //get high and low peak
649 for (i
=0;i
<loopCnt
;++i
){
657 peak
=(int)((peak
-128)*.75)+128;
658 low
= (int)((low
-128)*.75)+128;
663 int errCnt
[]={0,0,0,0,0,0,0,0};
664 //test each valid clock from smallest to greatest to see which lines up
665 for(clkCnt
=0; clkCnt
<6;++clkCnt
){
666 if (clk
[clkCnt
]==32){
672 //try lining up the peaks by moving starting point (try first 256)
673 for (ii
=0; ii
<loopCnt
; ++ii
){
674 if ((dest
[ii
]>=peak
) || (dest
[ii
]<=low
)){
676 // now that we have the first one lined up test rest of wave array
677 for (i
=0; i
<((int)(size
/clk
[clkCnt
])-1); ++i
){
678 if (dest
[ii
+(i
*clk
[clkCnt
])]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])]<=low
){
679 }else if(dest
[ii
+(i
*clk
[clkCnt
])-tol
]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])-tol
]<=low
){
680 }else if(dest
[ii
+(i
*clk
[clkCnt
])+tol
]>=peak
|| dest
[ii
+(i
*clk
[clkCnt
])+tol
]<=low
){
681 }else{ //error no peak detected
685 //if we found no errors this is correct one - return this clock
686 if(errCnt
[clkCnt
]==0) return clk
[clkCnt
];
687 //if we found errors see if it is lowest so far and save it as best run
688 if(errCnt
[clkCnt
]<bestErr
) bestErr
=errCnt
[clkCnt
];
694 for (iii
=0; iii
<6;++iii
){
695 if (errCnt
[iii
]<errCnt
[best
]){