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