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