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