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