]> git.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
projects / proxmark3-svn / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
Merge branch 'master' of https://github.com/holiman/proxmark3
[proxmark3-svn] / common / lfdemod.c
1 //-----------------------------------------------------------------------------
2 // Copyright (C) 2014
3 //
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
6 // the license.
7 //-----------------------------------------------------------------------------
8 // Low frequency commands
9 //-----------------------------------------------------------------------------
10
11 #include <stdlib.h>
12 #include <string.h>
13 #include "lfdemod.h"
14
15 //by marshmellow
16 //takes 1s and 0s and searches for EM410x format - output EM ID
17 uint64_t Em410xDecode(uint8_t *BitStream, size_t size)
18 {
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
21 //set defaults
22 uint64_t lo=0;
23 uint32_t i = 0;
24 if (BitStream[10]>1){ //allow only 1s and 0s
25 // PrintAndLog("no data found");
26 return 0;
27 }
28 uint8_t parityTest=0;
29 // 111111111 bit pattern represent start of frame
30 uint8_t frame_marker_mask[] = {1,1,1,1,1,1,1,1,1};
31 uint32_t idx = 0;
32 uint32_t ii=0;
33 uint8_t resetCnt = 0;
34 while( (idx + 64) < size) {
35 restart:
36 // search for a start of frame marker
37 if ( memcmp(BitStream+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
38 { // frame marker found
39 idx+=9;
40 for (i=0; i<10;i++){
41 for(ii=0; ii<5; ++ii){
42 parityTest ^= BitStream[(i*5)+ii+idx];
43 }
44 if (!parityTest){
45 parityTest=0;
46 for (ii=0; ii<4;++ii){
47 lo=(lo<<1LL)|(BitStream[(i*5)+ii+idx]);
48 }
49 //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);
50 }else {//parity failed
51 //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]);
52 parityTest=0;
53 idx-=8;
54 if (resetCnt>5)return 0; //try 5 times
55 resetCnt++;
56 goto restart;//continue;
57 }
58 }
59 //skip last 5 bit parity test for simplicity.
60 return lo;
61 }else{
62 idx++;
63 }
64 }
65 return 0;
66 }
67
68 //by marshmellow
69 //takes 2 arguments - clock and invert both as integers
70 //attempts to demodulate ask while decoding manchester
71 //prints binary found and saves in graphbuffer for further commands
72 int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert)
73 {
74 int i;
75 int high = 0, low = 255;
76 *clk=DetectASKClock(BinStream, *size, *clk); //clock default
77
78 if (*clk<8) *clk =64;
79 if (*clk<32) *clk=32;
80 if (*invert != 0 && *invert != 1) *invert=0;
81 uint32_t initLoopMax = 200;
82 if (initLoopMax > *size) initLoopMax=*size;
83 // Detect high and lows
84 for (i = 0; i < initLoopMax; ++i) //200 samples should be enough to find high and low values
85 {
86 if (BinStream[i] > high)
87 high = BinStream[i];
88 else if (BinStream[i] < low)
89 low = BinStream[i];
90 }
91 if ((high < 129) ){ //throw away static (anything < 1 graph)
92 //PrintAndLog("no data found");
93 return -2;
94 }
95 //25% fuzz in case highs and lows aren't clipped [marshmellow]
96 high=(int)(((high-128)*.75)+128);
97 low= (int)(((low-128)*.75)+128);
98
99 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
100 int lastBit = 0; //set first clock check
101 uint32_t bitnum = 0; //output counter
102 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
103 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
104 int iii = 0;
105 uint32_t gLen = *size;
106 if (gLen > 3000) gLen=3000;
107 uint8_t errCnt =0;
108 uint32_t bestStart = *size;
109 uint32_t bestErrCnt = (*size/1000);
110 uint32_t maxErr = (*size/1000);
111 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
112 //loop to find first wave that works
113 for (iii=0; iii < gLen; ++iii){
114 if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){
115 lastBit=iii-*clk;
116 errCnt=0;
117 //loop through to see if this start location works
118 for (i = iii; i < *size; ++i) {
119 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
120 lastBit+=*clk;
121 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
122 //low found and we are expecting a bar
123 lastBit+=*clk;
124 } else {
125 //mid value found or no bar supposed to be here
126 if ((i-lastBit)>(*clk+tol)){
127 //should have hit a high or low based on clock!!
128
129 //debug
130 //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);
131
132 errCnt++;
133 lastBit+=*clk;//skip over until hit too many errors
134 if (errCnt>(maxErr)) break; //allow 1 error for every 1000 samples else start over
135 }
136 }
137 if ((i-iii) >(400 * *clk)) break; //got plenty of bits
138 }
139 //we got more than 64 good bits and not all errors
140 if ((((i-iii)/ *clk) > (64+errCnt)) && (errCnt<maxErr)) {
141 //possible good read
142 if (errCnt==0){
143 bestStart=iii;
144 bestErrCnt=errCnt;
145 break; //great read - finish
146 }
147 if (errCnt<bestErrCnt){ //set this as new best run
148 bestErrCnt=errCnt;
149 bestStart = iii;
150 }
151 }
152 }
153 }
154 if (bestErrCnt<maxErr){
155 //best run is good enough set to best run and set overwrite BinStream
156 iii=bestStart;
157 lastBit = bestStart - *clk;
158 bitnum=0;
159 for (i = iii; i < *size; ++i) {
160 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
161 lastBit += *clk;
162 BinStream[bitnum] = *invert;
163 bitnum++;
164 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
165 //low found and we are expecting a bar
166 lastBit+=*clk;
167 BinStream[bitnum] = 1-*invert;
168 bitnum++;
169 } else {
170 //mid value found or no bar supposed to be here
171 if ((i-lastBit)>(*clk+tol)){
172 //should have hit a high or low based on clock!!
173
174 //debug
175 //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);
176 if (bitnum > 0){
177 BinStream[bitnum]=77;
178 bitnum++;
179 }
180
181 lastBit+=*clk;//skip over error
182 }
183 }
184 if (bitnum >=400) break;
185 }
186 *size=bitnum;
187 } else{
188 *invert=bestStart;
189 *clk=iii;
190 return -1;
191 }
192 return bestErrCnt;
193 }
194
195 //by marshmellow
196 //take 10 and 01 and manchester decode
197 //run through 2 times and take least errCnt
198 int manrawdecode(uint8_t * BitStream, size_t *size)
199 {
200 int bitnum=0;
201 int errCnt =0;
202 int i=1;
203 int bestErr = 1000;
204 int bestRun = 0;
205 int ii=1;
206 for (ii=1;ii<3;++ii){
207 i=1;
208 for (i=i+ii;i<*size-2;i+=2){
209 if(BitStream[i]==1 && (BitStream[i+1]==0)){
210 } else if((BitStream[i]==0)&& BitStream[i+1]==1){
211 } else {
212 errCnt++;
213 }
214 if(bitnum>300) break;
215 }
216 if (bestErr>errCnt){
217 bestErr=errCnt;
218 bestRun=ii;
219 }
220 errCnt=0;
221 }
222 errCnt=bestErr;
223 if (errCnt<20){
224 ii=bestRun;
225 i=1;
226 for (i=i+ii;i < *size-2;i+=2){
227 if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
228 BitStream[bitnum++]=0;
229 } else if((BitStream[i] == 0) && BitStream[i+1] == 1){
230 BitStream[bitnum++]=1;
231 } else {
232 BitStream[bitnum++]=77;
233 //errCnt++;
234 }
235 if(bitnum>300) break;
236 }
237 *size=bitnum;
238 }
239 return errCnt;
240 }
241
242
243 //by marshmellow
244 //take 01 or 10 = 0 and 11 or 00 = 1
245 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset)
246 {
247 uint8_t bitnum=0;
248 uint32_t errCnt =0;
249 uint32_t i=1;
250 i=offset;
251 for (;i<*size-2;i+=2){
252 if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
253 BitStream[bitnum++]=1;
254 } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
255 BitStream[bitnum++]=0;
256 } else {
257 BitStream[bitnum++]=77;
258 errCnt++;
259 }
260 if(bitnum>250) break;
261 }
262 *size=bitnum;
263 return errCnt;
264 }
265
266 //by marshmellow
267 //takes 2 arguments - clock and invert both as integers
268 //attempts to demodulate ask only
269 //prints binary found and saves in graphbuffer for further commands
270 int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert)
271 {
272 uint32_t i;
273 // int invert=0; //invert default
274 int high = 0, low = 255;
275 *clk=DetectASKClock(BinStream, *size, *clk); //clock default
276 uint8_t BitStream[502] = {0};
277
278 if (*clk<8) *clk =64;
279 if (*clk<32) *clk=32;
280 if (*invert != 0 && *invert != 1) *invert =0;
281 uint32_t initLoopMax = 200;
282 if (initLoopMax > *size) initLoopMax=*size;
283 // Detect high and lows
284 for (i = 0; i < initLoopMax; ++i) //200 samples should be plenty to find high and low values
285 {
286 if (BinStream[i] > high)
287 high = BinStream[i];
288 else if (BinStream[i] < low)
289 low = BinStream[i];
290 }
291 if ((high < 129)){ //throw away static high has to be more than 0 on graph.
292 //noise <= -10 here
293 // PrintAndLog("no data found");
294 return -2;
295 }
296 //25% fuzz in case highs and lows aren't clipped [marshmellow]
297 high=(int)(((high-128)*.75)+128);
298 low= (int)(((low-128)*.75)+128);
299
300 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
301 int lastBit = 0; //set first clock check
302 uint32_t bitnum = 0; //output counter
303 uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock
304 // if they fall + or - this value + clock from last valid wave
305 if (*clk == 32) tol=1; //clock tolerance may not be needed anymore currently set to
306 // + or - 1 but could be increased for poor waves or removed entirely
307 uint32_t iii = 0;
308 uint32_t gLen = *size;
309 if (gLen > 500) gLen=500;
310 uint8_t errCnt =0;
311 uint32_t bestStart = *size;
312 uint32_t bestErrCnt = (*size/1000);
313 uint8_t midBit=0;
314 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
315 //loop to find first wave that works
316 for (iii=0; iii < gLen; ++iii){
317 if ((BinStream[iii]>=high) || (BinStream[iii]<=low)){
318 lastBit=iii-*clk;
319 //loop through to see if this start location works
320 for (i = iii; i < *size; ++i) {
321 if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){
322 lastBit+=*clk;
323 BitStream[bitnum] = *invert;
324 bitnum++;
325 midBit=0;
326 } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){
327 //low found and we are expecting a bar
328 lastBit+=*clk;
329 BitStream[bitnum] = 1- *invert;
330 bitnum++;
331 midBit=0;
332 } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
333 //mid bar?
334 midBit=1;
335 BitStream[bitnum]= 1- *invert;
336 bitnum++;
337 } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
338 //mid bar?
339 midBit=1;
340 BitStream[bitnum]= *invert;
341 bitnum++;
342 } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){
343 //no mid bar found
344 midBit=1;
345 BitStream[bitnum]= BitStream[bitnum-1];
346 bitnum++;
347 } else {
348 //mid value found or no bar supposed to be here
349
350 if ((i-lastBit)>(*clk+tol)){
351 //should have hit a high or low based on clock!!
352 //debug
353 //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);
354 if (bitnum > 0){
355 BitStream[bitnum]=77;
356 bitnum++;
357 }
358
359 errCnt++;
360 lastBit+=*clk;//skip over until hit too many errors
361 if (errCnt > ((*size/1000))){ //allow 1 error for every 1000 samples else start over
362 errCnt=0;
363 bitnum=0;//start over
364 break;
365 }
366 }
367 }
368 if (bitnum>500) break;
369 }
370 //we got more than 64 good bits and not all errors
371 if ((bitnum > (64+errCnt)) && (errCnt<(*size/1000))) {
372 //possible good read
373 if (errCnt==0) break; //great read - finish
374 if (bestStart == iii) break; //if current run == bestErrCnt run (after exhausted testing) then finish
375 if (errCnt<bestErrCnt){ //set this as new best run
376 bestErrCnt=errCnt;
377 bestStart = iii;
378 }
379 }
380 }
381 if (iii>=gLen){ //exhausted test
382 //if there was a ok test go back to that one and re-run the best run (then dump after that run)
383 if (bestErrCnt < (*size/1000)) iii=bestStart;
384 }
385 }
386 if (bitnum>16){
387 for (i=0; i < bitnum; ++i){
388 BinStream[i]=BitStream[i];
389 }
390 *size=bitnum;
391 } else return -1;
392 return errCnt;
393 }
394 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
395 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow)
396 {
397 uint32_t last_transition = 0;
398 uint32_t idx = 1;
399 //uint32_t maxVal=0;
400 if (fchigh==0) fchigh=10;
401 if (fclow==0) fclow=8;
402 //set the threshold close to 0 (graph) or 128 std to avoid static
403 uint8_t threshold_value = 123;
404
405 // sync to first lo-hi transition, and threshold
406
407 // Need to threshold first sample
408
409 if(dest[0] < threshold_value) dest[0] = 0;
410 else dest[0] = 1;
411
412 size_t numBits = 0;
413 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
414 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
415 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
416 for(idx = 1; idx < size; idx++) {
417 // threshold current value
418
419 if (dest[idx] < threshold_value) dest[idx] = 0;
420 else dest[idx] = 1;
421
422 // Check for 0->1 transition
423 if (dest[idx-1] < dest[idx]) { // 0 -> 1 transition
424 if ((idx-last_transition)<(fclow-2)){ //0-5 = garbage noise
425 //do nothing with extra garbage
426 } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves
427 dest[numBits]=1;
428 } else { //9+ = 10 waves
429 dest[numBits]=0;
430 }
431 last_transition = idx;
432 numBits++;
433 }
434 }
435 return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
436 }
437
438 uint32_t myround2(float f)
439 {
440 if (f >= 2000) return 2000;//something bad happened
441 return (uint32_t) (f + (float)0.5);
442 }
443
444 //translate 11111100000 to 10
445 size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxConsequtiveBits,
446 uint8_t invert, uint8_t fchigh, uint8_t fclow)
447 {
448 uint8_t lastval=dest[0];
449 uint32_t idx=0;
450 size_t numBits=0;
451 uint32_t n=1;
452
453 for( idx=1; idx < size; idx++) {
454
455 if (dest[idx]==lastval) {
456 n++;
457 continue;
458 }
459 //if lastval was 1, we have a 1->0 crossing
460 if ( dest[idx-1]==1 ) {
461 n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow));
462 } else {// 0->1 crossing
463 n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh)); //-1 for fudge factor
464 }
465 if (n == 0) n = 1;
466
467 if(n < maxConsequtiveBits) //Consecutive
468 {
469 if(invert==0){ //invert bits
470 memset(dest+numBits, dest[idx-1] , n);
471 }else{
472 memset(dest+numBits, dest[idx-1]^1 , n);
473 }
474 numBits += n;
475 }
476 n=0;
477 lastval=dest[idx];
478 }//end for
479 return numBits;
480 }
481 //by marshmellow (from holiman's base)
482 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
483 int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow)
484 {
485 // FSK demodulator
486 size = fsk_wave_demod(dest, size, fchigh, fclow);
487 size = aggregate_bits(dest, size, rfLen, 192, invert, fchigh, fclow);
488 return size;
489 }
490 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
491 int HIDdemodFSK(uint8_t *dest, size_t size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
492 {
493
494 size_t idx=0; //, found=0; //size=0,
495 // FSK demodulator
496 size = fskdemod(dest, size,50,0,10,8);
497
498 // final loop, go over previously decoded manchester data and decode into usable tag ID
499 // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
500 uint8_t frame_marker_mask[] = {1,1,1,0,0,0};
501 int numshifts = 0;
502 idx = 0;
503 //one scan
504 while( idx + sizeof(frame_marker_mask) < size) {
505 // search for a start of frame marker
506 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
507 { // frame marker found
508 idx+=sizeof(frame_marker_mask);
509 while(dest[idx] != dest[idx+1] && idx < size-2)
510 {
511 // Keep going until next frame marker (or error)
512 // Shift in a bit. Start by shifting high registers
513 *hi2 = (*hi2<<1)|(*hi>>31);
514 *hi = (*hi<<1)|(*lo>>31);
515 //Then, shift in a 0 or one into low
516 if (dest[idx] && !dest[idx+1]) // 1 0
517 *lo=(*lo<<1)|0;
518 else // 0 1
519 *lo=(*lo<<1)|1;
520 numshifts++;
521 idx += 2;
522 }
523 // Hopefully, we read a tag and hit upon the next frame marker
524 if(idx + sizeof(frame_marker_mask) < size)
525 {
526 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
527 {
528 //good return
529 return idx;
530 }
531 }
532 // reset
533 *hi2 = *hi = *lo = 0;
534 numshifts = 0;
535 }else {
536 idx++;
537 }
538 }
539 return -1;
540 }
541
542 uint32_t bytebits_to_byte(uint8_t* src, size_t numbits)
543 {
544 uint32_t num = 0;
545 for(int i = 0 ; i < numbits ; i++)
546 {
547 num = (num << 1) | (*src);
548 src++;
549 }
550 return num;
551 }
552
553 int IOdemodFSK(uint8_t *dest, size_t size)
554 {
555 static const uint8_t THRESHOLD = 129;
556 uint32_t idx=0;
557 //make sure buffer has data
558 if (size < 66) return -1;
559 //test samples are not just noise
560 uint8_t justNoise = 1;
561 for(idx=0;idx< size && justNoise ;idx++){
562 justNoise = dest[idx] < THRESHOLD;
563 }
564 if(justNoise) return 0;
565
566 // FSK demodulator
567 size = fskdemod(dest, size, 64, 1, 10, 8); // RF/64 and invert
568 if (size < 65) return -1; //did we get a good demod?
569 //Index map
570 //0 10 20 30 40 50 60
571 //| | | | | | |
572 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
573 //-----------------------------------------------------------------------------
574 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
575 //
576 //XSF(version)facility:codeone+codetwo
577 //Handle the data
578 uint8_t mask[] = {0,0,0,0,0,0,0,0,0,1};
579 for( idx=0; idx < (size - 65); idx++) {
580 if ( memcmp(dest + idx, mask, sizeof(mask))==0) {
581 //frame marker found
582 if (!dest[idx+8] && dest[idx+17]==1 && dest[idx+26]==1 && dest[idx+35]==1 && dest[idx+44]==1 && dest[idx+53]==1){
583 //confirmed proper separator bits found
584 //return start position
585 return (int) idx;
586 }
587 }
588 }
589 return 0;
590 }
591
592 // by marshmellow
593 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
594 // maybe somehow adjust peak trimming value based on samples to fix?
595 int DetectASKClock(uint8_t dest[], size_t size, int clock)
596 {
597 int i=0;
598 int peak=0;
599 int low=255;
600 int clk[]={16,32,40,50,64,100,128,256};
601 int loopCnt = 256; //don't need to loop through entire array...
602 if (size<loopCnt) loopCnt = size;
603
604 //if we already have a valid clock quit
605 for (;i<8;++i)
606 if (clk[i] == clock) return clock;
607
608 //get high and low peak
609 for (i=0; i < loopCnt; ++i){
610 if(dest[i] > peak){
611 peak = dest[i];
612 }
613 if(dest[i] < low){
614 low = dest[i];
615 }
616 }
617 peak=(int)(((peak-128)*.75)+128);
618 low= (int)(((low-128)*.75)+128);
619 int ii;
620 int clkCnt;
621 int tol = 0;
622 int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000};
623 int errCnt=0;
624 //test each valid clock from smallest to greatest to see which lines up
625 for(clkCnt=0; clkCnt < 6; ++clkCnt){
626 if (clk[clkCnt] == 32){
627 tol=1;
628 }else{
629 tol=0;
630 }
631 bestErr[clkCnt]=1000;
632 //try lining up the peaks by moving starting point (try first 256)
633 for (ii=0; ii< loopCnt; ++ii){
634 if ((dest[ii] >= peak) || (dest[ii] <= low)){
635 errCnt=0;
636 // now that we have the first one lined up test rest of wave array
637 for (i=0; i<((int)(size/clk[clkCnt])-1); ++i){
638 if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
639 }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){
640 }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){
641 }else{ //error no peak detected
642 errCnt++;
643 }
644 }
645 //if we found no errors this is correct one - return this clock
646 if(errCnt==0) return clk[clkCnt];
647 //if we found errors see if it is lowest so far and save it as best run
648 if(errCnt<bestErr[clkCnt]) bestErr[clkCnt]=errCnt;
649 }
650 }
651 }
652 int iii=0;
653 int best=0;
654 for (iii=0; iii<7;++iii){
655 if (bestErr[iii]<bestErr[best]){
656 // current best bit to error ratio vs new bit to error ratio
657 if (((size/clk[best])/bestErr[best] < (size/clk[iii])/bestErr[iii]) ){
658 best = iii;
659 }
660 }
661 }
662 return clk[best];
663 }
664
665 //by marshmellow
666 //detect psk clock by reading #peaks vs no peaks(or errors)
667 int DetectpskNRZClock(uint8_t dest[], size_t size, int clock)
668 {
669 int i=0;
670 int peak=0;
671 int low=255;
672 int clk[]={16,32,40,50,64,100,128,256};
673 int loopCnt = 2048; //don't need to loop through entire array...
674 if (size<loopCnt) loopCnt = size;
675
676 //if we already have a valid clock quit
677 for (; i < 8; ++i)
678 if (clk[i] == clock) return clock;
679
680 //get high and low peak
681 for (i=0; i < loopCnt; ++i){
682 if(dest[i] > peak){
683 peak = dest[i];
684 }
685 if(dest[i] < low){
686 low = dest[i];
687 }
688 }
689 peak=(int)(((peak-128)*.75)+128);
690 low= (int)(((low-128)*.75)+128);
691 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
692 int ii;
693 uint8_t clkCnt;
694 uint8_t tol = 0;
695 int peakcnt=0;
696 int errCnt=0;
697 int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
698 int peaksdet[]={0,0,0,0,0,0,0,0,0};
699 //test each valid clock from smallest to greatest to see which lines up
700 for(clkCnt=0; clkCnt < 6; ++clkCnt){
701 if (clk[clkCnt] == 32){
702 tol=1;
703 }else{
704 tol=0;
705 }
706 //try lining up the peaks by moving starting point (try first 256)
707 for (ii=0; ii< loopCnt; ++ii){
708 if ((dest[ii] >= peak) || (dest[ii] <= low)){
709 errCnt=0;
710 peakcnt=0;
711 // now that we have the first one lined up test rest of wave array
712 for (i=0; i < ((int)(size/clk[clkCnt])-1); ++i){
713 if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
714 peakcnt++;
715 }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){
716 peakcnt++;
717 }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){
718 peakcnt++;
719 }else{ //error no peak detected
720 errCnt++;
721 }
722 }
723 if(peakcnt>peaksdet[clkCnt]) {
724 peaksdet[clkCnt]=peakcnt;
725 bestErr[clkCnt]=errCnt;
726 }
727 }
728 }
729 }
730 int iii=0;
731 int best=0;
732 //int ratio2; //debug
733 int ratio;
734 //int bits;
735 for (iii=0; iii < 7; ++iii){
736 ratio=1000;
737 //ratio2=1000; //debug
738 //bits=size/clk[iii]; //debug
739 if (peaksdet[iii] > 0){
740 ratio=bestErr[iii]/peaksdet[iii];
741 if (((bestErr[best]/peaksdet[best]) > (ratio)+1)){
742 best = iii;
743 }
744 //ratio2=bits/peaksdet[iii]; //debug
745 }
746 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d, ratio: %d, bits: %d, peakbitr: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best],ratio, bits,ratio2);
747 }
748 return clk[best];
749 }
750
751 //by marshmellow (attempt to get rid of high immediately after a low)
752 void pskCleanWave(uint8_t *bitStream, size_t size)
753 {
754 int i;
755 int low=255;
756 int high=0;
757 int gap = 4;
758 // int loopMax = 2048;
759 int newLow=0;
760 int newHigh=0;
761 for (i=0; i < size; ++i){
762 if (bitStream[i] < low) low=bitStream[i];
763 if (bitStream[i] > high) high=bitStream[i];
764 }
765 high = (int)(((high-128)*.80)+128);
766 low = (int)(((low-128)*.90)+128);
767 //low = (uint8_t)(((int)(low)-128)*.80)+128;
768 for (i=0; i < size; ++i){
769 if (newLow == 1){
770 bitStream[i]=low+8;
771 gap--;
772 if (gap == 0){
773 newLow=0;
774 gap=4;
775 }
776 }else if (newHigh == 1){
777 bitStream[i]=high-8;
778 gap--;
779 if (gap == 0){
780 newHigh=0;
781 gap=4;
782 }
783 }
784 if (bitStream[i] <= low) newLow=1;
785 if (bitStream[i] >= high) newHigh=1;
786 }
787 return;
788 }
789
790
791 //redesigned by marshmellow adjusted from existing decode functions
792 //indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
793 int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert)
794 {
795 //26 bit 40134 format (don't know other formats)
796 int i;
797 int long_wait=29;//29 leading zeros in format
798 int start;
799 int first = 0;
800 int first2 = 0;
801 int bitCnt = 0;
802 int ii;
803 // Finding the start of a UID
804 for (start = 0; start <= *size - 250; start++) {
805 first = bitStream[start];
806 for (i = start; i < start + long_wait; i++) {
807 if (bitStream[i] != first) {
808 break;
809 }
810 }
811 if (i == (start + long_wait)) {
812 break;
813 }
814 }
815 if (start == *size - 250 + 1) {
816 // did not find start sequence
817 return -1;
818 }
819 // Inverting signal if needed
820 if (first == 1) {
821 for (i = start; i < *size; i++) {
822 bitStream[i] = !bitStream[i];
823 }
824 *invert = 1;
825 }else *invert=0;
826
827 int iii;
828 //found start once now test length by finding next one
829 for (ii=start+29; ii <= *size - 250; ii++) {
830 first2 = bitStream[ii];
831 for (iii = ii; iii < ii + long_wait; iii++) {
832 if (bitStream[iii] != first2) {
833 break;
834 }
835 }
836 if (iii == (ii + long_wait)) {
837 break;
838 }
839 }
840 if (ii== *size - 250 + 1){
841 // did not find second start sequence
842 return -2;
843 }
844 bitCnt=ii-start;
845
846 // Dumping UID
847 i = start;
848 for (ii = 0; ii < bitCnt; ii++) {
849 bitStream[ii] = bitStream[i++];
850 }
851 *size=bitCnt;
852 return 1;
853 }
854
855
856 //by marshmellow - demodulate PSK wave or NRZ wave (both similar enough)
857 //peaks switch bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
858 int pskNRZrawDemod(uint8_t *dest, size_t *size, int *clk, int *invert)
859 {
860 pskCleanWave(dest,*size);
861 int clk2 = DetectpskNRZClock(dest, *size, *clk);
862 *clk=clk2;
863 uint32_t i;
864 uint8_t high=0, low=255;
865 uint32_t gLen = *size;
866 if (gLen > 1280) gLen=1280;
867 // get high
868 for (i=0; i < gLen; ++i){
869 if (dest[i] > high) high = dest[i];
870 if (dest[i] < low) low = dest[i];
871 }
872 //fudge high/low bars by 25%
873 high = (uint8_t)((((int)(high)-128)*.75)+128);
874 low = (uint8_t)((((int)(low)-128)*.80)+128);
875
876 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
877 int lastBit = 0; //set first clock check
878 uint32_t bitnum = 0; //output counter
879 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
880 if (*clk==32) tol = 2; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely
881 uint32_t iii = 0;
882 uint8_t errCnt =0;
883 uint32_t bestStart = *size;
884 uint32_t maxErr = (*size/1000);
885 uint32_t bestErrCnt = maxErr;
886 //uint8_t midBit=0;
887 uint8_t curBit=0;
888 uint8_t bitHigh=0;
889 uint8_t ignorewin=*clk/8;
890 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
891 //loop to find first wave that works - align to clock
892 for (iii=0; iii < gLen; ++iii){
893 if ((dest[iii]>=high) || (dest[iii]<=low)){
894 lastBit=iii-*clk;
895 //loop through to see if this start location works
896 for (i = iii; i < *size; ++i) {
897 //if we found a high bar and we are at a clock bit
898 if ((dest[i]>=high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
899 bitHigh=1;
900 lastBit+=*clk;
901 ignorewin=*clk/8;
902 bitnum++;
903 //else if low bar found and we are at a clock point
904 }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
905 bitHigh=1;
906 lastBit+=*clk;
907 ignorewin=*clk/8;
908 bitnum++;
909 //else if no bars found
910 }else if(dest[i] < high && dest[i] > low) {
911 if (ignorewin==0){
912 bitHigh=0;
913 }else ignorewin--;
914 //if we are past a clock point
915 if (i >= lastBit+*clk+tol){ //clock val
916 lastBit+=*clk;
917 bitnum++;
918 }
919 //else if bar found but we are not at a clock bit and we did not just have a clock bit
920 }else if ((dest[i]>=high || dest[i]<=low) && (i<lastBit+*clk-tol || i>lastBit+*clk+tol) && (bitHigh==0)){
921 //error bar found no clock...
922 errCnt++;
923 }
924 if (bitnum>=1000) break;
925 }
926 //we got more than 64 good bits and not all errors
927 if ((bitnum > (64+errCnt)) && (errCnt < (maxErr))) {
928 //possible good read
929 if (errCnt == 0){
930 bestStart = iii;
931 bestErrCnt = errCnt;
932 break; //great read - finish
933 }
934 if (bestStart == iii) break; //if current run == bestErrCnt run (after exhausted testing) then finish
935 if (errCnt < bestErrCnt){ //set this as new best run
936 bestErrCnt = errCnt;
937 bestStart = iii;
938 }
939 }
940 }
941 }
942 if (bestErrCnt < maxErr){
943 //best run is good enough set to best run and set overwrite BinStream
944 iii=bestStart;
945 lastBit=bestStart-*clk;
946 bitnum=0;
947 for (i = iii; i < *size; ++i) {
948 //if we found a high bar and we are at a clock bit
949 if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
950 bitHigh=1;
951 lastBit+=*clk;
952 curBit=1-*invert;
953 dest[bitnum]=curBit;
954 ignorewin=*clk/8;
955 bitnum++;
956 //else if low bar found and we are at a clock point
957 }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
958 bitHigh=1;
959 lastBit+=*clk;
960 curBit=*invert;
961 dest[bitnum]=curBit;
962 ignorewin=*clk/8;
963 bitnum++;
964 //else if no bars found
965 }else if(dest[i]<high && dest[i]>low) {
966 if (ignorewin==0){
967 bitHigh=0;
968 }else ignorewin--;
969 //if we are past a clock point
970 if (i>=lastBit+*clk+tol){ //clock val
971 lastBit+=*clk;
972 dest[bitnum]=curBit;
973 bitnum++;
974 }
975 //else if bar found but we are not at a clock bit and we did not just have a clock bit
976 }else if ((dest[i]>=high || dest[i]<=low) && ((i<lastBit+*clk-tol) || (i>lastBit+*clk+tol)) && (bitHigh==0)){
977 //error bar found no clock...
978 bitHigh=1;
979 dest[bitnum]=77;
980 bitnum++;
981 errCnt++;
982 }
983 if (bitnum >=1000) break;
984 }
985 *size=bitnum;
986 } else{
987 *size=bitnum;
988 *clk=bestStart;
989 return -1;
990 }
991
992 if (bitnum>16){
993 *size=bitnum;
994 } else return -1;
995 return errCnt;
996 }
997
Proxmark3 GIT tracking
Impressum, Datenschutz