]> git.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
47e63ef6ec64515b7ff190bbc9cce0a79c99f074
[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 demod/decode commands
9 //-----------------------------------------------------------------------------
10
11 #include <stdlib.h>
12 #include <string.h>
13 #include "lfdemod.h"
14
15
16 uint8_t justNoise(uint8_t *BitStream, size_t size)
17 {
18 static const uint8_t THRESHOLD = 123;
19 //test samples are not just noise
20 uint8_t justNoise1 = 1;
21 for(size_t idx=0; idx < size && justNoise1 ;idx++){
22 justNoise1 = BitStream[idx] < THRESHOLD;
23 }
24 return justNoise1;
25 }
26
27 //by marshmellow
28 //get high and low with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise
29 int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo)
30 {
31 *high=0;
32 *low=255;
33 // get high and low thresholds
34 for (int i=0; i < size; i++){
35 if (BitStream[i] > *high) *high = BitStream[i];
36 if (BitStream[i] < *low) *low = BitStream[i];
37 }
38 if (*high < 123) return -1; // just noise
39 *high = (int)(((*high-128)*(((float)fuzzHi)/100))+128);
40 *low = (int)(((*low-128)*(((float)fuzzLo)/100))+128);
41 return 1;
42 }
43
44 // by marshmellow
45 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
46 // returns 1 if passed
47 uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType)
48 {
49 uint8_t ans = 0;
50 for (uint8_t i = 0; i < bitLen; i++){
51 ans ^= ((bits >> i) & 1);
52 }
53 //PrintAndLog("DEBUG: ans: %d, ptype: %d",ans,pType);
54 return (ans == pType);
55 }
56
57 //by marshmellow
58 //search for given preamble in given BitStream and return startIndex and length
59 uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx)
60 {
61 uint8_t foundCnt=0;
62 for (int idx=0; idx < *size - pLen; idx++){
63 if (memcmp(BitStream+idx, preamble, pLen) == 0){
64 //first index found
65 foundCnt++;
66 if (foundCnt == 1){
67 *startIdx = idx;
68 }
69 if (foundCnt == 2){
70 *size = idx - *startIdx;
71 return 1;
72 }
73 }
74 }
75 return 0;
76 }
77
78
79 //by marshmellow
80 //takes 1s and 0s and searches for EM410x format - output EM ID
81 uint64_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx)
82 {
83 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
84 // otherwise could be a void with no arguments
85 //set defaults
86 uint64_t lo=0;
87 uint32_t i = 0;
88 if (BitStream[1]>1){ //allow only 1s and 0s
89 // PrintAndLog("no data found");
90 return 0;
91 }
92 // 111111111 bit pattern represent start of frame
93 uint8_t preamble[] = {1,1,1,1,1,1,1,1,1};
94 uint32_t idx = 0;
95 uint32_t parityBits = 0;
96 uint8_t errChk = 0;
97 *startIdx = 0;
98 for (uint8_t extraBitChk=0; extraBitChk<5; extraBitChk++){
99 errChk = preambleSearch(BitStream+extraBitChk+*startIdx, preamble, sizeof(preamble), size, startIdx);
100 if (errChk == 0) return 0;
101 idx = *startIdx + 9;
102 for (i=0; i<10;i++){ //loop through 10 sets of 5 bits (50-10p = 40 bits)
103 parityBits = bytebits_to_byte(BitStream+(i*5)+idx,5);
104 //check even parity
105 if (parityTest(parityBits, 5, 0) == 0){
106 //parity failed try next bit (in the case of 1111111111) but last 9 = preamble
107 startIdx++;
108 errChk = 0;
109 break;
110 }
111 for (uint8_t ii=0; ii<4; ii++){
112 lo = (lo << 1LL) | (BitStream[(i*5)+ii+idx]);
113 }
114 }
115 if (errChk != 0) return lo;
116 //skip last 5 bit parity test for simplicity.
117 // *size = 64;
118 }
119 return 0;
120 }
121
122 //by marshmellow
123 //takes 3 arguments - clock, invert, maxErr as integers
124 //attempts to demodulate ask while decoding manchester
125 //prints binary found and saves in graphbuffer for further commands
126 int askmandemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr)
127 {
128 int i;
129 //int clk2=*clk;
130 int start = DetectASKClock(BinStream, *size, clk, 20); //clock default
131 if (*clk==0) return -3;
132 if (start < 0) return -3;
133 // if autodetected too low then adjust //MAY NEED ADJUSTMENT
134 //if (clk2==0 && *clk<8) *clk =64;
135 //if (clk2==0 && *clk<32) *clk=32;
136 if (*invert != 0 && *invert != 1) *invert=0;
137 uint32_t initLoopMax = 200;
138 if (initLoopMax > *size) initLoopMax=*size;
139 // Detect high and lows
140 // 25% fuzz in case highs and lows aren't clipped [marshmellow]
141 int high, low, ans;
142 ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75);
143 if (ans<1) return -2; //just noise
144
145 // PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
146 int lastBit = 0; //set first clock check
147 uint32_t bitnum = 0; //output counter
148 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
149 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
150 int iii = 0;
151 uint32_t gLen = *size;
152 if (gLen > 3000) gLen=3000;
153 uint8_t errCnt =0;
154 uint16_t MaxBits = 500;
155 uint32_t bestStart = *size;
156 int bestErrCnt = maxErr+1;
157 // PrintAndLog("DEBUG - lastbit - %d",lastBit);
158 // loop to find first wave that works
159 for (iii=0; iii < gLen; ++iii){
160 if ((BinStream[iii] >= high) || (BinStream[iii] <= low)){
161 lastBit=iii-*clk;
162 errCnt=0;
163 // loop through to see if this start location works
164 for (i = iii; i < *size; ++i) {
165 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
166 lastBit+=*clk;
167 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
168 //low found and we are expecting a bar
169 lastBit+=*clk;
170 } else {
171 //mid value found or no bar supposed to be here
172 if ((i-lastBit)>(*clk+tol)){
173 //should have hit a high or low based on clock!!
174
175 //debug
176 //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);
177
178 errCnt++;
179 lastBit+=*clk;//skip over until hit too many errors
180 if (errCnt>(maxErr)) break; //allow 1 error for every 1000 samples else start over
181 }
182 }
183 if ((i-iii) >(MaxBits * *clk)) break; //got plenty of bits
184 }
185 //we got more than 64 good bits and not all errors
186 if ((((i-iii)/ *clk) > (64)) && (errCnt<=maxErr)) {
187 //possible good read
188 if (errCnt==0){
189 bestStart=iii;
190 bestErrCnt=errCnt;
191 break; //great read - finish
192 }
193 if (errCnt<bestErrCnt){ //set this as new best run
194 bestErrCnt=errCnt;
195 bestStart = iii;
196 }
197 }
198 }
199 }
200 if (bestErrCnt<=maxErr){
201 //best run is good enough set to best run and set overwrite BinStream
202 iii=bestStart;
203 lastBit = bestStart - *clk;
204 bitnum=0;
205 for (i = iii; i < *size; ++i) {
206 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
207 lastBit += *clk;
208 BinStream[bitnum] = *invert;
209 bitnum++;
210 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
211 //low found and we are expecting a bar
212 lastBit+=*clk;
213 BinStream[bitnum] = 1-*invert;
214 bitnum++;
215 } else {
216 //mid value found or no bar supposed to be here
217 if ((i-lastBit)>(*clk+tol)){
218 //should have hit a high or low based on clock!!
219
220 //debug
221 //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);
222 if (bitnum > 0){
223 BinStream[bitnum]=77;
224 bitnum++;
225 }
226
227 lastBit+=*clk;//skip over error
228 }
229 }
230 if (bitnum >=MaxBits) break;
231 }
232 *size=bitnum;
233 } else{
234 *invert=bestStart;
235 *clk=iii;
236 return -1;
237 }
238 return bestErrCnt;
239 }
240
241 //by marshmellow
242 //encode binary data into binary manchester
243 int ManchesterEncode(uint8_t *BitStream, size_t size)
244 {
245 size_t modIdx=20000, i=0;
246 if (size>modIdx) return -1;
247 for (size_t idx=0; idx < size; idx++){
248 BitStream[idx+modIdx++] = BitStream[idx];
249 BitStream[idx+modIdx++] = BitStream[idx]^1;
250 }
251 for (; i<(size*2); i++){
252 BitStream[i] = BitStream[i+20000];
253 }
254 return i;
255 }
256
257 //by marshmellow
258 //take 10 and 01 and manchester decode
259 //run through 2 times and take least errCnt
260 int manrawdecode(uint8_t * BitStream, size_t *size)
261 {
262 uint16_t bitnum=0;
263 uint16_t MaxBits = 500;
264 uint16_t errCnt = 0;
265 size_t i=1;
266 uint16_t bestErr = 1000;
267 uint16_t bestRun = 0;
268 size_t ii=1;
269 if (size == 0) return -1;
270 for (ii=1;ii<3;++ii){
271 i=1;
272 for (i=i+ii;i<*size-2;i+=2){
273 if(BitStream[i]==1 && (BitStream[i+1]==0)){
274 } else if((BitStream[i]==0)&& BitStream[i+1]==1){
275 } else {
276 errCnt++;
277 }
278 if(bitnum>MaxBits) break;
279 }
280 if (bestErr>errCnt){
281 bestErr=errCnt;
282 bestRun=ii;
283 }
284 errCnt=0;
285 }
286 errCnt=bestErr;
287 if (errCnt<20){
288 ii=bestRun;
289 i=1;
290 for (i=i+ii; i < *size-2; i+=2){
291 if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
292 BitStream[bitnum++]=0;
293 } else if((BitStream[i] == 0) && BitStream[i+1] == 1){
294 BitStream[bitnum++]=1;
295 } else {
296 BitStream[bitnum++]=77;
297 //errCnt++;
298 }
299 if(bitnum>MaxBits) break;
300 }
301 *size=bitnum;
302 }
303 return errCnt;
304 }
305
306 //by marshmellow
307 //take 01 or 10 = 0 and 11 or 00 = 1
308 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert)
309 {
310 uint16_t bitnum=0;
311 uint32_t errCnt =0;
312 uint32_t i;
313 uint16_t MaxBits=500;
314 i=offset;
315 if (size == 0) return -1;
316 for (;i<*size-2; i+=2){
317 if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
318 BitStream[bitnum++]=1^invert;
319 } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
320 BitStream[bitnum++]=invert;
321 } else {
322 BitStream[bitnum++]=77;
323 errCnt++;
324 }
325 if(bitnum>MaxBits) break;
326 }
327 *size=bitnum;
328 return errCnt;
329 }
330
331 //by marshmellow
332 void askAmp(uint8_t *BitStream, size_t size)
333 {
334 int shift = 127;
335 int shiftedVal=0;
336 for(int i = 1; i<size; i++){
337 if (BitStream[i]-BitStream[i-1]>=30) //large jump up
338 shift=127;
339 else if(BitStream[i]-BitStream[i-1]<=-20) //large jump down
340 shift=-127;
341
342 shiftedVal=BitStream[i]+shift;
343
344 if (shiftedVal>255)
345 shiftedVal=255;
346 else if (shiftedVal<0)
347 shiftedVal=0;
348 BitStream[i-1] = shiftedVal;
349 }
350 return;
351 }
352
353 //by marshmellow
354 //takes 3 arguments - clock, invert and maxErr as integers
355 //attempts to demodulate ask only
356 //prints binary found and saves in graphbuffer for further commands
357 int askrawdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp)
358 {
359 uint32_t i;
360 if (*size==0) return -1;
361 int start = DetectASKClock(BinStream, *size, clk, 20); //clock default
362 if (*clk==0) return -1;
363 if (start<0) return -1;
364 if (*invert != 0 && *invert != 1) *invert =0;
365 uint32_t initLoopMax = 200;
366 if (initLoopMax > *size) initLoopMax=*size;
367 // Detect high and lows
368 //25% fuzz in case highs and lows aren't clipped [marshmellow]
369 int high, low, ans;
370 if (amp==1) askAmp(BinStream, *size);
371 ans = getHiLo(BinStream, initLoopMax, &high, &low, 75, 75);
372 if (ans<1) return -1; //just noise
373
374 //PrintAndLog("DEBUG - valid high: %d - valid low: %d",high,low);
375 int lastBit = 0; //set first clock check
376 uint32_t bitnum = 0; //output counter
377 uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock
378 // if they fall + or - this value + clock from last valid wave
379 if (*clk == 32) tol=0; //clock tolerance may not be needed anymore currently set to
380 // + or - 1 but could be increased for poor waves or removed entirely
381 uint32_t iii = 0;
382 uint32_t gLen = *size;
383 if (gLen > 500) gLen=500;
384 uint8_t errCnt =0;
385 uint32_t bestStart = *size;
386 uint32_t bestErrCnt = maxErr; //(*size/1000);
387 uint8_t midBit=0;
388 uint16_t MaxBits=1000;
389 //PrintAndLog("DEBUG - lastbit - %d",lastBit);
390 //loop to find first wave that works
391 for (iii=start; iii < gLen; ++iii){
392 if ((BinStream[iii]>=high) || (BinStream[iii]<=low)){
393 lastBit=iii-*clk;
394 errCnt=0;
395 //loop through to see if this start location works
396 for (i = iii; i < *size; ++i) {
397 if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){
398 lastBit+=*clk;
399 midBit=0;
400 } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){
401 //low found and we are expecting a bar
402 lastBit+=*clk;
403 midBit=0;
404 } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
405 //mid bar?
406 midBit=1;
407 } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
408 //mid bar?
409 midBit=1;
410 } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){
411 //no mid bar found
412 midBit=1;
413 } else {
414 //mid value found or no bar supposed to be here
415
416 if ((i-lastBit)>(*clk+tol)){
417 //should have hit a high or low based on clock!!
418 //debug
419 //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);
420
421 errCnt++;
422 lastBit+=*clk;//skip over until hit too many errors
423 if (errCnt > maxErr){
424 //errCnt=0;
425 break;
426 }
427 }
428 }
429 if ((i-iii)>(MaxBits * *clk)) break; //got enough bits
430 }
431 //we got more than 64 good bits and not all errors
432 if ((((i-iii)/ *clk) > (64)) && (errCnt<=maxErr)) {
433 //possible good read
434 if (errCnt==0){
435 bestStart=iii;
436 bestErrCnt=errCnt;
437 break; //great read - finish
438 }
439 if (errCnt<bestErrCnt){ //set this as new best run
440 bestErrCnt=errCnt;
441 bestStart = iii;
442 }
443 }
444 }
445 }
446 if (bestErrCnt<=maxErr){
447 //best run is good enough - set to best run and overwrite BinStream
448 iii = bestStart;
449 lastBit = bestStart - *clk;
450 bitnum=0;
451 for (i = iii; i < *size; ++i) {
452 if ((BinStream[i] >= high) && ((i-lastBit) > (*clk-tol))){
453 lastBit += *clk;
454 BinStream[bitnum] = *invert;
455 bitnum++;
456 midBit=0;
457 } else if ((BinStream[i] <= low) && ((i-lastBit) > (*clk-tol))){
458 //low found and we are expecting a bar
459 lastBit+=*clk;
460 BinStream[bitnum] = 1 - *invert;
461 bitnum++;
462 midBit=0;
463 } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
464 //mid bar?
465 midBit=1;
466 BinStream[bitnum] = 1 - *invert;
467 bitnum++;
468 } else if ((BinStream[i]>=high) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
469 //mid bar?
470 midBit=1;
471 BinStream[bitnum] = *invert;
472 bitnum++;
473 } else if ((i-lastBit)>((*clk/2)+tol) && (midBit==0)){
474 //no mid bar found
475 midBit=1;
476 if (bitnum!=0) BinStream[bitnum] = BinStream[bitnum-1];
477 bitnum++;
478
479 } else {
480 //mid value found or no bar supposed to be here
481 if ((i-lastBit)>(*clk+tol)){
482 //should have hit a high or low based on clock!!
483
484 //debug
485 //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);
486 if (bitnum > 0){
487 BinStream[bitnum]=77;
488 bitnum++;
489 }
490 lastBit+=*clk;//skip over error
491 }
492 }
493 if (bitnum >= MaxBits) break;
494 }
495 *size=bitnum;
496 } else{
497 *invert=bestStart;
498 *clk=iii;
499 return -1;
500 }
501 return bestErrCnt;
502 }
503 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
504 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow)
505 {
506 uint32_t last_transition = 0;
507 uint32_t idx = 1;
508 //uint32_t maxVal=0;
509 if (fchigh==0) fchigh=10;
510 if (fclow==0) fclow=8;
511 //set the threshold close to 0 (graph) or 128 std to avoid static
512 uint8_t threshold_value = 123;
513
514 // sync to first lo-hi transition, and threshold
515
516 // Need to threshold first sample
517
518 if(dest[0] < threshold_value) dest[0] = 0;
519 else dest[0] = 1;
520
521 size_t numBits = 0;
522 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
523 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
524 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
525 for(idx = 1; idx < size; idx++) {
526 // threshold current value
527
528 if (dest[idx] < threshold_value) dest[idx] = 0;
529 else dest[idx] = 1;
530
531 // Check for 0->1 transition
532 if (dest[idx-1] < dest[idx]) { // 0 -> 1 transition
533 if ((idx-last_transition)<(fclow-2)){ //0-5 = garbage noise
534 //do nothing with extra garbage
535 } else if ((idx-last_transition) < (fchigh-1)) { //6-8 = 8 waves
536 dest[numBits]=1;
537 } else { //9+ = 10 waves
538 dest[numBits]=0;
539 }
540 last_transition = idx;
541 numBits++;
542 }
543 }
544 return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
545 }
546
547 uint32_t myround2(float f)
548 {
549 if (f >= 2000) return 2000;//something bad happened
550 return (uint32_t) (f + (float)0.5);
551 }
552
553 //translate 11111100000 to 10
554 size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxConsequtiveBits,
555 uint8_t invert, uint8_t fchigh, uint8_t fclow)
556 {
557 uint8_t lastval=dest[0];
558 uint32_t idx=0;
559 size_t numBits=0;
560 uint32_t n=1;
561
562 for( idx=1; idx < size; idx++) {
563
564 if (dest[idx]==lastval) {
565 n++;
566 continue;
567 }
568 //if lastval was 1, we have a 1->0 crossing
569 if ( dest[idx-1]==1 ) {
570 n=myround2((float)(n+1)/((float)(rfLen)/(float)fclow));
571 } else {// 0->1 crossing
572 n=myround2((float)(n+1)/((float)(rfLen-1)/(float)fchigh)); //-1 for fudge factor
573 }
574 if (n == 0) n = 1;
575
576 if(n < maxConsequtiveBits) //Consecutive
577 {
578 if(invert==0){ //invert bits
579 memset(dest+numBits, dest[idx-1] , n);
580 }else{
581 memset(dest+numBits, dest[idx-1]^1 , n);
582 }
583 numBits += n;
584 }
585 n=0;
586 lastval=dest[idx];
587 }//end for
588 return numBits;
589 }
590 //by marshmellow (from holiman's base)
591 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
592 int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow)
593 {
594 // FSK demodulator
595 size = fsk_wave_demod(dest, size, fchigh, fclow);
596 size = aggregate_bits(dest, size, rfLen, 192, invert, fchigh, fclow);
597 return size;
598 }
599
600 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
601 int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
602 {
603 if (justNoise(dest, *size)) return -1;
604
605 size_t numStart=0, size2=*size, startIdx=0;
606 // FSK demodulator
607 *size = fskdemod(dest, size2,50,1,10,8); //fsk2a
608 if (*size < 96) return -2;
609 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
610 uint8_t preamble[] = {0,0,0,1,1,1,0,1};
611 // find bitstring in array
612 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
613 if (errChk == 0) return -3; //preamble not found
614
615 numStart = startIdx + sizeof(preamble);
616 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
617 for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
618 if (dest[idx] == dest[idx+1]){
619 return -4; //not manchester data
620 }
621 *hi2 = (*hi2<<1)|(*hi>>31);
622 *hi = (*hi<<1)|(*lo>>31);
623 //Then, shift in a 0 or one into low
624 if (dest[idx] && !dest[idx+1]) // 1 0
625 *lo=(*lo<<1)|1;
626 else // 0 1
627 *lo=(*lo<<1)|0;
628 }
629 return (int)startIdx;
630 }
631
632 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
633 int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
634 {
635 if (justNoise(dest, *size)) return -1;
636
637 size_t numStart=0, size2=*size, startIdx=0;
638 // FSK demodulator
639 *size = fskdemod(dest, size2,50,1,10,8); //fsk2a
640 if (*size < 96) return -2;
641
642 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
643 uint8_t preamble[] = {0,0,0,0,1,1,1,1};
644
645 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
646 if (errChk == 0) return -3; //preamble not found
647
648 numStart = startIdx + sizeof(preamble);
649 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
650 for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
651 if (dest[idx] == dest[idx+1])
652 return -4; //not manchester data
653 *hi2 = (*hi2<<1)|(*hi>>31);
654 *hi = (*hi<<1)|(*lo>>31);
655 //Then, shift in a 0 or one into low
656 if (dest[idx] && !dest[idx+1]) // 1 0
657 *lo=(*lo<<1)|1;
658 else // 0 1
659 *lo=(*lo<<1)|0;
660 }
661 return (int)startIdx;
662 }
663
664 uint32_t bytebits_to_byte(uint8_t* src, size_t numbits)
665 {
666 uint32_t num = 0;
667 for(int i = 0 ; i < numbits ; i++)
668 {
669 num = (num << 1) | (*src);
670 src++;
671 }
672 return num;
673 }
674
675 int IOdemodFSK(uint8_t *dest, size_t size)
676 {
677 if (justNoise(dest, size)) return -1;
678 //make sure buffer has data
679 if (size < 66*64) return -2;
680 // FSK demodulator
681 size = fskdemod(dest, size, 64, 1, 10, 8); // FSK2a RF/64
682 if (size < 65) return -3; //did we get a good demod?
683 //Index map
684 //0 10 20 30 40 50 60
685 //| | | | | | |
686 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
687 //-----------------------------------------------------------------------------
688 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
689 //
690 //XSF(version)facility:codeone+codetwo
691 //Handle the data
692 size_t startIdx = 0;
693 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,1};
694 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), &size, &startIdx);
695 if (errChk == 0) return -4; //preamble not found
696
697 if (!dest[startIdx+8] && dest[startIdx+17]==1 && dest[startIdx+26]==1 && dest[startIdx+35]==1 && dest[startIdx+44]==1 && dest[startIdx+53]==1){
698 //confirmed proper separator bits found
699 //return start position
700 return (int) startIdx;
701 }
702 return -5;
703 }
704
705 // by marshmellow
706 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
707 // Parity Type (1 for odd 0 for even), and binary Length (length to run)
708 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen)
709 {
710 uint32_t parityWd = 0;
711 size_t j = 0, bitCnt = 0;
712 for (int word = 0; word < (bLen); word+=pLen){
713 for (int bit=0; bit < pLen; bit++){
714 parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
715 BitStream[j++] = (BitStream[startIdx+word+bit]);
716 }
717 j--;
718 // if parity fails then return 0
719 if (parityTest(parityWd, pLen, pType) == 0) return -1;
720 bitCnt+=(pLen-1);
721 parityWd = 0;
722 }
723 // if we got here then all the parities passed
724 //return ID start index and size
725 return bitCnt;
726 }
727
728 // by marshmellow
729 // FSK Demod then try to locate an AWID ID
730 int AWIDdemodFSK(uint8_t *dest, size_t *size)
731 {
732 //make sure buffer has enough data
733 if (*size < 96*50) return -1;
734
735 if (justNoise(dest, *size)) return -2;
736
737 // FSK demodulator
738 *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
739 if (*size < 96) return -3; //did we get a good demod?
740
741 uint8_t preamble[] = {0,0,0,0,0,0,0,1};
742 size_t startIdx = 0;
743 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
744 if (errChk == 0) return -4; //preamble not found
745 if (*size != 96) return -5;
746 return (int)startIdx;
747 }
748
749 // by marshmellow
750 // FSK Demod then try to locate an Farpointe Data (pyramid) ID
751 int PyramiddemodFSK(uint8_t *dest, size_t *size)
752 {
753 //make sure buffer has data
754 if (*size < 128*50) return -5;
755
756 //test samples are not just noise
757 if (justNoise(dest, *size)) return -1;
758
759 // FSK demodulator
760 *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
761 if (*size < 128) return -2; //did we get a good demod?
762
763 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
764 size_t startIdx = 0;
765 uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
766 if (errChk == 0) return -4; //preamble not found
767 if (*size != 128) return -3;
768 return (int)startIdx;
769 }
770
771
772 uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, int high, int low)
773 {
774 uint8_t allPeaks=1;
775 uint16_t cntPeaks=0;
776 for (size_t i=20; i<255; i++){
777 if (dest[i]>low && dest[i]<high)
778 allPeaks=0;
779 else
780 cntPeaks++;
781 }
782 if (allPeaks==0){
783 if (cntPeaks>190) return 1;
784 }
785 return allPeaks;
786 }
787
788 // by marshmellow
789 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
790 // maybe somehow adjust peak trimming value based on samples to fix?
791 // return start index of best starting position for that clock and return clock (by reference)
792 int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr)
793 {
794 int i=0;
795 int clk[]={8,16,32,40,50,64,100,128,256};
796 int loopCnt = 256; //don't need to loop through entire array...
797 if (size == 0) return -1;
798 if (size<loopCnt) loopCnt = size;
799 //if we already have a valid clock quit
800
801 for (;i<8;++i)
802 if (clk[i] == *clock) return 0;
803
804 //get high and low peak
805 int peak, low;
806 getHiLo(dest, loopCnt, &peak, &low, 75, 75);
807
808 //test for large clean peaks
809 if (DetectCleanAskWave(dest, size, peak, low)==1){
810 uint16_t fcTest=0;
811 uint8_t mostFC=0;
812 fcTest=countFC(dest, size, &mostFC);
813 uint8_t fc1 = fcTest >> 8;
814 uint8_t fc2 = fcTest & 0xFF;
815
816 for (i=0; i<8; i++){
817 if (clk[i] == fc1) {
818 *clock=fc1;
819 return 0;
820 }
821 if (clk[i] == fc2) {
822 *clock=fc2;
823 return 0;
824 }
825 }
826 }
827
828 int ii;
829 int clkCnt;
830 int tol = 0;
831 int bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
832 int bestStart[]={0,0,0,0,0,0,0,0,0};
833 int errCnt=0;
834 //test each valid clock from smallest to greatest to see which lines up
835 for(clkCnt=0; clkCnt < 8; clkCnt++){
836 if (clk[clkCnt] == 32){
837 tol=1;
838 }else{
839 tol=0;
840 }
841 bestErr[clkCnt]=1000;
842 //try lining up the peaks by moving starting point (try first 256)
843 for (ii=0; ii < loopCnt; ii++){
844 if ((dest[ii] >= peak) || (dest[ii] <= low)){
845 errCnt=0;
846 // now that we have the first one lined up test rest of wave array
847 for (i=0; i<((int)((size-ii-tol)/clk[clkCnt])-1); ++i){
848 if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
849 }else if(dest[ii+(i*clk[clkCnt])-tol]>=peak || dest[ii+(i*clk[clkCnt])-tol]<=low){
850 }else if(dest[ii+(i*clk[clkCnt])+tol]>=peak || dest[ii+(i*clk[clkCnt])+tol]<=low){
851 }else{ //error no peak detected
852 errCnt++;
853 }
854 }
855 //if we found no errors then we can stop here
856 // this is correct one - return this clock
857 //PrintAndLog("DEBUG: clk %d, err %d, ii %d, i %d",clk[clkCnt],errCnt,ii,i);
858 if(errCnt==0 && clkCnt<6) {
859 *clock = clk[clkCnt];
860 return ii;
861 }
862 //if we found errors see if it is lowest so far and save it as best run
863 if(errCnt<bestErr[clkCnt]){
864 bestErr[clkCnt]=errCnt;
865 bestStart[clkCnt]=ii;
866 }
867 }
868 }
869 }
870 uint8_t iii=0;
871 uint8_t best=0;
872 for (iii=0; iii<8; ++iii){
873 if (bestErr[iii]<bestErr[best]){
874 if (bestErr[iii]==0) bestErr[iii]=1;
875 // current best bit to error ratio vs new bit to error ratio
876 if (((size/clk[best])/bestErr[best] < (size/clk[iii])/bestErr[iii]) ){
877 best = iii;
878 }
879 }
880 }
881 if (bestErr[best]>maxErr) return -1;
882 *clock=clk[best];
883 return bestStart[best];
884 }
885
886 //by marshmellow
887 //detect psk clock by reading each phase shift
888 // a phase shift is determined by measuring the sample length of each wave
889 int DetectPSKClock(uint8_t dest[], size_t size, int clock)
890 {
891 uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
892 uint16_t loopCnt = 4096; //don't need to loop through entire array...
893 if (size == 0) return 0;
894 if (size<loopCnt) loopCnt = size;
895
896 //if we already have a valid clock quit
897 size_t i=1;
898 for (; i < 8; ++i)
899 if (clk[i] == clock) return clock;
900
901 size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
902 uint8_t clkCnt, fc=0, fullWaveLen=0, tol=1;
903 uint16_t peakcnt=0, errCnt=0, waveLenCnt=0;
904 uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
905 uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0};
906 countFC(dest, size, &fc);
907 //PrintAndLog("DEBUG: FC: %d",fc);
908
909 //find first full wave
910 for (i=0; i<loopCnt; i++){
911 if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
912 if (waveStart == 0) {
913 waveStart = i+1;
914 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
915 } else {
916 waveEnd = i+1;
917 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
918 waveLenCnt = waveEnd-waveStart;
919 if (waveLenCnt > fc){
920 firstFullWave = waveStart;
921 fullWaveLen=waveLenCnt;
922 break;
923 }
924 waveStart=0;
925 }
926 }
927 }
928 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
929
930 //test each valid clock from greatest to smallest to see which lines up
931 for(clkCnt=7; clkCnt >= 1 ; clkCnt--){
932 lastClkBit = firstFullWave; //set end of wave as clock align
933 waveStart = 0;
934 errCnt=0;
935 peakcnt=0;
936 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
937
938 for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){
939 //top edge of wave = start of new wave
940 if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
941 if (waveStart == 0) {
942 waveStart = i+1;
943 waveLenCnt=0;
944 } else { //waveEnd
945 waveEnd = i+1;
946 waveLenCnt = waveEnd-waveStart;
947 if (waveLenCnt > fc){
948 //if this wave is a phase shift
949 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc);
950 if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
951 peakcnt++;
952 lastClkBit+=clk[clkCnt];
953 } else if (i<lastClkBit+8){
954 //noise after a phase shift - ignore
955 } else { //phase shift before supposed to based on clock
956 errCnt++;
957 }
958 } else if (i+1 > lastClkBit + clk[clkCnt] + tol + fc){
959 lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
960 }
961 waveStart=i+1;
962 }
963 }
964 }
965 if (errCnt == 0){
966 return clk[clkCnt];
967 }
968 if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt]=errCnt;
969 if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt]=peakcnt;
970 }
971 //all tested with errors
972 //return the highest clk with the most peaks found
973 uint8_t best=7;
974 for (i=7; i>=1; i--){
975 if (peaksdet[i] > peaksdet[best]) {
976 best = i;
977 }
978 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
979 }
980 return clk[best];
981 }
982
983 //by marshmellow
984 //detect nrz clock by reading #peaks vs no peaks(or errors)
985 int DetectNRZClock(uint8_t dest[], size_t size, int clock)
986 {
987 int i=0;
988 int clk[]={8,16,32,40,50,64,100,128,256};
989 int loopCnt = 4096; //don't need to loop through entire array...
990 if (size == 0) return 0;
991 if (size<loopCnt) loopCnt = size;
992
993 //if we already have a valid clock quit
994 for (; i < 8; ++i)
995 if (clk[i] == clock) return clock;
996
997 //get high and low peak
998 int peak, low;
999 getHiLo(dest, loopCnt, &peak, &low, 75, 75);
1000
1001 //PrintAndLog("DEBUG: peak: %d, low: %d",peak,low);
1002 int ii;
1003 uint8_t clkCnt;
1004 uint8_t tol = 0;
1005 int peakcnt=0;
1006 int peaksdet[]={0,0,0,0,0,0,0,0};
1007 int maxPeak=0;
1008 //test for large clipped waves
1009 for (i=0; i<loopCnt; i++){
1010 if (dest[i] >= peak || dest[i] <= low){
1011 peakcnt++;
1012 } else {
1013 if (peakcnt>0 && maxPeak < peakcnt){
1014 maxPeak = peakcnt;
1015 }
1016 peakcnt=0;
1017 }
1018 }
1019 peakcnt=0;
1020 //test each valid clock from smallest to greatest to see which lines up
1021 for(clkCnt=0; clkCnt < 8; ++clkCnt){
1022 //ignore clocks smaller than largest peak
1023 if (clk[clkCnt]<maxPeak) continue;
1024
1025 //try lining up the peaks by moving starting point (try first 256)
1026 for (ii=0; ii< loopCnt; ++ii){
1027 if ((dest[ii] >= peak) || (dest[ii] <= low)){
1028 peakcnt=0;
1029 // now that we have the first one lined up test rest of wave array
1030 for (i=0; i < ((int)((size-ii-tol)/clk[clkCnt])-1); ++i){
1031 if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){
1032 peakcnt++;
1033 }
1034 }
1035 if(peakcnt>peaksdet[clkCnt]) {
1036 peaksdet[clkCnt]=peakcnt;
1037 }
1038 }
1039 }
1040 }
1041 int iii=7;
1042 int best=0;
1043 for (iii=7; iii > 0; iii--){
1044 if (peaksdet[iii] > peaksdet[best]){
1045 best = iii;
1046 }
1047 //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]);
1048 }
1049 return clk[best];
1050 }
1051
1052 // by marshmellow
1053 // convert psk1 demod to psk2 demod
1054 // only transition waves are 1s
1055 void psk1TOpsk2(uint8_t *BitStream, size_t size)
1056 {
1057 size_t i=1;
1058 uint8_t lastBit=BitStream[0];
1059 for (; i<size; i++){
1060 if (lastBit!=BitStream[i]){
1061 lastBit=BitStream[i];
1062 BitStream[i]=1;
1063 } else {
1064 BitStream[i]=0;
1065 }
1066 }
1067 return;
1068 }
1069
1070 // redesigned by marshmellow adjusted from existing decode functions
1071 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1072 int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert)
1073 {
1074 //26 bit 40134 format (don't know other formats)
1075 int i;
1076 int long_wait=29;//29 leading zeros in format
1077 int start;
1078 int first = 0;
1079 int first2 = 0;
1080 int bitCnt = 0;
1081 int ii;
1082 // Finding the start of a UID
1083 for (start = 0; start <= *size - 250; start++) {
1084 first = bitStream[start];
1085 for (i = start; i < start + long_wait; i++) {
1086 if (bitStream[i] != first) {
1087 break;
1088 }
1089 }
1090 if (i == (start + long_wait)) {
1091 break;
1092 }
1093 }
1094 if (start == *size - 250 + 1) {
1095 // did not find start sequence
1096 return -1;
1097 }
1098 // Inverting signal if needed
1099 if (first == 1) {
1100 for (i = start; i < *size; i++) {
1101 bitStream[i] = !bitStream[i];
1102 }
1103 *invert = 1;
1104 }else *invert=0;
1105
1106 int iii;
1107 //found start once now test length by finding next one
1108 for (ii=start+29; ii <= *size - 250; ii++) {
1109 first2 = bitStream[ii];
1110 for (iii = ii; iii < ii + long_wait; iii++) {
1111 if (bitStream[iii] != first2) {
1112 break;
1113 }
1114 }
1115 if (iii == (ii + long_wait)) {
1116 break;
1117 }
1118 }
1119 if (ii== *size - 250 + 1){
1120 // did not find second start sequence
1121 return -2;
1122 }
1123 bitCnt=ii-start;
1124
1125 // Dumping UID
1126 i = start;
1127 for (ii = 0; ii < bitCnt; ii++) {
1128 bitStream[ii] = bitStream[i++];
1129 }
1130 *size=bitCnt;
1131 return 1;
1132 }
1133
1134 // by marshmellow - demodulate NRZ wave (both similar enough)
1135 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1136 // there probably is a much simpler way to do this....
1137 int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr)
1138 {
1139 if (justNoise(dest, *size)) return -1;
1140 *clk = DetectNRZClock(dest, *size, *clk);
1141 if (*clk==0) return -2;
1142 uint32_t i;
1143 int high, low, ans;
1144 ans = getHiLo(dest, 1260, &high, &low, 75, 75); //25% fuzz on high 25% fuzz on low
1145 if (ans<1) return -2; //just noise
1146 uint32_t gLen = 256;
1147 if (gLen>*size) gLen = *size;
1148 int lastBit = 0; //set first clock check
1149 uint32_t bitnum = 0; //output counter
1150 uint8_t tol = 1; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
1151 uint32_t iii = 0;
1152 uint16_t errCnt =0;
1153 uint16_t MaxBits = 1000;
1154 uint32_t bestErrCnt = maxErr+1;
1155 uint32_t bestPeakCnt = 0;
1156 uint32_t bestPeakStart=0;
1157 uint8_t curBit=0;
1158 uint8_t bitHigh=0;
1159 uint8_t errBitHigh=0;
1160 uint16_t peakCnt=0;
1161 uint8_t ignoreWindow=4;
1162 uint8_t ignoreCnt=ignoreWindow; //in case of noice near peak
1163 //loop to find first wave that works - align to clock
1164 for (iii=0; iii < gLen; ++iii){
1165 if ((dest[iii]>=high) || (dest[iii]<=low)){
1166 lastBit=iii-*clk;
1167 peakCnt=0;
1168 errCnt=0;
1169 bitnum=0;
1170 //loop through to see if this start location works
1171 for (i = iii; i < *size; ++i) {
1172 //if we found a high bar and we are at a clock bit
1173 if ((dest[i]>=high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1174 bitHigh=1;
1175 lastBit+=*clk;
1176 bitnum++;
1177 peakCnt++;
1178 errBitHigh=0;
1179 ignoreCnt=ignoreWindow;
1180 //else if low bar found and we are at a clock point
1181 }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1182 bitHigh=1;
1183 lastBit+=*clk;
1184 bitnum++;
1185 peakCnt++;
1186 errBitHigh=0;
1187 ignoreCnt=ignoreWindow;
1188 //else if no bars found
1189 }else if(dest[i] < high && dest[i] > low) {
1190 if (ignoreCnt==0){
1191 bitHigh=0;
1192 if (errBitHigh==1){
1193 errCnt++;
1194 }
1195 errBitHigh=0;
1196 } else {
1197 ignoreCnt--;
1198 }
1199 //if we are past a clock point
1200 if (i >= lastBit+*clk+tol){ //clock val
1201 lastBit+=*clk;
1202 bitnum++;
1203 }
1204 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1205 }else if ((dest[i]>=high || dest[i]<=low) && (i<lastBit+*clk-tol || i>lastBit+*clk+tol) && (bitHigh==0)){
1206 //error bar found no clock...
1207 errBitHigh=1;
1208 }
1209 if (bitnum>=MaxBits) break;
1210 }
1211 //we got more than 64 good bits and not all errors
1212 if (bitnum > (64) && (errCnt <= (maxErr))) {
1213 //possible good read
1214 if (errCnt == 0){
1215 //bestStart = iii;
1216 bestErrCnt = errCnt;
1217 bestPeakCnt = peakCnt;
1218 bestPeakStart = iii;
1219 break; //great read - finish
1220 }
1221 if (errCnt < bestErrCnt){ //set this as new best run
1222 bestErrCnt = errCnt;
1223 //bestStart = iii;
1224 }
1225 if (peakCnt > bestPeakCnt){
1226 bestPeakCnt=peakCnt;
1227 bestPeakStart=iii;
1228 }
1229 }
1230 }
1231 }
1232 //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart);
1233 if (bestErrCnt <= maxErr){
1234 //best run is good enough set to best run and set overwrite BinStream
1235 iii=bestPeakStart;
1236 lastBit=bestPeakStart-*clk;
1237 bitnum=0;
1238 for (i = iii; i < *size; ++i) {
1239 //if we found a high bar and we are at a clock bit
1240 if ((dest[i] >= high ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1241 bitHigh=1;
1242 lastBit+=*clk;
1243 curBit=1-*invert;
1244 dest[bitnum]=curBit;
1245 bitnum++;
1246 errBitHigh=0;
1247 ignoreCnt=ignoreWindow;
1248 //else if low bar found and we are at a clock point
1249 }else if ((dest[i]<=low ) && (i>=lastBit+*clk-tol && i<=lastBit+*clk+tol)){
1250 bitHigh=1;
1251 lastBit+=*clk;
1252 curBit=*invert;
1253 dest[bitnum]=curBit;
1254 bitnum++;
1255 errBitHigh=0;
1256 ignoreCnt=ignoreWindow;
1257 //else if no bars found
1258 }else if(dest[i]<high && dest[i]>low) {
1259 if (ignoreCnt==0){
1260 bitHigh=0;
1261 //if peak is done was it an error peak?
1262 if (errBitHigh==1){
1263 dest[bitnum]=77;
1264 bitnum++;
1265 errCnt++;
1266 }
1267 errBitHigh=0;
1268 } else {
1269 ignoreCnt--;
1270 }
1271 //if we are past a clock point
1272 if (i>=lastBit+*clk+tol){ //clock val
1273 lastBit+=*clk;
1274 dest[bitnum]=curBit;
1275 bitnum++;
1276 }
1277 //else if bar found but we are not at a clock bit and we did not just have a clock bit
1278 }else if ((dest[i]>=high || dest[i]<=low) && ((i<lastBit+*clk-tol) || (i>lastBit+*clk+tol)) && (bitHigh==0)){
1279 //error bar found no clock...
1280 errBitHigh=1;
1281 }
1282 if (bitnum >= MaxBits) break;
1283 }
1284 *size=bitnum;
1285 } else{
1286 *size=bitnum;
1287 return -1;
1288 }
1289
1290 if (bitnum>16){
1291 *size=bitnum;
1292 } else return -1;
1293 return errCnt;
1294 }
1295
1296 //by marshmellow
1297 //detects the bit clock for FSK given the high and low Field Clocks
1298 uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow)
1299 {
1300 uint8_t clk[] = {8,16,32,40,50,64,100,128,0};
1301 uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1302 uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1303 uint8_t rfLensFnd = 0;
1304 uint8_t lastFCcnt=0;
1305 uint32_t fcCounter = 0;
1306 uint16_t rfCounter = 0;
1307 uint8_t firstBitFnd = 0;
1308 size_t i;
1309 if (size == 0) return 0;
1310
1311 uint8_t fcTol = (uint8_t)(0.5+(float)(fcHigh-fcLow)/2);
1312 rfLensFnd=0;
1313 fcCounter=0;
1314 rfCounter=0;
1315 firstBitFnd=0;
1316 //PrintAndLog("DEBUG: fcTol: %d",fcTol);
1317 // prime i to first up transition
1318 for (i = 1; i < size-1; i++)
1319 if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1])
1320 break;
1321
1322 for (; i < size-1; i++){
1323 if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1]){
1324 // new peak
1325 fcCounter++;
1326 rfCounter++;
1327 // if we got less than the small fc + tolerance then set it to the small fc
1328 if (fcCounter < fcLow+fcTol)
1329 fcCounter = fcLow;
1330 else //set it to the large fc
1331 fcCounter = fcHigh;
1332
1333 //look for bit clock (rf/xx)
1334 if ((fcCounter<lastFCcnt || fcCounter>lastFCcnt)){
1335 //not the same size as the last wave - start of new bit sequence
1336
1337 if (firstBitFnd>1){ //skip first wave change - probably not a complete bit
1338 for (int ii=0; ii<15; ii++){
1339 if (rfLens[ii]==rfCounter){
1340 rfCnts[ii]++;
1341 rfCounter=0;
1342 break;
1343 }
1344 }
1345 if (rfCounter>0 && rfLensFnd<15){
1346 //PrintAndLog("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1347 rfCnts[rfLensFnd]++;
1348 rfLens[rfLensFnd++]=rfCounter;
1349 }
1350 } else {
1351 firstBitFnd++;
1352 }
1353 rfCounter=0;
1354 lastFCcnt=fcCounter;
1355 }
1356 fcCounter=0;
1357 } else {
1358 // count sample
1359 fcCounter++;
1360 rfCounter++;
1361 }
1362 }
1363 uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15;
1364
1365 for (i=0; i<15; i++){
1366 //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1367 //get highest 2 RF values (might need to get more values to compare or compare all?)
1368 if (rfCnts[i]>rfCnts[rfHighest]){
1369 rfHighest3=rfHighest2;
1370 rfHighest2=rfHighest;
1371 rfHighest=i;
1372 } else if(rfCnts[i]>rfCnts[rfHighest2]){
1373 rfHighest3=rfHighest2;
1374 rfHighest2=i;
1375 } else if(rfCnts[i]>rfCnts[rfHighest3]){
1376 rfHighest3=i;
1377 }
1378 }
1379 // set allowed clock remainder tolerance to be 1 large field clock length+1
1380 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1381 uint8_t tol1 = fcHigh+1;
1382
1383 //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1384
1385 // loop to find the highest clock that has a remainder less than the tolerance
1386 // compare samples counted divided by
1387 int ii=7;
1388 for (; ii>=0; ii--){
1389 if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){
1390 if (rfLens[rfHighest2] % clk[ii] < tol1 || rfLens[rfHighest2] % clk[ii] > clk[ii]-tol1){
1391 if (rfLens[rfHighest3] % clk[ii] < tol1 || rfLens[rfHighest3] % clk[ii] > clk[ii]-tol1){
1392 break;
1393 }
1394 }
1395 }
1396 }
1397
1398 if (ii<0) return 0; // oops we went too far
1399
1400 return clk[ii];
1401 }
1402
1403 //by marshmellow
1404 //countFC is to detect the field clock lengths.
1405 //counts and returns the 2 most common wave lengths
1406 //mainly used for FSK field clock detection
1407 uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t *mostFC)
1408 {
1409 uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0};
1410 uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0};
1411 uint8_t fcLensFnd = 0;
1412 uint8_t lastFCcnt=0;
1413 uint32_t fcCounter = 0;
1414 size_t i;
1415 if (size == 0) return 0;
1416
1417 // prime i to first up transition
1418 for (i = 1; i < size-1; i++)
1419 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
1420 break;
1421
1422 for (; i < size-1; i++){
1423 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
1424 // new up transition
1425 fcCounter++;
1426
1427 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1428 if (lastFCcnt==5 && fcCounter==9) fcCounter--;
1429 //if odd and not rc/5 add one (for when we get a fc 9 instead of 10)
1430 if ((fcCounter==9 && fcCounter & 1) || fcCounter==4) fcCounter++;
1431
1432 // save last field clock count (fc/xx)
1433 // find which fcLens to save it to:
1434 for (int ii=0; ii<10; ii++){
1435 if (fcLens[ii]==fcCounter){
1436 fcCnts[ii]++;
1437 fcCounter=0;
1438 break;
1439 }
1440 }
1441 if (fcCounter>0 && fcLensFnd<10){
1442 //add new fc length
1443 fcCnts[fcLensFnd]++;
1444 fcLens[fcLensFnd++]=fcCounter;
1445 }
1446 fcCounter=0;
1447 } else {
1448 // count sample
1449 fcCounter++;
1450 }
1451 }
1452
1453 uint8_t best1=9, best2=9, best3=9;
1454 uint16_t maxCnt1=0;
1455 // go through fclens and find which ones are bigest 2
1456 for (i=0; i<10; i++){
1457 // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt);
1458 // get the 3 best FC values
1459 if (fcCnts[i]>maxCnt1) {
1460 best3=best2;
1461 best2=best1;
1462 maxCnt1=fcCnts[i];
1463 best1=i;
1464 } else if(fcCnts[i]>fcCnts[best2]){
1465 best3=best2;
1466 best2=i;
1467 } else if(fcCnts[i]>fcCnts[best3]){
1468 best3=i;
1469 }
1470 }
1471 uint8_t fcH=0, fcL=0;
1472 if (fcLens[best1]>fcLens[best2]){
1473 fcH=fcLens[best1];
1474 fcL=fcLens[best2];
1475 } else{
1476 fcH=fcLens[best2];
1477 fcL=fcLens[best1];
1478 }
1479
1480 *mostFC=fcLens[best1];
1481 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1482
1483 uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
1484 // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]);
1485
1486 return fcs;
1487 }
1488
1489 //by marshmellow
1490 //countPSK_FC is to detect the psk carrier clock length.
1491 //counts and returns the 1 most common wave length
1492 uint8_t countPSK_FC(uint8_t *BitStream, size_t size)
1493 {
1494 uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0};
1495 uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0};
1496 uint8_t fcLensFnd = 0;
1497 uint32_t fcCounter = 0;
1498 size_t i;
1499 if (size == 0) return 0;
1500
1501 // prime i to first up transition
1502 for (i = 1; i < size-1; i++)
1503 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
1504 break;
1505
1506 for (; i < size-1; i++){
1507 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
1508 // new up transition
1509 fcCounter++;
1510
1511 // save last field clock count (fc/xx)
1512 // find which fcLens to save it to:
1513 for (int ii=0; ii<10; ii++){
1514 if (fcLens[ii]==fcCounter){
1515 fcCnts[ii]++;
1516 fcCounter=0;
1517 break;
1518 }
1519 }
1520 if (fcCounter>0 && fcLensFnd<10){
1521 //add new fc length
1522 fcCnts[fcLensFnd]++;
1523 fcLens[fcLensFnd++]=fcCounter;
1524 }
1525 fcCounter=0;
1526 } else {
1527 // count sample
1528 fcCounter++;
1529 }
1530 }
1531
1532 uint8_t best1=9;
1533 uint16_t maxCnt1=0;
1534 // go through fclens and find which ones are bigest
1535 for (i=0; i<10; i++){
1536 //PrintAndLog("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]);
1537 // get the best FC value
1538 if (fcCnts[i]>maxCnt1) {
1539 maxCnt1=fcCnts[i];
1540 best1=i;
1541 }
1542 }
1543 return fcLens[best1];
1544 }
1545
1546 //by marshmellow - demodulate PSK1 wave
1547 //uses wave lengths (# Samples)
1548 int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
1549 {
1550 uint16_t loopCnt = 4096; //don't need to loop through entire array...
1551 if (size == 0) return -1;
1552 if (*size<loopCnt) loopCnt = *size;
1553
1554 uint8_t curPhase = *invert;
1555 size_t i, waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
1556 uint8_t fc=0, fullWaveLen=0, tol=1;
1557 uint16_t errCnt=0, waveLenCnt=0;
1558 fc = countPSK_FC(dest, *size);
1559 if (fc!=2 && fc!=4 && fc!=8) return -1;
1560 //PrintAndLog("DEBUG: FC: %d",fc);
1561 *clock = DetectPSKClock(dest, *size, *clock);
1562 if (*clock==0) return -1;
1563 int avgWaveVal=0, lastAvgWaveVal=0;
1564 //find first full wave
1565 for (i=0; i<loopCnt; i++){
1566 if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
1567 if (waveStart == 0) {
1568 waveStart = i+1;
1569 avgWaveVal=dest[i+1];
1570 //PrintAndLog("DEBUG: waveStart: %d",waveStart);
1571 } else {
1572 waveEnd = i+1;
1573 //PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
1574 waveLenCnt = waveEnd-waveStart;
1575 lastAvgWaveVal = avgWaveVal/waveLenCnt;
1576 if (waveLenCnt > fc){
1577 firstFullWave = waveStart;
1578 fullWaveLen=waveLenCnt;
1579 //if average wave value is > graph 0 then it is an up wave or a 1
1580 if (lastAvgWaveVal > 128) curPhase^=1;
1581 break;
1582 }
1583 waveStart=0;
1584 avgWaveVal=0;
1585 }
1586 }
1587 avgWaveVal+=dest[i+1];
1588 }
1589 //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1590 lastClkBit = firstFullWave; //set start of wave as clock align
1591 waveStart = 0;
1592 errCnt=0;
1593 size_t numBits=0;
1594 //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit);
1595
1596 for (i = firstFullWave+fullWaveLen-1; i < *size-3; i++){
1597 //top edge of wave = start of new wave
1598 if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
1599 if (waveStart == 0) {
1600 waveStart = i+1;
1601 waveLenCnt=0;
1602 avgWaveVal = dest[i+1];
1603 } else { //waveEnd
1604 waveEnd = i+1;
1605 waveLenCnt = waveEnd-waveStart;
1606 lastAvgWaveVal = avgWaveVal/waveLenCnt;
1607 if (waveLenCnt > fc){
1608 //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1609 //if this wave is a phase shift
1610 //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1611 if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
1612 curPhase^=1;
1613 dest[numBits] = curPhase;
1614 numBits++;
1615 lastClkBit += *clock;
1616 } else if (i<lastClkBit+10){
1617 //noise after a phase shift - ignore
1618 } else { //phase shift before supposed to based on clock
1619 errCnt++;
1620 dest[numBits] = 77;
1621 numBits++;
1622 }
1623 } else if (i+1 > lastClkBit + *clock + tol + fc){
1624 lastClkBit += *clock; //no phase shift but clock bit
1625 dest[numBits] = curPhase;
1626 numBits++;
1627 }
1628 avgWaveVal=0;
1629 waveStart=i+1;
1630 }
1631 }
1632 avgWaveVal+=dest[i+1];
1633 }
1634 *size = numBits;
1635 return errCnt;
1636 }
Impressum, Datenschutz