]> cvs.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
FIX: fixed a little bug I introduced from last commit in fskdemod
[proxmark3-svn] / common / lfdemod.c
1 //-----------------------------------------------------------------------------
2 // Copyright (C) 2014
3 //
4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
5 // at your option, any later version. See the LICENSE.txt file for the text of
6 // the license.
7 //-----------------------------------------------------------------------------
8 // Low frequency commands
9 //-----------------------------------------------------------------------------
10
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <string.h>
14 #include "lfdemod.h"
15
16 //by marshmellow
17 //takes 1s and 0s and searches for EM410x format - output EM ID
18 uint64_t Em410xDecode(uint8_t *BitStream, uint32_t BitLen)
19 {
20 //no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
21 // otherwise could be a void with no arguments
22 //set defaults
23 int high = 0, low = 128;
24 uint64_t lo = 0;
25 uint32_t i = 0;
26 uint32_t initLoopMax = 65;
27
28 if (initLoopMax > BitLen)
29 initLoopMax = BitLen;
30
31 for (; i < initLoopMax; ++i) //65 samples should be plenty to find high and low values
32 {
33 if (BitStream[i] > high)
34 high = BitStream[i];
35 else if (BitStream[i] < low)
36 low = BitStream[i];
37 }
38
39 if (((high !=1)||(low !=0))){ //allow only 1s and 0s
40 return 0;
41 }
42
43 uint8_t parityTest = 0;
44 // 111111111 bit pattern represent start of frame
45 uint8_t frame_marker_mask[] = {1,1,1,1,1,1,1,1,1};
46 uint32_t idx = 0;
47 uint32_t j = 0;
48 uint8_t resetCnt = 0;
49 while( (idx + 64) < BitLen) {
50
51 restart:
52
53 // search for a start of frame marker
54 if ( memcmp(BitStream+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0) {
55 // frame marker found
56 idx += 9;//sizeof(frame_marker_mask);
57 for ( i = 0; i < 10; ++i){
58 for( j = 0; j < 5; ++j){
59 parityTest += BitStream[(i*5) + j + idx];
60 }
61 if (parityTest == ( (parityTest >> 1) << 1)){
62 parityTest = 0;
63 for (j = 0; j < 4; ++j){
64 lo = ( lo << 1LL)|( BitStream[( i * 5 ) + j + idx]);
65 }
66 } else {
67 //parity failed
68 parityTest = 0;
69 idx -= 8;
70 if (resetCnt > 5) return 0;
71 resetCnt++;
72 goto restart;//continue;
73 }
74 }
75 //skip last 5 bit parity test for simplicity.
76 return lo;
77 } else {
78 idx++;
79 }
80 }
81 return 0;
82 }
83
84 //by marshmellow
85 //takes 2 arguments - clock and invert both as integers
86 //attempts to demodulate ask while decoding manchester
87 //prints binary found and saves in graphbuffer for further commands
88 int askmandemod(uint8_t *BinStream, uint32_t *BitLen, int *clk, int *invert)
89 {
90 int i;
91 int high = 0, low = 128;
92 *clk = DetectASKClock(BinStream, (size_t)*BitLen, *clk); //clock default
93
94 if (*clk < 8 ) *clk = 64;
95 if (*clk < 32 ) *clk = 32;
96 if (*invert != 1) *invert = 0;
97
98 uint32_t initLoopMax = 200;
99 if (initLoopMax > *BitLen)
100 initLoopMax = *BitLen;
101
102 // Detect high and lows
103 // 200 samples should be enough to find high and low values
104 for (i = 0; i < initLoopMax; ++i) {
105 if (BinStream[i] > high)
106 high = BinStream[i];
107 else if (BinStream[i] < low)
108 low = BinStream[i];
109 }
110
111 //throw away static
112 if ((high < 158) )
113 return -2;
114
115 //25% fuzz in case highs and lows aren't clipped [marshmellow]
116 high = (int)(high * .75);
117 low = (int)(low+128 * .25);
118
119 int lastBit = 0; // set first clock check
120 uint32_t bitnum = 0; // output counter
121
122 // clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
123 //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely
124 int tol = ( *clk == 32 ) ? 1 : 0;
125
126 int j = 0;
127 uint32_t gLen = *BitLen;
128
129 if (gLen > 3000) gLen = 3000;
130
131 uint8_t errCnt = 0;
132 uint32_t bestStart = *BitLen;
133 uint32_t bestErrCnt = (*BitLen/1000);
134 uint32_t maxErr = bestErrCnt;
135
136 //loop to find first wave that works
137 for (j=0; j < gLen; ++j){
138
139 if ((BinStream[j] >= high)||(BinStream[j] <= low)){
140 lastBit = j - *clk;
141 errCnt = 0;
142
143 //loop through to see if this start location works
144 for (i = j; i < *BitLen; ++i) {
145 if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){
146 lastBit += *clk;
147 } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){
148 //low found and we are expecting a bar
149 lastBit += *clk;
150 } else {
151 //mid value found or no bar supposed to be here
152 if ((i-lastBit) > (*clk + tol)){
153 //should have hit a high or low based on clock!!
154
155 errCnt++;
156 lastBit += *clk;//skip over until hit too many errors
157 if (errCnt > maxErr) break; //allow 1 error for every 1000 samples else start over
158 }
159 }
160 if ((i-j) >(400 * *clk)) break; //got plenty of bits
161 }
162 //we got more than 64 good bits and not all errors
163 if ((((i-j)/ *clk) > (64 + errCnt)) && (errCnt < maxErr)) {
164 //possible good read
165 if (errCnt == 0){
166 bestStart = j;
167 bestErrCnt = errCnt;
168 break; //great read - finish
169 }
170 if (errCnt < bestErrCnt){ //set this as new best run
171 bestErrCnt = errCnt;
172 bestStart = j;
173 }
174 }
175 }
176 }
177 if (bestErrCnt < maxErr){
178 //best run is good enough set to best run and set overwrite BinStream
179 j = bestStart;
180 lastBit = bestStart - *clk;
181 bitnum = 0;
182 for (i = j; i < *BitLen; ++i) {
183 if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){
184 lastBit += *clk;
185 BinStream[bitnum] = *invert;
186 bitnum++;
187 } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){
188 //low found and we are expecting a bar
189 lastBit += *clk;
190 BinStream[bitnum] = 1 - *invert;
191 bitnum++;
192 } else {
193 //mid value found or no bar supposed to be here
194 if ((i-lastBit) > (*clk+tol)){
195 //should have hit a high or low based on clock!!
196 if (bitnum > 0){
197 BinStream[bitnum] = 77;
198 bitnum++;
199 }
200 lastBit += *clk;//skip over error
201 }
202 }
203 if (bitnum >= 400) break;
204 }
205 *BitLen = bitnum;
206 } else {
207 *invert = bestStart;
208 *clk = j;
209 return -1;
210 }
211 return bestErrCnt;
212 }
213
214 //by marshmellow
215 //take 10 and 01 and manchester decode
216 //run through 2 times and take least errCnt
217 int manrawdecode(uint8_t * bits, int *bitlen)
218 {
219 int bitnum = 0;
220 int errCnt = 0;
221 int bestErr = 1000;
222 int bestRun = 0;
223 int i = 1;
224 int j = 1;
225
226 for (; j < 3; ++j){
227 i = 1;
228 for ( i = i + j; i < *bitlen-2; i += 2){
229 if ( bits[i]==1 && (bits[i+1]==0)){
230 } else if ((bits[i]==0)&& bits[i+1]==1){
231 } else {
232 errCnt++;
233 }
234 if(bitnum > 300) break;
235 }
236 if (bestErr > errCnt){
237 bestErr = errCnt;
238 bestRun = j;
239 }
240 errCnt = 0;
241 }
242 errCnt = bestErr;
243 if (errCnt < 20){
244 j = bestRun;
245 i = 1;
246 for ( i = i+j; i < *bitlen-2; i += 2){
247 if ( bits[i] == 1 && bits[i + 1] == 0 ){
248 bits[bitnum++] = 0;
249 } else if ( bits[i] == 0 && bits[i + 1] == 1 ){
250 bits[bitnum++] = 1;
251 } else {
252 bits[bitnum++] = 77;
253 }
254 if ( bitnum > 300 ) break;
255 }
256 *bitlen = bitnum;
257 }
258 return errCnt;
259 }
260
261
262 //by marshmellow
263 //take 01 or 10 = 0 and 11 or 00 = 1
264 int BiphaseRawDecode(uint8_t * bits, int *bitlen, int offset)
265 {
266 uint8_t bitnum = 0;
267 uint32_t errCnt = 0;
268 uint32_t i = offset;
269
270 for (; i < *bitlen-2; i += 2 ){
271 if ( (bits[i]==1 && bits[i+1]==0)||
272 (bits[i]==0 && bits[i+1]==1)){
273 bits[bitnum++] = 1;
274 } else if ( (bits[i]==0 && bits[i+1]==0)||
275 (bits[i]==1 && bits[i+1]==1)){
276 bits[bitnum++] = 0;
277 } else {
278 bits[bitnum++] = 77;
279 errCnt++;
280 }
281 if ( bitnum > 250) break;
282 }
283 *bitlen = bitnum;
284 return errCnt;
285 }
286
287 //by marshmellow
288 //takes 2 arguments - clock and invert both as integers
289 //attempts to demodulate ask only
290 //prints binary found and saves in graphbuffer for further commands
291 int askrawdemod(uint8_t *BinStream, int *bitLen, int *clk, int *invert)
292 {
293 uint32_t i;
294 uint32_t initLoopMax = 200;
295 int high = 0, low = 128;
296 uint8_t BitStream[502] = {0x00};
297
298 *clk = DetectASKClock(BinStream, *bitLen, *clk); //clock default
299
300 if (*clk < 8) *clk = 64;
301 if (*clk < 32) *clk = 32;
302 if (*invert != 1) *invert = 0;
303
304 if (initLoopMax > *bitLen)
305 initLoopMax = *bitLen;
306
307 // Detect high and lows
308 for (i = 0; i < initLoopMax; ++i) //200 samples should be plenty to find high and low values
309 {
310 if (BinStream[i] > high)
311 high = BinStream[i];
312 else if (BinStream[i] < low)
313 low = BinStream[i];
314 }
315
316 //throw away static
317 if ((high < 158)){
318 return -2;
319 }
320
321 //25% fuzz in case highs and lows aren't clipped [marshmellow]
322 high = (int)(high * .75);
323 low = (int)(low+128 * .25);
324
325 int lastBit = 0; //set first clock check
326 uint32_t bitnum = 0; //output counter
327
328 uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave
329 if (*clk==32) tol = 1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely
330
331 uint32_t gLen = *bitLen;
332 if (gLen > 500) gLen = 500;
333
334 uint32_t j = 0;
335 uint8_t errCnt = 0;
336 uint32_t bestStart = *bitLen;
337 uint32_t bestErrCnt = (*bitLen / 1000);
338 uint32_t errCntLimit = bestErrCnt;
339 uint8_t midBit = 0;
340
341 //loop to find first wave that works
342 for (j = 0; j < gLen; ++j){
343
344 if ((BinStream[j] >= high)||(BinStream[j] <= low)){
345 lastBit = j - *clk;
346 //loop through to see if this start location works
347 for (i = j; i < *bitLen; ++i) {
348 if ((BinStream[i] >= high) && ((i-lastBit)>(*clk-tol))){
349 lastBit += *clk;
350 BitStream[bitnum] = *invert;
351 bitnum++;
352 midBit = 0;
353 } else if ((BinStream[i] <= low) && ((i-lastBit)>(*clk-tol))){
354 //low found and we are expecting a bar
355 lastBit += *clk;
356 BitStream[bitnum] = 1-*invert;
357 bitnum++;
358 midBit=0;
359 } else if ((BinStream[i]<=low) && (midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
360 //mid bar?
361 midBit = 1;
362 BitStream[bitnum] = 1 - *invert;
363 bitnum++;
364 } else if ((BinStream[i]>=high)&&(midBit==0) && ((i-lastBit)>((*clk/2)-tol))){
365 //mid bar?
366 midBit = 1;
367 BitStream[bitnum] = *invert;
368 bitnum++;
369 } else if ((i-lastBit)>((*clk/2)+tol)&&(midBit==0)){
370 //no mid bar found
371 midBit = 1;
372 BitStream[bitnum] = BitStream[bitnum-1];
373 bitnum++;
374 } else {
375 //mid value found or no bar supposed to be here
376
377 if (( i - lastBit) > ( *clk + tol)){
378 //should have hit a high or low based on clock!!
379
380 if (bitnum > 0){
381 BitStream[bitnum] = 77;
382 bitnum++;
383 }
384
385 errCnt++;
386 lastBit += *clk;//skip over until hit too many errors
387 if (errCnt > errCntLimit){ //allow 1 error for every 1000 samples else start over
388 errCnt = 0;
389 bitnum = 0;//start over
390 break;
391 }
392 }
393 }
394 if (bitnum > 500) break;
395 }
396 //we got more than 64 good bits and not all errors
397 //possible good read
398 if ((bitnum > (64 + errCnt)) && (errCnt < errCntLimit)) {
399
400 //great read - finish
401 if (errCnt == 0) break;
402
403 //if current run == bestErrCnt run (after exhausted testing) then finish
404 if (bestStart == j) break;
405
406 //set this as new best run
407 if (errCnt < bestErrCnt){
408 bestErrCnt = errCnt;
409 bestStart = j;
410 }
411 }
412 }
413 if (j >= gLen){ //exhausted test
414 //if there was a ok test go back to that one and re-run the best run (then dump after that run)
415 if (bestErrCnt < errCntLimit)
416 j = bestStart;
417 }
418 }
419 if (bitnum > 16){
420
421 for (i = 0; i < bitnum; ++i){
422 BinStream[i] = BitStream[i];
423 }
424 *bitLen = bitnum;
425 } else {
426 return -1;
427 }
428 return errCnt;
429 }
430 //translate wave to 11111100000 (1 for each short wave 0 for each long wave)
431 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow)
432 {
433 uint32_t last_transition = 0;
434 uint32_t idx = 1;
435 uint32_t maxVal = 0;
436
437 if (fchigh == 0) fchigh = 10;
438 if (fclow == 0) fclow = 8;
439
440 // we do care about the actual theshold value as sometimes near the center of the
441 // wave we may get static that changes direction of wave for one value
442 // if our value is too low it might affect the read. and if our tag or
443 // antenna is weak a setting too high might not see anything. [marshmellow]
444 if ( size < 100)
445 return 0;
446
447 // Find high from first 100 samples
448 for ( idx = 1; idx < 100; idx++ ){
449 if ( maxVal < dest[idx])
450 maxVal = dest[idx];
451 }
452
453 // set close to the top of the wave threshold with 25% margin for error
454 // less likely to get a false transition up there.
455 // (but have to be careful not to go too high and miss some short waves)
456 uint8_t threshold_value = (uint8_t)(maxVal * .75);
457
458 // sync to first lo-hi transition, and threshold
459 // Need to threshold first sample
460
461 dest[0] = (dest[0] < threshold_value) ? 0 : 1;
462
463 size_t numBits = 0;
464
465 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
466 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
467 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
468 for(idx = 1; idx < size; idx++) {
469
470 // threshold current value
471 dest[idx] = (dest[idx] < threshold_value) ? 0 : 1;
472
473 // Check for 0->1 transition
474 if (dest[idx-1] < dest[idx]) { // 0 -> 1 transition
475 if ( ( idx - last_transition ) <( fclow - 2 ) ) { //0-5 = garbage noise
476 //do nothing with extra garbage
477 } else if ((idx - last_transition) < ( fchigh - 1 )) { //6-8 = 8 waves
478 dest[numBits]=1;
479 } else { //9+ = 10 waves
480 dest[numBits]=0;
481 }
482 last_transition = idx;
483 numBits++;
484 }
485 }
486 //it returns the number of bytes, but each byte represents a bit: 1 or 0
487 return numBits;
488 }
489
490 uint32_t myround2(float f)
491 {
492 if (f >= 2000) return 2000;//something bad happened
493 return (uint32_t) (f + (float)0.5);
494 }
495
496 //translate 11111100000 to 10
497 size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t maxConsequtiveBits, uint8_t invert, uint8_t fchigh, uint8_t fclow )
498 {
499 uint8_t lastval = dest[0];
500 uint32_t idx = 0;
501 uint32_t n = 1;
502 size_t numBits = 0;
503
504 for( idx = 1; idx < size; idx++) {
505
506 if (dest[idx] == lastval) {
507 n++;
508 continue;
509 }
510 //if lastval was 1, we have a 1->0 crossing
511 if ( dest[idx-1] == 1 ) {
512 n = myround2( (float)( n + 1 ) / ((float)(rfLen)/(float)fclow));
513 } else { // 0->1 crossing
514 n = myround2( (float)( n + 1 ) / ((float)(rfLen-2)/(float)fchigh)); //-2 for fudge factor
515 }
516 if (n == 0) n = 1;
517
518 if(n < maxConsequtiveBits) //Consecutive
519 {
520 if(invert == 0){ //invert bits
521 memset(dest+numBits, dest[idx-1] , n);
522 }else{
523 memset(dest+numBits, dest[idx-1]^1 , n);
524 }
525 numBits += n;
526 }
527 n = 0;
528 lastval = dest[idx];
529 }//end for
530 return numBits;
531 }
532
533 //by marshmellow (from holiman's base)
534 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
535 int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow)
536 {
537 // FSK demodulator
538 size = fsk_wave_demod(dest, size, fchigh, fclow);
539 if ( size > 0 )
540 size = aggregate_bits(dest, size, rfLen, 192, invert, fchigh, fclow);
541 return size;
542 }
543
544 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
545 int HIDdemodFSK(uint8_t *dest, size_t size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
546 {
547 size_t idx = 0;
548 int numshifts = 0;
549
550 // FSK demodulator
551 size = fskdemod(dest, size, 50, 0, 10, 8);
552
553 // final loop, go over previously decoded manchester data and decode into usable tag ID
554 // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
555 uint8_t frame_marker_mask[] = {1,1,1,0,0,0};
556
557 uint8_t mask_len = sizeof frame_marker_mask / sizeof frame_marker_mask[0];
558
559 //one scan
560 while( idx + mask_len < size) {
561 // search for a start of frame marker
562 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
563 { // frame marker found
564 idx += mask_len;
565 while(dest[idx] != dest[idx+1] && idx < size-2)
566 {
567 // Keep going until next frame marker (or error)
568 // Shift in a bit. Start by shifting high registers
569 *hi2 = ( *hi2 << 1 ) | ( *hi >> 31 );
570 *hi = ( *hi << 1 ) | ( *lo >> 31 );
571 //Then, shift in a 0 or one into low
572 if (dest[idx] && !dest[idx+1]) // 1 0
573 *lo = ( *lo << 1 ) | 0;
574 else // 0 1
575 *lo = ( *lo << 1 ) | 1;
576 numshifts++;
577 idx += 2;
578 }
579 // Hopefully, we read a tag and hit upon the next frame marker
580 if(idx + mask_len < size)
581 {
582 if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
583 {
584 //good return
585 return idx;
586 }
587 }
588 // reset
589 *hi2 = *hi = *lo = 0;
590 numshifts = 0;
591 }else {
592 idx++;
593 }
594 }
595 return -1;
596 }
597
598 uint32_t bytebits_to_byte(uint8_t *src, int numbits)
599 {
600 //HACK: potential overflow in numbits is larger then uint32 bits.
601
602 uint32_t num = 0;
603 for(int i = 0 ; i < numbits ; ++i) {
604 num = (num << 1) | (*src);
605 src++;
606 }
607 return num;
608 }
609
610 int IOdemodFSK(uint8_t *dest, size_t size)
611 {
612 //make sure buffer has data
613 if (size < 100) return -1;
614
615 uint32_t idx = 0;
616 uint8_t testMax = 0;
617
618 //test samples are not just noise
619 for (; idx < 65; ++idx ){
620 if (testMax < dest[idx])
621 testMax = dest[idx];
622 }
623
624 //if not just noise
625 if (testMax < 170) return -2;
626
627 // FSK demodulator
628 size = fskdemod(dest, size, 64, 1, 10, 8); // RF/64 and invert
629
630 //did we get a good demod?
631 if (size < 65) return -3;
632
633 //Index map
634 //0 10 20 30 40 50 60
635 //| | | | | | |
636 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
637 //-----------------------------------------------------------------------------
638 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
639 //
640 //XSF(version)facility:codeone+codetwo
641 //Handle the data
642
643 uint8_t mask[] = {0,0,0,0,0,0,0,0,0,1};
644
645 for( idx = 0; idx < (size - 65); ++idx) {
646 if ( memcmp(dest + idx, mask, sizeof(mask))==0) {
647 //frame marker found
648 if (!dest[idx+8] &&
649 dest[idx+17] == 1 &&
650 dest[idx+26] == 1 &&
651 dest[idx+35] == 1 &&
652 dest[idx+44] == 1 &&
653 dest[idx+53] == 1){
654 //confirmed proper separator bits found
655 //return start position
656 return (int) idx;
657 }
658 }
659 }
660 return 0;
661 }
662
663 // by marshmellow
664 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
665 // maybe somehow adjust peak trimming value based on samples to fix?
666 int DetectASKClock(uint8_t dest[], size_t size, int clock)
667 {
668 int i = 0;
669 int clk[] = {16,32,40,50,64,100,128,256};
670 uint8_t clkLen = sizeof clk / sizeof clk[0];
671
672 //if we already have a valid clock quit
673 for (; i < clkLen; ++i)
674 if (clk[i] == clock)
675 return clock;
676
677 int peak = 0;
678 int low = 128;
679 int loopCnt = 256;
680 if (size < loopCnt)
681 loopCnt = size;
682
683 //get high and low peak
684 for ( i = 0; i < loopCnt; ++i ){
685 if(dest[i] > peak)
686 peak = dest[i];
687 if(dest[i] < low)
688 low = dest[i];
689 }
690
691 peak = (int)(peak * .75);
692 low = (int)(low+128 * .25);
693
694 int ii, cnt, bestErr, tol = 0;
695 int errCnt[clkLen];
696 memset(errCnt, 0x00, clkLen);
697
698 int tmpIndex, tmphigh, tmplow;
699
700 //test each valid clock from smallest to greatest to see which lines up
701 for( cnt = 0; cnt < clkLen; ++cnt ){
702
703 tol = (clk[cnt] == 32) ? 1 : 0;
704 bestErr = 1000;
705 tmpIndex = tmphigh = tmplow = 0;
706
707 //try lining up the peaks by moving starting point (try first 256)
708 for (ii=0; ii < loopCnt; ++ii){
709
710 // not a peak? continue
711 if ( (dest[ii] < peak) && (dest[ii] > low))
712 continue;
713
714 errCnt[cnt] = 0;
715
716 // now that we have the first one lined up test rest of wave array
717 for ( i = 0; i < ((int)(size / clk[cnt]) - 1); ++i){
718
719 tmpIndex = ii + (i * clk[cnt] );
720 tmplow = dest[ tmpIndex - tol];
721 tmphigh = dest[ tmpIndex + tol];
722
723 if ( dest[tmpIndex] >= peak || dest[tmpIndex] <= low ) {
724 }
725 else if ( tmplow >= peak || tmplow <= low){
726 }
727 else if ( tmphigh >= peak || tmphigh <= low){
728 }
729 else
730 errCnt[cnt]++; //error no peak detected
731 }
732
733 //if we found no errors this is correct one - return this clock
734 if ( errCnt[cnt] == 0 )
735 return clk[cnt];
736
737 if ( errCnt[cnt] < bestErr)
738 bestErr = errCnt[cnt];
739 }
740 // save the least error.
741 errCnt[cnt] = bestErr;
742 }
743 // find best clock which has lowest number of errors
744 int j = 0, bestIndex = 0;
745 for (; j < clkLen; ++j){
746 if ( errCnt[j] < errCnt[bestIndex] )
747 bestIndex = j;
748 }
749 return clk[bestIndex];
750 }
Impressum, Datenschutz