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