]> git.zerfleddert.de Git - proxmark3-svn/blob - common/lfdemod.c
439699c6266643bca9053527d0cd00f500d9abf3
[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 #include "lfdemod.h"
11
12 //un_comment to allow debug print calls when used not on device
13 void dummy(char *fmt, ...){}
14
15
16 #ifndef ON_DEVICE
17 # include "ui.h"
18 # include "cmdparser.h"
19 # include "cmddata.h"
20 # define prnt PrintAndLog
21 #else
22 uint8_t g_debugMode=0;
23 # define prnt dummy
24 #endif
25
26 //test samples are not just noise
27 uint8_t justNoise(uint8_t *bits, size_t size) {
28 #define THRESHOLD 123
29 uint8_t val = 1;
30 for(size_t idx=0; idx < size && val ;idx++)
31 val = bits[idx] < THRESHOLD;
32 return val;
33 }
34
35 //by marshmellow
36 //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
37 int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo)
38 {
39 *high=0;
40 *low=255;
41 // get high and low thresholds
42 for (size_t i=0; i < size; i++){
43 if (BitStream[i] > *high) *high = BitStream[i];
44 if (BitStream[i] < *low) *low = BitStream[i];
45 }
46 if (*high < 123) return -1; // just noise
47 *high = ((*high-128)*fuzzHi + 12800)/100;
48 *low = ((*low-128)*fuzzLo + 12800)/100;
49 return 1;
50 }
51
52 // by marshmellow
53 // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
54 // returns 1 if passed
55 uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType)
56 {
57 uint8_t ans = 0;
58 for (uint8_t i = 0; i < bitLen; i++){
59 ans ^= ((bits >> i) & 1);
60 }
61 if (g_debugMode) prnt("DEBUG: ans: %d, ptype: %d, bits: %08X",ans,pType,bits);
62 return (ans == pType);
63 }
64
65 //by marshmellow
66 // takes a array of binary values, start position, length of bits per parity (includes parity bit),
67 // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run)
68 size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen)
69 {
70 uint32_t parityWd = 0;
71 size_t j = 0, bitCnt = 0;
72 for (int word = 0; word < (bLen); word += pLen){
73 for (int bit=0; bit < pLen; bit++){
74 parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
75 BitStream[j++] = (BitStream[startIdx+word+bit]);
76 }
77 if (word+pLen > bLen) break;
78
79 j--; // overwrite parity with next data
80 // if parity fails then return 0
81 switch (pType) {
82 case 3: if (BitStream[j]==1) { return 0; } break; //should be 0 spacer bit
83 case 2: if (BitStream[j]==0) { return 0; } break; //should be 1 spacer bit
84 default: if (parityTest(parityWd, pLen, pType) == 0) { return 0; } break; //test parity
85 }
86 bitCnt += (pLen-1);
87 parityWd = 0;
88 }
89 // if we got here then all the parities passed
90 //return ID start index and size
91 return bitCnt;
92 }
93
94 // by marshmellow
95 // takes a array of binary values, length of bits per parity (includes parity bit),
96 // Parity Type (1 for odd; 0 for even; 2 Always 1's; 3 Always 0's), and binary Length (length to run)
97 // Make sure *dest is long enough to store original sourceLen + #_of_parities_to_be_added
98 size_t addParity(uint8_t *BitSource, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, uint8_t pType)
99 {
100 uint32_t parityWd = 0;
101 size_t j = 0, bitCnt = 0;
102 for (int word = 0; word < sourceLen; word+=pLen-1) {
103 for (int bit=0; bit < pLen-1; bit++){
104 parityWd = (parityWd << 1) | BitSource[word+bit];
105 dest[j++] = (BitSource[word+bit]);
106 }
107
108 // if parity fails then return 0
109 switch (pType) {
110 case 3: dest[j++]=0; break; // marker bit which should be a 0
111 case 2: dest[j++]=1; break; // marker bit which should be a 1
112 default:
113 dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1;
114 break;
115 }
116 bitCnt += pLen;
117 parityWd = 0;
118 }
119 // if we got here then all the parities passed
120 //return ID start index and size
121 return bitCnt;
122 }
123
124 uint32_t bytebits_to_byte(uint8_t *src, size_t numbits)
125 {
126 uint32_t num = 0;
127 for(int i = 0 ; i < numbits ; i++) {
128 num = (num << 1) | (*src);
129 src++;
130 }
131 return num;
132 }
133
134 //least significant bit first
135 uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits)
136 {
137 uint32_t num = 0;
138 for(int i = 0 ; i < numbits ; i++) {
139 num = (num << 1) | *(src + (numbits-(i+1)));
140 }
141 return num;
142 }
143
144 //by marshmellow
145 // search for given preamble in given BitStream and return success=1 or fail=0 and startIndex (where it was found)
146 bool preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx){
147 return preambleSearchEx(BitStream, preamble, pLen, size, startIdx, false);
148 }
149 //by marshmellow
150 //search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
151 // param @findone: look for a repeating preamble or only the first.
152 // em4x05/4x69 only sends preamble once, so look for it once in the first pLen bits
153 bool preambleSearchEx(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx, bool findone)
154 {
155 // Sanity check. If preamble length is bigger than bitstream length.
156 if ( *size <= pLen ) return false;
157
158 uint8_t foundCnt = 0;
159 for (int idx = 0; idx < *size - pLen; idx++){
160 if (memcmp(BitStream+idx, preamble, pLen) == 0){
161 if (g_debugMode) prnt("DEBUG: preamble found at %i", idx);
162 //first index found
163 foundCnt++;
164 if (foundCnt == 1){
165 *startIdx = idx;
166 if (findone) return true;
167 }
168 if (foundCnt == 2){
169 *size = idx - *startIdx;
170 return true;
171 }
172 }
173 }
174 return false;
175 }
176
177 // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.
178 size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_t expWaveSize) {
179 size_t i = 0;
180 size_t waveSizeCnt = 0;
181 uint8_t thresholdCnt = 0;
182 bool isAboveThreshold = dest[i++] >= threshold_value;
183 for (; i < size-20; i++ ) {
184 if(dest[i] < threshold_value && isAboveThreshold) {
185 thresholdCnt++;
186 if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
187 isAboveThreshold = false;
188 waveSizeCnt = 0;
189 } else if (dest[i] >= threshold_value && !isAboveThreshold) {
190 thresholdCnt++;
191 if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
192 isAboveThreshold = true;
193 waveSizeCnt = 0;
194 } else {
195 waveSizeCnt++;
196 }
197 if (thresholdCnt > 10) break;
198 }
199 if (g_debugMode == 2) prnt("DEBUG: threshold Count reached at %u, count: %u",i, thresholdCnt);
200 return i;
201 }
202
203 //by marshmellow
204 //takes 1s and 0s and searches for EM410x format - output EM ID
205 // actually, no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
206 int Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo)
207 {
208 // sanity check
209 if (BitStream[1] > 1) return -1;
210
211 uint8_t fmtlen;
212 *startIdx = 0;
213
214 // preamble 0111111111
215 // include 0 in front to help get start pos
216 uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1};
217 if (!preambleSearch(BitStream, preamble, sizeof(preamble), size, startIdx))
218 return -2;
219 if (*size < 64) return -3;
220
221 fmtlen = (*size == 110) ? 22 : 10;
222
223 //skip last 4bit parity row for simplicity
224 *size = removeParity(BitStream, *startIdx + sizeof(preamble), 5, 0, fmtlen * 5);
225
226 switch (*size) {
227 case 40: {
228 // std em410x format
229 *hi = 0;
230 *lo = ((uint64_t)(bytebits_to_byte(BitStream, 8)) << 32) | (bytebits_to_byte(BitStream + 8, 32));
231 break;
232 }
233 case 88: {
234 // long em format
235 *hi = (bytebits_to_byte(BitStream, 24));
236 *lo = ((uint64_t)(bytebits_to_byte(BitStream + 24, 32)) << 32) | (bytebits_to_byte(BitStream + 24 + 32, 32));
237 break;
238 }
239 default: return -4;
240 }
241 return 1;
242 }
243
244 //by marshmellow
245 //demodulates strong heavily clipped samples
246 int cleanAskRawDemod(uint8_t *BinStream, size_t *size, int clk, int invert, int high, int low)
247 {
248 size_t bitCnt=0, smplCnt=0, errCnt=0;
249 uint8_t waveHigh = 0;
250 for (size_t i=0; i < *size; i++){
251 if (BinStream[i] >= high && waveHigh){
252 smplCnt++;
253 } else if (BinStream[i] <= low && !waveHigh){
254 smplCnt++;
255 } else { //transition
256 if ((BinStream[i] >= high && !waveHigh) || (BinStream[i] <= low && waveHigh)){
257
258 if (smplCnt > clk-(clk/4)-1) { //full clock
259 if (smplCnt > clk + (clk/4)+1) { //too many samples
260 errCnt++;
261 if (g_debugMode==2) prnt("DEBUG ASK: Modulation Error at: %u", i);
262 BinStream[bitCnt++] = 7;
263 } else if (waveHigh) {
264 BinStream[bitCnt++] = invert;
265 BinStream[bitCnt++] = invert;
266 } else if (!waveHigh) {
267 BinStream[bitCnt++] = invert ^ 1;
268 BinStream[bitCnt++] = invert ^ 1;
269 }
270 waveHigh ^= 1;
271 smplCnt = 0;
272 } else if (smplCnt > (clk/2) - (clk/4)-1) {
273 if (waveHigh) {
274 BinStream[bitCnt++] = invert;
275 } else if (!waveHigh) {
276 BinStream[bitCnt++] = invert ^ 1;
277 }
278 waveHigh ^= 1;
279 smplCnt = 0;
280 } else if (!bitCnt) {
281 //first bit
282 waveHigh = (BinStream[i] >= high);
283 smplCnt = 1;
284 } else {
285 smplCnt++;
286 //transition bit oops
287 }
288 } else { //haven't hit new high or new low yet
289 smplCnt++;
290 }
291 }
292 }
293 *size = bitCnt;
294 return errCnt;
295 }
296
297 //by marshmellow
298 void askAmp(uint8_t *BitStream, size_t size)
299 {
300 uint8_t last = 128;
301 for(size_t i = 1; i < size; ++i){
302 if (BitStream[i]-BitStream[i-1] >= 30) //large jump up
303 last = 255;
304 else if(BitStream[i-1] - BitStream[i] >= 20) //large jump down
305 last = 0;
306
307 BitStream[i] = last;
308 }
309 }
310
311 //by marshmellow
312 //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester
313 int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType)
314 {
315 if (*size==0) return -1;
316 int start = DetectASKClock(BinStream, *size, clk, maxErr); //clock default
317
318 if (*clk==0 || start < 0) return -3;
319 if (*invert != 1) *invert = 0;
320 if (amp==1) askAmp(BinStream, *size);
321 if (g_debugMode==2) prnt("DEBUG ASK: clk %d, beststart %d, amp %d", *clk, start, amp);
322
323 uint8_t initLoopMax = 255;
324 if (initLoopMax > *size) initLoopMax = *size;
325 // Detect high and lows
326 //25% clip in case highs and lows aren't clipped [marshmellow]
327 int high, low;
328 if (getHiLo(BinStream, initLoopMax, &high, &low, 75, 75) < 1)
329 return -2; //just noise
330
331 size_t errCnt = 0;
332 // if clean clipped waves detected run alternate demod
333 if (DetectCleanAskWave(BinStream, *size, high, low)) {
334 if (g_debugMode==2) prnt("DEBUG ASK: Clean Wave Detected - using clean wave demod");
335 errCnt = cleanAskRawDemod(BinStream, size, *clk, *invert, high, low);
336 if (askType) //askman
337 return manrawdecode(BinStream, size, 0);
338 //askraw
339 return errCnt;
340 }
341 if (g_debugMode==2) prnt("DEBUG ASK: Weak Wave Detected - using weak wave demod");
342
343 int lastBit; //set first clock check - can go negative
344 size_t i, bitnum = 0; //output counter
345 uint8_t midBit = 0;
346 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
347 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
348 size_t MaxBits = 3072; //max bits to collect
349 lastBit = start - *clk;
350
351 for (i = start; i < *size; ++i) {
352 if (i-lastBit >= *clk-tol){
353 if (BinStream[i] >= high) {
354 BinStream[bitnum++] = *invert;
355 } else if (BinStream[i] <= low) {
356 BinStream[bitnum++] = *invert ^ 1;
357 } else if (i-lastBit >= *clk+tol) {
358 if (bitnum > 0) {
359 if (g_debugMode==2) prnt("DEBUG ASK: Modulation Error at: %u", i);
360 BinStream[bitnum++]=7;
361 errCnt++;
362 }
363 } else { //in tolerance - looking for peak
364 continue;
365 }
366 midBit = 0;
367 lastBit += *clk;
368 } else if (i-lastBit >= (*clk/2-tol) && !midBit && !askType){
369 if (BinStream[i] >= high) {
370 BinStream[bitnum++] = *invert;
371 } else if (BinStream[i] <= low) {
372 BinStream[bitnum++] = *invert ^ 1;
373 } else if (i-lastBit >= *clk/2+tol) {
374 BinStream[bitnum] = BinStream[bitnum-1];
375 bitnum++;
376 } else { //in tolerance - looking for peak
377 continue;
378 }
379 midBit = 1;
380 }
381 if (bitnum >= MaxBits) break;
382 }
383 *size = bitnum;
384 return errCnt;
385 }
386 //by marshmellow
387 //take 10 and 01 and manchester decode
388 //run through 2 times and take least errCnt
389 int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert){
390 int errCnt = 0, bestErr = 1000;
391 uint16_t bitnum = 0, MaxBits = 512, bestRun = 0;
392 size_t i, k;
393 if (*size < 16) return -1;
394 //find correct start position [alignment]
395 for (k=0; k < 2; ++k){
396 for (i=k; i<*size-3; i += 2)
397 if (BitStream[i] == BitStream[i+1])
398 errCnt++;
399
400 if (bestErr > errCnt){
401 bestErr = errCnt;
402 bestRun = k;
403 }
404 errCnt=0;
405 }
406 //decode
407 for (i=bestRun; i < *size-3; i += 2){
408 if (BitStream[i] == 1 && (BitStream[i+1] == 0)){
409 BitStream[bitnum++] = invert;
410 } else if ((BitStream[i] == 0) && BitStream[i+1] == 1){
411 BitStream[bitnum++] = invert^1;
412 } else {
413 BitStream[bitnum++] = 7;
414 }
415 if (bitnum>MaxBits) break;
416 }
417 *size=bitnum;
418 return bestErr;
419 }
420
421 uint32_t manchesterEncode2Bytes(uint16_t datain) {
422 uint32_t output = 0;
423 uint8_t curBit = 0;
424 for (uint8_t i=0; i<16; i++) {
425 curBit = (datain >> (15-i) & 1);
426 output |= (1<<(((15-i)*2)+curBit));
427 }
428 return output;
429 }
430
431 //by marshmellow
432 //encode binary data into binary manchester
433 int ManchesterEncode(uint8_t *BitStream, size_t size)
434 {
435 size_t modIdx=20000, i=0;
436 if (size>modIdx) return -1;
437 for (size_t idx=0; idx < size; idx++){
438 BitStream[idx+modIdx++] = BitStream[idx];
439 BitStream[idx+modIdx++] = BitStream[idx]^1;
440 }
441 for (; i<(size*2); i++){
442 BitStream[i] = BitStream[i+20000];
443 }
444 return i;
445 }
446
447 //by marshmellow
448 //take 01 or 10 = 1 and 11 or 00 = 0
449 //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
450 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding
451 int BiphaseRawDecode(uint8_t *BitStream, size_t *size, int offset, int invert)
452 {
453 uint16_t bitnum = 0;
454 uint16_t errCnt = 0;
455 size_t i = offset;
456 uint16_t MaxBits=512;
457 //if not enough samples - error
458 if (*size < 51) return -1;
459 //check for phase change faults - skip one sample if faulty
460 uint8_t offsetA = 1, offsetB = 1;
461 for (; i<48; i+=2){
462 if (BitStream[i+1]==BitStream[i+2]) offsetA=0;
463 if (BitStream[i+2]==BitStream[i+3]) offsetB=0;
464 }
465 if (!offsetA && offsetB) offset++;
466 for (i=offset; i<*size-3; i+=2){
467 //check for phase error
468 if (BitStream[i+1]==BitStream[i+2]) {
469 BitStream[bitnum++]=7;
470 errCnt++;
471 }
472 if((BitStream[i]==1 && BitStream[i+1]==0) || (BitStream[i]==0 && BitStream[i+1]==1)){
473 BitStream[bitnum++]=1^invert;
474 } else if((BitStream[i]==0 && BitStream[i+1]==0) || (BitStream[i]==1 && BitStream[i+1]==1)){
475 BitStream[bitnum++]=invert;
476 } else {
477 BitStream[bitnum++]=7;
478 errCnt++;
479 }
480 if(bitnum>MaxBits) break;
481 }
482 *size=bitnum;
483 return errCnt;
484 }
485
486 // by marshmellow
487 // demod gProxIIDemod
488 // error returns as -x
489 // success returns start position in BitStream
490 // BitStream must contain previously askrawdemod and biphasedemoded data
491 int gProxII_Demod(uint8_t BitStream[], size_t *size)
492 {
493 size_t startIdx=0;
494 uint8_t preamble[] = {1,1,1,1,1,0};
495
496 if (!preambleSearch(BitStream, preamble, sizeof(preamble), size, &startIdx))
497 return -3; //preamble not found
498
499 if (*size != 96) return -2; //should have found 96 bits
500
501 //check first 6 spacer bits to verify format
502 if (!BitStream[startIdx+5] && !BitStream[startIdx+10] && !BitStream[startIdx+15] && !BitStream[startIdx+20] && !BitStream[startIdx+25] && !BitStream[startIdx+30]){
503 //confirmed proper separator bits found
504 //return start position
505 return (int) startIdx;
506 }
507 return -5; //spacer bits not found - not a valid gproxII
508 }
509
510 //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq])
511 size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow)
512 {
513 size_t last_transition = 0;
514 size_t idx = 1;
515 if (fchigh==0) fchigh=10;
516 if (fclow==0) fclow=8;
517 //set the threshold close to 0 (graph) or 128 std to avoid static
518 uint8_t threshold_value = 123;
519 size_t preLastSample = 0;
520 size_t LastSample = 0;
521 size_t currSample = 0;
522 if ( size < 1024 ) return 0; // not enough samples
523
524 //find start of modulating data in trace
525 idx = findModStart(dest, size, threshold_value, fchigh);
526
527 // Need to threshold first sample
528 if(dest[idx] < threshold_value) dest[0] = 0;
529 else dest[0] = 1;
530 idx++;
531
532 size_t numBits = 0;
533 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
534 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with anywhere
535 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
536 // (could also be fc/5 && fc/7 for fsk1 = 4-9)
537 for(; idx < size-20; idx++) {
538 // threshold current value
539
540 if (dest[idx] < threshold_value) dest[idx] = 0;
541 else dest[idx] = 1;
542
543 // Check for 0->1 transition
544 if (dest[idx-1] < dest[idx]) {
545 preLastSample = LastSample;
546 LastSample = currSample;
547 currSample = idx-last_transition;
548 if (currSample < (fclow-2)){ //0-5 = garbage noise (or 0-3)
549 //do nothing with extra garbage
550 } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves (or 3-6 = 5)
551 //correct previous 9 wave surrounded by 8 waves (or 6 surrounded by 5)
552 if (LastSample > (fchigh-2) && (preLastSample < (fchigh-1))){
553 dest[numBits-1]=1;
554 }
555 dest[numBits++]=1;
556
557 } else if (currSample > (fchigh+1) && numBits < 3) { //12 + and first two bit = unusable garbage
558 //do nothing with beginning garbage and reset.. should be rare..
559 numBits = 0;
560 } else if (currSample == (fclow+1) && LastSample == (fclow-1)) { // had a 7 then a 9 should be two 8's (or 4 then a 6 should be two 5's)
561 dest[numBits++]=1;
562 } else { //9+ = 10 sample waves (or 6+ = 7)
563 dest[numBits++]=0;
564 }
565 last_transition = idx;
566 }
567 }
568 return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
569 }
570
571 //translate 11111100000 to 10
572 //rfLen = clock, fchigh = larger field clock, fclow = smaller field clock
573 size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen,
574 uint8_t invert, uint8_t fchigh, uint8_t fclow)
575 {
576 uint8_t lastval=dest[0];
577 size_t idx=0;
578 size_t numBits=0;
579 uint32_t n=1;
580 for( idx=1; idx < size; idx++) {
581 n++;
582 if (dest[idx]==lastval) continue; //skip until we hit a transition
583
584 //find out how many bits (n) we collected
585 //if lastval was 1, we have a 1->0 crossing
586 if (dest[idx-1]==1) {
587 n = (n * fclow + rfLen/2) / rfLen;
588 } else {// 0->1 crossing
589 n = (n * fchigh + rfLen/2) / rfLen;
590 }
591 if (n == 0) n = 1;
592
593 //add to our destination the bits we collected
594 memset(dest+numBits, dest[idx-1]^invert , n);
595 numBits += n;
596 n=0;
597 lastval=dest[idx];
598 }//end for
599 // if valid extra bits at the end were all the same frequency - add them in
600 if (n > rfLen/fchigh) {
601 if (dest[idx-2]==1) {
602 n = (n * fclow + rfLen/2) / rfLen;
603 } else {
604 n = (n * fchigh + rfLen/2) / rfLen;
605 }
606 memset(dest+numBits, dest[idx-1]^invert , n);
607 numBits += n;
608 }
609 return numBits;
610 }
611
612 //by marshmellow (from holiman's base)
613 // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod)
614 int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow)
615 {
616 // FSK demodulator
617 size = fsk_wave_demod(dest, size, fchigh, fclow);
618 size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow);
619 return size;
620 }
621
622 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
623 int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
624 {
625 if (justNoise(dest, *size)) return -1;
626
627 size_t numStart=0, size2 = *size, startIdx=0;
628 // FSK demodulator
629 *size = fskdemod(dest, size2,50,1,10,8); //fsk2a
630 if (*size < 96*2) return -2;
631 // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
632 uint8_t preamble[] = {0,0,0,1,1,1,0,1};
633 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
634 return -3; //preamble not found
635
636 numStart = startIdx + sizeof(preamble);
637 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
638 for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
639 if (dest[idx] == dest[idx+1]){
640 return -4; //not manchester data
641 }
642 *hi2 = (*hi2<<1)|(*hi>>31);
643 *hi = (*hi<<1)|(*lo>>31);
644 //Then, shift in a 0 or one into low
645 *lo <<= 1;
646 if (dest[idx] && !dest[idx+1]) // 1 0
647 *lo |= 1;
648 else // 0 1
649 *lo |= 0;
650 }
651 return (int)startIdx;
652 }
653
654 // loop to get raw paradox waveform then FSK demodulate the TAG ID from it
655 int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo)
656 {
657 if (justNoise(dest, *size)) return -1;
658
659 size_t numStart=0, size2 = *size, startIdx=0;
660 // FSK demodulator
661 *size = fskdemod(dest, size2,50,1,10,8); //fsk2a
662 if (*size < 96) return -2;
663
664 // 00001111 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
665 uint8_t preamble[] = {0,0,0,0,1,1,1,1};
666 if (preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
667 return -3; //preamble not found
668
669 numStart = startIdx + sizeof(preamble);
670 // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
671 for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
672 if (dest[idx] == dest[idx+1])
673 return -4; //not manchester data
674 *hi2 = (*hi2<<1)|(*hi>>31);
675 *hi = (*hi<<1)|(*lo>>31);
676 //Then, shift in a 0 or one into low
677 if (dest[idx] && !dest[idx+1]) // 1 0
678 *lo=(*lo<<1)|1;
679 else // 0 1
680 *lo=(*lo<<1)|0;
681 }
682 return (int)startIdx;
683 }
684
685 int IOdemodFSK(uint8_t *dest, size_t size)
686 {
687 if (justNoise(dest, size)) return -1;
688 //make sure buffer has data
689 if (size < 66*64) return -2;
690 // FSK demodulator
691 size = fskdemod(dest, size, 64, 1, 10, 8); // FSK2a RF/64
692 if (size < 65) return -3; //did we get a good demod?
693 //Index map
694 //0 10 20 30 40 50 60
695 //| | | | | | |
696 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
697 //-----------------------------------------------------------------------------
698 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
699 //
700 //XSF(version)facility:codeone+codetwo
701 //Handle the data
702 size_t startIdx = 0;
703 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,1};
704 if (! preambleSearch(dest, preamble, sizeof(preamble), &size, &startIdx))
705 return -4; //preamble not found
706
707 if (!dest[startIdx+8] && dest[startIdx+17]==1 && dest[startIdx+26]==1 && dest[startIdx+35]==1 && dest[startIdx+44]==1 && dest[startIdx+53]==1){
708 //confirmed proper separator bits found
709 //return start position
710 return (int) startIdx;
711 }
712 return -5;
713 }
714
715 // by marshmellow
716 // find viking preamble 0xF200 in already demoded data
717 int VikingDemod_AM(uint8_t *dest, size_t *size) {
718 //make sure buffer has data
719 if (*size < 64*2) return -2;
720 size_t startIdx = 0;
721 uint8_t preamble[] = {1,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
722 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
723 return -4; //preamble not found
724
725 uint32_t checkCalc = bytebits_to_byte(dest+startIdx,8) ^
726 bytebits_to_byte(dest+startIdx+8,8) ^
727 bytebits_to_byte(dest+startIdx+16,8) ^
728 bytebits_to_byte(dest+startIdx+24,8) ^
729 bytebits_to_byte(dest+startIdx+32,8) ^
730 bytebits_to_byte(dest+startIdx+40,8) ^
731 bytebits_to_byte(dest+startIdx+48,8) ^
732 bytebits_to_byte(dest+startIdx+56,8);
733 if ( checkCalc != 0xA8 ) return -5;
734 if (*size != 64) return -6;
735 //return start position
736 return (int)startIdx;
737 }
738
739 // by iceman
740 // find Visa2000 preamble in already demoded data
741 int Visa2kDemod_AM(uint8_t *dest, size_t *size) {
742 if (*size < 96) return -1; //make sure buffer has data
743 size_t startIdx = 0;
744 uint8_t preamble[] = {0,1,0,1,0,1,1,0,0,1,0,0,1,0,0,1,0,1,0,1,0,0,1,1,0,0,1,1,0,0,1,0};
745 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
746 return -2; //preamble not found
747 if (*size != 96) return -3; //wrong demoded size
748 //return start position
749 return (int)startIdx;
750 }
751 // by iceman
752 // find Noralsy preamble in already demoded data
753 int NoralsyDemod_AM(uint8_t *dest, size_t *size) {
754 if (*size < 96) return -1; //make sure buffer has data
755 size_t startIdx = 0;
756 uint8_t preamble[] = {1,0,1,1,1,0,1,1,0,0,0,0};
757 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
758 return -2; //preamble not found
759 if (*size != 96) return -3; //wrong demoded size
760 //return start position
761 return (int)startIdx;
762 }
763 // find presco preamble 0x10D in already demoded data
764 int PrescoDemod(uint8_t *dest, size_t *size) {
765 if (*size < 128*2) return -1; //make sure buffer has data
766 size_t startIdx = 0;
767 uint8_t preamble[] = {0,0,0,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0};
768 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
769 return -2; //preamble not found
770 if (*size != 128) return -3; //wrong demoded size
771 //return start position
772 return (int)startIdx;
773 }
774
775 // Ask/Biphase Demod then try to locate an ISO 11784/85 ID
776 // BitStream must contain previously askrawdemod and biphasedemoded data
777 int FDXBdemodBI(uint8_t *dest, size_t *size) {
778 if (*size < 128*2) return -1; //make sure buffer has enough data
779 size_t startIdx = 0;
780 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,1};
781 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
782 return -2; //preamble not found
783 if (*size != 128) return -3; //wrong demoded size
784 //return start position
785 return (int)startIdx;
786 }
787
788 // ASK/Diphase fc/64 (inverted Biphase)
789 // Note: this i s not a demod, this is only a detection
790 // the parameter *dest needs to be demoded before call
791 // 0xFFFF preamble, 64bits
792 int JablotronDemod(uint8_t *dest, size_t *size){
793 if (*size < 64*2) return -1; //make sure buffer has enough data
794 size_t startIdx = 0;
795 uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0};
796 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
797 return -2; //preamble not found
798 if (*size != 64) return -3; // wrong demoded size
799
800 uint8_t checkchksum = 0;
801 for (int i=16; i < 56; i += 8) {
802 checkchksum += bytebits_to_byte(dest+startIdx+i,8);
803 }
804 checkchksum ^= 0x3A;
805 uint8_t crc = bytebits_to_byte(dest+startIdx+56, 8);
806 if ( checkchksum != crc ) return -5;
807 return (int)startIdx;
808 }
809
810 // by marshmellow
811 // FSK Demod then try to locate an AWID ID
812 int AWIDdemodFSK(uint8_t *dest, size_t *size)
813 {
814 //make sure buffer has enough data
815 if (*size < 96*50) return -1;
816
817 if (justNoise(dest, *size)) return -2;
818
819 // FSK demodulator
820 *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
821 if (*size < 96) return -3; //did we get a good demod?
822
823 uint8_t preamble[] = {0,0,0,0,0,0,0,1};
824 size_t startIdx = 0;
825 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
826 return -4; //preamble not found
827 if (*size != 96) return -5;
828 return (int)startIdx;
829 }
830
831 // by marshmellow
832 // FSK Demod then try to locate a Farpointe Data (pyramid) ID
833 int PyramiddemodFSK(uint8_t *dest, size_t *size)
834 {
835 //make sure buffer has data
836 if (*size < 128*50) return -5;
837
838 //test samples are not just noise
839 if (justNoise(dest, *size)) return -1;
840
841 // FSK demodulator
842 *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
843 if (*size < 128) return -2; //did we get a good demod?
844 size_t startIdx = 0;
845 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1};
846 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
847 return -4; //preamble not found
848 if (*size != 128) return -3;
849 return (int)startIdx;
850 }
851
852 // find nedap preamble in already demoded data
853 int NedapDemod(uint8_t *dest, size_t *size) {
854 //make sure buffer has data
855 if (*size < 128) return -3;
856
857 size_t startIdx = 0;
858 //uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,0,0,0,1};
859 uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,0};
860 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
861 return -4; //preamble not found
862 return (int) startIdx;
863 }
864
865 // Find IDTEC PSK1, RF Preamble == 0x4944544B, Demodsize 64bits
866 // by iceman
867 int IdteckDemodPSK(uint8_t *dest, size_t *size) {
868 //make sure buffer has data
869 if (*size < 64*2) return -1;
870 size_t startIdx = 0;
871 uint8_t preamble[] = {0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,0,0,1,0,0,1,0,1,1};
872 if (!preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx))
873 return -2; //preamble not found
874 if (*size != 64) return -3; // wrong demoded size
875 return (int) startIdx;
876 }
877
878 // by marshmellow
879 // to detect a wave that has heavily clipped (clean) samples
880 uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low)
881 {
882 bool allArePeaks = true;
883 uint16_t cntPeaks=0;
884 size_t loopEnd = 512+160;
885 if (loopEnd > size) loopEnd = size;
886 for (size_t i=160; i<loopEnd; i++){
887 if (dest[i]>low && dest[i]<high)
888 allArePeaks = false;
889 else
890 cntPeaks++;
891 }
892 if (!allArePeaks){
893 if (cntPeaks > 300) return true;
894 }
895 return allArePeaks;
896 }
897 // by marshmellow
898 // to help detect clocks on heavily clipped samples
899 // based on count of low to low
900 int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low)
901 {
902 uint8_t fndClk[] = {8,16,32,40,50,64,128};
903 size_t startwave;
904 size_t i = 100;
905 size_t minClk = 255;
906 // get to first full low to prime loop and skip incomplete first pulse
907 while ((dest[i] < high) && (i < size))
908 ++i;
909 while ((dest[i] > low) && (i < size))
910 ++i;
911
912 // loop through all samples
913 while (i < size) {
914 // measure from low to low
915 while ((dest[i] > low) && (i < size))
916 ++i;
917 startwave= i;
918 while ((dest[i] < high) && (i < size))
919 ++i;
920 while ((dest[i] > low) && (i < size))
921 ++i;
922 //get minimum measured distance
923 if (i-startwave < minClk && i < size)
924 minClk = i - startwave;
925 }
926 // set clock
927 if (g_debugMode==2) prnt("DEBUG ASK: detectstrongASKclk smallest wave: %d",minClk);
928 for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) {
929 if (minClk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && minClk <= fndClk[clkCnt]+1)
930 return fndClk[clkCnt];
931 }
932 return 0;
933 }
934
935 // by marshmellow
936 // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping)
937 // maybe somehow adjust peak trimming value based on samples to fix?
938 // return start index of best starting position for that clock and return clock (by reference)
939 int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr)
940 {
941 size_t i=1;
942 uint8_t clk[] = {255,8,16,32,40,50,64,100,128,255};
943 uint8_t clkEnd = 9;
944 uint8_t loopCnt = 255; //don't need to loop through entire array...
945 if (size <= loopCnt+60) return -1; //not enough samples
946 size -= 60; //sometimes there is a strange end wave - filter out this....
947 //if we already have a valid clock
948 uint8_t clockFnd=0;
949 for (;i<clkEnd;++i)
950 if (clk[i] == *clock) clockFnd = i;
951 //clock found but continue to find best startpos
952
953 //get high and low peak
954 int peak, low;
955 if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return -1;
956
957 //test for large clean peaks
958 if (!clockFnd){
959 if (DetectCleanAskWave(dest, size, peak, low)==1){
960 int ans = DetectStrongAskClock(dest, size, peak, low);
961 if (g_debugMode==2) prnt("DEBUG ASK: detectaskclk Clean Ask Wave Detected: clk %d",ans);
962 for (i=clkEnd-1; i>0; i--){
963 if (clk[i] == ans) {
964 *clock = ans;
965 //clockFnd = i;
966 return 0; // for strong waves i don't use the 'best start position' yet...
967 //break; //clock found but continue to find best startpos [not yet]
968 }
969 }
970 }
971 }
972 uint8_t ii;
973 uint8_t clkCnt, tol = 0;
974 uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
975 uint8_t bestStart[]={0,0,0,0,0,0,0,0,0};
976 size_t errCnt = 0;
977 size_t arrLoc, loopEnd;
978
979 if (clockFnd>0) {
980 clkCnt = clockFnd;
981 clkEnd = clockFnd+1;
982 } else {
983 clkCnt=1;
984 }
985
986 //test each valid clock from smallest to greatest to see which lines up
987 for(; clkCnt < clkEnd; clkCnt++) {
988 if (clk[clkCnt] <= 32) {
989 tol=1;
990 } else {
991 tol=0;
992 }
993 //if no errors allowed - keep start within the first clock
994 if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt]<128)
995 loopCnt = clk[clkCnt] * 2;
996
997 bestErr[clkCnt] = 1000;
998
999 //try lining up the peaks by moving starting point (try first few clocks)
1000 for (ii=0; ii < loopCnt; ii++){
1001 if (dest[ii] < peak && dest[ii] > low) continue;
1002
1003 errCnt = 0;
1004 // now that we have the first one lined up test rest of wave array
1005 loopEnd = ((size-ii-tol) / clk[clkCnt]) - 1;
1006 for (i=0; i < loopEnd; ++i){
1007 arrLoc = ii + (i * clk[clkCnt]);
1008 if (dest[arrLoc] >= peak || dest[arrLoc] <= low){
1009 }else if (dest[arrLoc-tol] >= peak || dest[arrLoc-tol] <= low){
1010 }else if (dest[arrLoc+tol] >= peak || dest[arrLoc+tol] <= low){
1011 }else{ //error no peak detected
1012 errCnt++;
1013 }
1014 }
1015 //if we found no errors then we can stop here and a low clock (common clocks)
1016 // this is correct one - return this clock
1017 if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d", clk[clkCnt], errCnt, ii, i);
1018 if (errCnt==0 && clkCnt<7) {
1019 if (!clockFnd) *clock = clk[clkCnt];
1020 return ii;
1021 }
1022 //if we found errors see if it is lowest so far and save it as best run
1023 if (errCnt < bestErr[clkCnt]) {
1024 bestErr[clkCnt] = errCnt;
1025 bestStart[clkCnt] = ii;
1026 }
1027 }
1028 }
1029 uint8_t k;
1030 uint8_t best = 0;
1031 for (k=1; k < clkEnd; ++k){
1032 if (bestErr[k] < bestErr[best]){
1033 if (bestErr[k] == 0) bestErr[k]=1;
1034 // current best bit to error ratio vs new bit to error ratio
1035 if ( (size/clk[best])/bestErr[best] < (size/clk[k])/bestErr[k] ){
1036 best = k;
1037 }
1038 }
1039 if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d", clk[k], bestErr[k], clk[best], bestStart[best]);
1040 }
1041 if (!clockFnd) *clock = clk[best];
1042 return bestStart[best];
1043 }
1044
1045 //by marshmellow
1046 //detect psk clock by reading each phase shift
1047 // a phase shift is determined by measuring the sample length of each wave
1048 int DetectPSKClock(uint8_t dest[], size_t size, int clock)
1049 {
1050 uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
1051 uint16_t loopCnt = 4096; //don't need to loop through entire array...
1052 if (size == 0) return 0;
1053 if (size<loopCnt) loopCnt = size-20;
1054
1055 //if we already have a valid clock quit
1056 size_t i=1;
1057 for (; i < 8; ++i)
1058 if (clk[i] == clock) return clock;
1059
1060 size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
1061 uint8_t clkCnt, fc=0, fullWaveLen=0, tol=1;
1062 uint16_t peakcnt=0, errCnt=0, waveLenCnt=0;
1063 uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
1064 uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0};
1065 fc = countFC(dest, size, 0);
1066 if (fc!=2 && fc!=4 && fc!=8) return -1;
1067 if (g_debugMode==2) prnt("DEBUG PSK: FC: %d",fc);
1068
1069 //find first full wave
1070 for (i=160; i<loopCnt; i++){
1071 if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
1072 if (waveStart == 0) {
1073 waveStart = i+1;
1074 //prnt("DEBUG: waveStart: %d",waveStart);
1075 } else {
1076 waveEnd = i+1;
1077 //prnt("DEBUG: waveEnd: %d",waveEnd);
1078 waveLenCnt = waveEnd-waveStart;
1079 if (waveLenCnt > fc){
1080 firstFullWave = waveStart;
1081 fullWaveLen=waveLenCnt;
1082 break;
1083 }
1084 waveStart=0;
1085 }
1086 }
1087 }
1088 if (g_debugMode ==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
1089
1090 //test each valid clock from greatest to smallest to see which lines up
1091 for(clkCnt=7; clkCnt >= 1 ; clkCnt--){
1092 lastClkBit = firstFullWave; //set end of wave as clock align
1093 waveStart = 0;
1094 errCnt=0;
1095 peakcnt=0;
1096 if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit);
1097
1098 for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){
1099 //top edge of wave = start of new wave
1100 if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
1101 if (waveStart == 0) {
1102 waveStart = i+1;
1103 waveLenCnt=0;
1104 } else { //waveEnd
1105 waveEnd = i+1;
1106 waveLenCnt = waveEnd-waveStart;
1107 if (waveLenCnt > fc){
1108 //if this wave is a phase shift
1109 if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,i+1,fc);
1110 if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
1111 peakcnt++;
1112 lastClkBit+=clk[clkCnt];
1113 } else if (i<lastClkBit+8){
1114 //noise after a phase shift - ignore
1115 } else { //phase shift before supposed to based on clock
1116 errCnt++;
1117 }
1118 } else if (i+1 > lastClkBit + clk[clkCnt] + tol + fc){
1119 lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
1120 }
1121 waveStart=i+1;
1122 }
1123 }
1124 }
1125 if (errCnt == 0){
1126 return clk[clkCnt];
1127 }
1128 if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt]=errCnt;
1129 if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt]=peakcnt;
1130 }
1131 //all tested with errors
1132 //return the highest clk with the most peaks found
1133 uint8_t best=7;
1134 for (i=7; i>=1; i--){
1135 if (peaksdet[i] > peaksdet[best]) {
1136 best = i;
1137 }
1138 if (g_debugMode == 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[i],peaksdet[i],bestErr[i],clk[best]);
1139 }
1140 return clk[best];
1141 }
1142
1143 int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low){
1144 //find shortest transition from high to low
1145 size_t i = 0;
1146 size_t transition1 = 0;
1147 int lowestTransition = 255;
1148 bool lastWasHigh = false;
1149
1150 //find first valid beginning of a high or low wave
1151 while ((dest[i] >= peak || dest[i] <= low) && (i < size))
1152 ++i;
1153 while ((dest[i] < peak && dest[i] > low) && (i < size))
1154 ++i;
1155 lastWasHigh = (dest[i] >= peak);
1156
1157 if (i==size) return 0;
1158 transition1 = i;
1159
1160 for (;i < size; i++) {
1161 if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) {
1162 lastWasHigh = (dest[i] >= peak);
1163 if (i-transition1 < lowestTransition) lowestTransition = i-transition1;
1164 transition1 = i;
1165 }
1166 }
1167 if (lowestTransition == 255) lowestTransition = 0;
1168 if (g_debugMode==2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d",lowestTransition);
1169 return lowestTransition;
1170 }
1171
1172 //by marshmellow
1173 //detect nrz clock by reading #peaks vs no peaks(or errors)
1174 int DetectNRZClock(uint8_t dest[], size_t size, int clock)
1175 {
1176 size_t i=0;
1177 uint8_t clk[]={8,16,32,40,50,64,100,128,255};
1178 size_t loopCnt = 4096; //don't need to loop through entire array...
1179 if (size == 0) return 0;
1180 if (size<loopCnt) loopCnt = size-20;
1181 //if we already have a valid clock quit
1182 for (; i < 8; ++i)
1183 if (clk[i] == clock) return clock;
1184
1185 //get high and low peak
1186 int peak, low;
1187 if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return 0;
1188
1189 int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low);
1190 size_t ii;
1191 uint8_t clkCnt;
1192 uint8_t tol = 0;
1193 uint16_t smplCnt = 0;
1194 int16_t peakcnt = 0;
1195 int16_t peaksdet[] = {0,0,0,0,0,0,0,0};
1196 uint16_t maxPeak = 255;
1197 bool firstpeak = false;
1198 //test for large clipped waves
1199 for (i=0; i<loopCnt; i++){
1200 if (dest[i] >= peak || dest[i] <= low){
1201 if (!firstpeak) continue;
1202 smplCnt++;
1203 } else {
1204 firstpeak=true;
1205 if (smplCnt > 6 ){
1206 if (maxPeak > smplCnt){
1207 maxPeak = smplCnt;
1208 //prnt("maxPk: %d",maxPeak);
1209 }
1210 peakcnt++;
1211 //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt);
1212 smplCnt=0;
1213 }
1214 }
1215 }
1216 bool errBitHigh = 0;
1217 bool bitHigh = 0;
1218 uint8_t ignoreCnt = 0;
1219 uint8_t ignoreWindow = 4;
1220 bool lastPeakHigh = 0;
1221 int lastBit = 0;
1222 peakcnt=0;
1223 //test each valid clock from smallest to greatest to see which lines up
1224 for(clkCnt=0; clkCnt < 8; ++clkCnt){
1225 //ignore clocks smaller than smallest peak
1226 if (clk[clkCnt] < maxPeak - (clk[clkCnt]/4)) continue;
1227 //try lining up the peaks by moving starting point (try first 256)
1228 for (ii=20; ii < loopCnt; ++ii){
1229 if ((dest[ii] >= peak) || (dest[ii] <= low)){
1230 peakcnt=0;
1231 bitHigh = false;
1232 ignoreCnt = 0;
1233 lastBit = ii-clk[clkCnt];
1234 //loop through to see if this start location works
1235 for (i = ii; i < size-20; ++i) {
1236 //if we are at a clock bit
1237 if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) {
1238 //test high/low
1239 if (dest[i] >= peak || dest[i] <= low) {
1240 //if same peak don't count it
1241 if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) {
1242 peakcnt++;
1243 }
1244 lastPeakHigh = (dest[i] >= peak);
1245 bitHigh = true;
1246 errBitHigh = false;
1247 ignoreCnt = ignoreWindow;
1248 lastBit += clk[clkCnt];
1249 } else if (i == lastBit + clk[clkCnt] + tol) {
1250 lastBit += clk[clkCnt];
1251 }
1252 //else if not a clock bit and no peaks
1253 } else if (dest[i] < peak && dest[i] > low){
1254 if (ignoreCnt==0){
1255 bitHigh=false;
1256 if (errBitHigh==true) peakcnt--;
1257 errBitHigh=false;
1258 } else {
1259 ignoreCnt--;
1260 }
1261 // else if not a clock bit but we have a peak
1262 } else if ((dest[i]>=peak || dest[i]<=low) && (!bitHigh)) {
1263 //error bar found no clock...
1264 errBitHigh=true;
1265 }
1266 }
1267 if(peakcnt>peaksdet[clkCnt]) {
1268 peaksdet[clkCnt]=peakcnt;
1269 }
1270 }
1271 }
1272 }
1273 int iii=7;
1274 uint8_t best=0;
1275 for (iii=7; iii > 0; iii--){
1276 if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) {
1277 if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) {
1278 best = iii;
1279 }
1280 } else if (peaksdet[iii] > peaksdet[best]){
1281 best = iii;
1282 }
1283 if (g_debugMode==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, maxPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],maxPeak, clk[best], lowestTransition);
1284 }
1285
1286 return clk[best];
1287 }
1288
1289 // by marshmellow
1290 // convert psk1 demod to psk2 demod
1291 // only transition waves are 1s
1292 void psk1TOpsk2(uint8_t *bits, size_t size) {
1293 uint8_t lastBit = bits[0];
1294 for (size_t i = 1; i < size; i++){
1295 //ignore errors
1296 if (bits[i] == 7) continue;
1297
1298 if (lastBit != bits[i]){
1299 lastBit = bits[i];
1300 bits[i] = 1;
1301 } else {
1302 bits[i] = 0;
1303 }
1304 }
1305 }
1306
1307 // by marshmellow
1308 // convert psk2 demod to psk1 demod
1309 // from only transition waves are 1s to phase shifts change bit
1310 void psk2TOpsk1(uint8_t *bits, size_t size) {
1311 uint8_t phase = 0;
1312 for (size_t i = 0; i < size; i++){
1313 if (bits[i] == 1){
1314 phase ^= 1;
1315 }
1316 bits[i] = phase;
1317 }
1318 }
1319
1320 // redesigned by marshmellow adjusted from existing decode functions
1321 // indala id decoding - only tested on 26 bit tags, but attempted to make it work for more
1322 int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert)
1323 {
1324 //26 bit 40134 format (don't know other formats)
1325 uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
1326 uint8_t preamble_i[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0};
1327 size_t startidx = 0;
1328 if (!preambleSearch(bitStream, preamble, sizeof(preamble), size, &startidx)){
1329 // if didn't find preamble try again inverting
1330 if (!preambleSearch(bitStream, preamble_i, sizeof(preamble_i), size, &startidx)) return -1;
1331 *invert ^= 1;
1332 }
1333 if (*size != 64 && *size != 224) return -2;
1334 if (*invert==1)
1335 for (size_t i = startidx; i < *size; i++)
1336 bitStream[i] ^= 1;
1337
1338 return (int) startidx;
1339 }
1340
1341 // by marshmellow - demodulate NRZ wave - requires a read with strong signal
1342 // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
1343 int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert){
1344 if (justNoise(dest, *size)) return -1;
1345 *clk = DetectNRZClock(dest, *size, *clk);
1346 if (*clk==0) return -2;
1347 size_t i, gLen = 4096;
1348 if (gLen>*size) gLen = *size-20;
1349 int high, low;
1350 if (getHiLo(dest, gLen, &high, &low, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low
1351
1352 uint8_t bit=0;
1353 //convert wave samples to 1's and 0's
1354 for(i=20; i < *size-20; i++){
1355 if (dest[i] >= high) bit = 1;
1356 if (dest[i] <= low) bit = 0;
1357 dest[i] = bit;
1358 }
1359 //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit)
1360 size_t lastBit = 0;
1361 size_t numBits = 0;
1362 for(i=21; i < *size-20; i++) {
1363 //if transition detected or large number of same bits - store the passed bits
1364 if (dest[i] != dest[i-1] || (i-lastBit) == (10 * *clk)) {
1365 memset(dest+numBits, dest[i-1] ^ *invert, (i - lastBit + (*clk/4)) / *clk);
1366 numBits += (i - lastBit + (*clk/4)) / *clk;
1367 lastBit = i-1;
1368 }
1369 }
1370 *size = numBits;
1371 return 0;
1372 }
1373
1374 //by marshmellow
1375 //detects the bit clock for FSK given the high and low Field Clocks
1376 uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fcLow)
1377 {
1378 uint8_t clk[] = {8,16,32,40,50,64,100,128,0};
1379 uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1380 uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1381 uint8_t rfLensFnd = 0;
1382 uint8_t lastFCcnt = 0;
1383 uint16_t fcCounter = 0;
1384 uint16_t rfCounter = 0;
1385 uint8_t firstBitFnd = 0;
1386 size_t i;
1387 if (size == 0) return 0;
1388
1389 uint8_t fcTol = ((fcHigh*100 - fcLow*100)/2 + 50)/100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2);
1390 rfLensFnd=0;
1391 fcCounter=0;
1392 rfCounter=0;
1393 firstBitFnd=0;
1394 //prnt("DEBUG: fcTol: %d",fcTol);
1395 // prime i to first peak / up transition
1396 for (i = 160; i < size-20; i++)
1397 if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1])
1398 break;
1399
1400 for (; i < size-20; i++){
1401 fcCounter++;
1402 rfCounter++;
1403
1404 if (BitStream[i] <= BitStream[i-1] || BitStream[i] < BitStream[i+1])
1405 continue;
1406 // else new peak
1407 // if we got less than the small fc + tolerance then set it to the small fc
1408 // if it is inbetween set it to the last counter
1409 if (fcCounter < fcHigh && fcCounter > fcLow)
1410 fcCounter = lastFCcnt;
1411 else if (fcCounter < fcLow+fcTol)
1412 fcCounter = fcLow;
1413 else //set it to the large fc
1414 fcCounter = fcHigh;
1415
1416 //look for bit clock (rf/xx)
1417 if ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)){
1418 //not the same size as the last wave - start of new bit sequence
1419 if (firstBitFnd > 1){ //skip first wave change - probably not a complete bit
1420 for (int ii=0; ii<15; ii++){
1421 if (rfLens[ii] >= (rfCounter-4) && rfLens[ii] <= (rfCounter+4)){
1422 rfCnts[ii]++;
1423 rfCounter = 0;
1424 break;
1425 }
1426 }
1427 if (rfCounter > 0 && rfLensFnd < 15){
1428 //prnt("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter);
1429 rfCnts[rfLensFnd]++;
1430 rfLens[rfLensFnd++] = rfCounter;
1431 }
1432 } else {
1433 firstBitFnd++;
1434 }
1435 rfCounter=0;
1436 lastFCcnt=fcCounter;
1437 }
1438 fcCounter=0;
1439 }
1440 uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15;
1441
1442 for (i=0; i<15; i++){
1443 //get highest 2 RF values (might need to get more values to compare or compare all?)
1444 if (rfCnts[i]>rfCnts[rfHighest]){
1445 rfHighest3=rfHighest2;
1446 rfHighest2=rfHighest;
1447 rfHighest=i;
1448 } else if(rfCnts[i]>rfCnts[rfHighest2]){
1449 rfHighest3=rfHighest2;
1450 rfHighest2=i;
1451 } else if(rfCnts[i]>rfCnts[rfHighest3]){
1452 rfHighest3=i;
1453 }
1454 if (g_debugMode==2) prnt("DEBUG FSK: RF %d, cnts %d",rfLens[i], rfCnts[i]);
1455 }
1456 // set allowed clock remainder tolerance to be 1 large field clock length+1
1457 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off
1458 uint8_t tol1 = fcHigh+1;
1459
1460 if (g_debugMode==2) prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]);
1461
1462 // loop to find the highest clock that has a remainder less than the tolerance
1463 // compare samples counted divided by
1464 // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less)
1465 int ii=7;
1466 for (; ii>=2; ii--){
1467 if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){
1468 if (rfLens[rfHighest2] % clk[ii] < tol1 || rfLens[rfHighest2] % clk[ii] > clk[ii]-tol1){
1469 if (rfLens[rfHighest3] % clk[ii] < tol1 || rfLens[rfHighest3] % clk[ii] > clk[ii]-tol1){
1470 if (g_debugMode==2) prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance",clk[ii]);
1471 break;
1472 }
1473 }
1474 }
1475 }
1476
1477 if (ii<2) return 0; // oops we went too far
1478
1479 return clk[ii];
1480 }
1481
1482 //by marshmellow
1483 //countFC is to detect the field clock lengths.
1484 //counts and returns the 2 most common wave lengths
1485 //mainly used for FSK field clock detection
1486 uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj)
1487 {
1488 uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1489 uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
1490 uint8_t fcLensFnd = 0;
1491 uint8_t lastFCcnt = 0;
1492 uint8_t fcCounter = 0;
1493 size_t i;
1494 if (size < 180) return 0;
1495
1496 // prime i to first up transition
1497 for (i = 160; i < size-20; i++)
1498 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1])
1499 break;
1500
1501 for (; i < size-20; i++){
1502 if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){
1503 // new up transition
1504 fcCounter++;
1505 if (fskAdj){
1506 //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8)
1507 if (lastFCcnt==5 && fcCounter==9) fcCounter--;
1508 //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5)
1509 if ((fcCounter==9) || fcCounter==4) fcCounter++;
1510 // save last field clock count (fc/xx)
1511 lastFCcnt = fcCounter;
1512 }
1513 // find which fcLens to save it to:
1514 for (int ii=0; ii<15; ii++){
1515 if (fcLens[ii]==fcCounter){
1516 fcCnts[ii]++;
1517 fcCounter=0;
1518 break;
1519 }
1520 }
1521 if (fcCounter>0 && fcLensFnd<15){
1522 //add new fc length
1523 fcCnts[fcLensFnd]++;
1524 fcLens[fcLensFnd++]=fcCounter;
1525 }
1526 fcCounter=0;
1527 } else {
1528 // count sample
1529 fcCounter++;
1530 }
1531 }
1532
1533 uint8_t best1=14, best2=14, best3=14;
1534 uint16_t maxCnt1=0;
1535 // go through fclens and find which ones are bigest 2
1536 for (i=0; i<15; i++){
1537 // get the 3 best FC values
1538 if (fcCnts[i]>maxCnt1) {
1539 best3=best2;
1540 best2=best1;
1541 maxCnt1=fcCnts[i];
1542 best1=i;
1543 } else if(fcCnts[i]>fcCnts[best2]){
1544 best3=best2;
1545 best2=i;
1546 } else if(fcCnts[i]>fcCnts[best3]){
1547 best3=i;
1548 }
1549 if (g_debugMode==2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u",fcLens[i],fcCnts[i],fcLens[best1],fcLens[best2]);
1550 }
1551 if (fcLens[best1]==0) return 0;
1552 uint8_t fcH=0, fcL=0;
1553 if (fcLens[best1]>fcLens[best2]){
1554 fcH=fcLens[best1];
1555 fcL=fcLens[best2];
1556 } else{
1557 fcH=fcLens[best2];
1558 fcL=fcLens[best1];
1559 }
1560 if ((size-180)/fcH/3 > fcCnts[best1]+fcCnts[best2]) {
1561 if (g_debugMode==2) prnt("DEBUG countfc: fc is too large: %u > %u. Not psk or fsk",(size-180)/fcH/3,fcCnts[best1]+fcCnts[best2]);
1562 return 0; //lots of waves not psk or fsk
1563 }
1564 // TODO: take top 3 answers and compare to known Field clocks to get top 2
1565
1566 uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
1567 if (fskAdj) return fcs;
1568 return fcLens[best1];
1569 }
1570
1571 //by marshmellow - demodulate PSK1 wave
1572 //uses wave lengths (# Samples)
1573 int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
1574 {
1575 if (size == 0) return -1;
1576 uint16_t loopCnt = 4096; //don't need to loop through entire array...
1577 if (*size<loopCnt) loopCnt = *size;
1578
1579 size_t numBits=0;
1580 uint8_t curPhase = *invert;
1581 size_t i=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
1582 uint16_t fc=0, fullWaveLen=0, tol=1;
1583 uint16_t errCnt=0, waveLenCnt=0, errCnt2=0;
1584 fc = countFC(dest, *size, 1);
1585 uint8_t fc2 = fc >> 8;
1586 if (fc2 == 10) return -1; //fsk found - quit
1587 fc = fc & 0xFF;
1588 if (fc!=2 && fc!=4 && fc!=8) return -1;
1589 //prnt("DEBUG: FC: %d",fc);
1590 *clock = DetectPSKClock(dest, *size, *clock);
1591 if (*clock == 0) return -1;
1592
1593 //find start of modulating data in trace
1594 uint8_t threshold_value = 123; //-5
1595 i = findModStart(dest, *size, threshold_value, fc);
1596
1597 //find first phase shift
1598 int avgWaveVal=0, lastAvgWaveVal=0;
1599 waveStart = i;
1600 for (; i<loopCnt; i++){
1601 // find peak
1602 if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
1603 waveEnd = i+1;
1604 if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u",waveEnd, waveStart);
1605 waveLenCnt = waveEnd-waveStart;
1606 if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+3)){ //not first peak and is a large wave but not out of whack
1607 lastAvgWaveVal = avgWaveVal/(waveLenCnt);
1608 firstFullWave = waveStart;
1609 fullWaveLen=waveLenCnt;
1610 //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
1611 if (lastAvgWaveVal > threshold_value) curPhase ^= 1;
1612 break;
1613 }
1614 waveStart = i+1;
1615 avgWaveVal = 0;
1616 }
1617 avgWaveVal += dest[i+2];
1618 }
1619 if (firstFullWave == 0) {
1620 // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
1621 // so skip a little to ensure we are past any Start Signal
1622 firstFullWave = 160;
1623 memset(dest, curPhase, firstFullWave / *clock);
1624 } else {
1625 memset(dest, curPhase^1, firstFullWave / *clock);
1626 }
1627 //advance bits
1628 numBits += (firstFullWave / *clock);
1629 //set start of wave as clock align
1630 lastClkBit = firstFullWave;
1631 if (g_debugMode==2) prnt("DEBUG PSK: firstFullWave: %u, waveLen: %u",firstFullWave,fullWaveLen);
1632 if (g_debugMode==2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit,(unsigned int) fc);
1633 waveStart = 0;
1634 dest[numBits++] = curPhase; //set first read bit
1635 for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){
1636 //top edge of wave = start of new wave
1637 if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
1638 if (waveStart == 0) {
1639 waveStart = i+1;
1640 waveLenCnt = 0;
1641 avgWaveVal = dest[i+1];
1642 } else { //waveEnd
1643 waveEnd = i+1;
1644 waveLenCnt = waveEnd-waveStart;
1645 lastAvgWaveVal = avgWaveVal/waveLenCnt;
1646 if (waveLenCnt > fc){
1647 //prnt("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
1648 //this wave is a phase shift
1649 //prnt("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
1650 if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
1651 curPhase ^= 1;
1652 dest[numBits++] = curPhase;
1653 lastClkBit += *clock;
1654 } else if (i < lastClkBit+10+fc){
1655 //noise after a phase shift - ignore
1656 } else { //phase shift before supposed to based on clock
1657 errCnt++;
1658 dest[numBits++] = 7;
1659 }
1660 } else if (i+1 > lastClkBit + *clock + tol + fc){
1661 lastClkBit += *clock; //no phase shift but clock bit
1662 dest[numBits++] = curPhase;
1663 } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often)
1664 errCnt2++;
1665 if(errCnt2 > 101) return errCnt2;
1666 }
1667 avgWaveVal = 0;
1668 waveStart = i+1;
1669 }
1670 }
1671 avgWaveVal += dest[i+1];
1672 }
1673 *size = numBits;
1674 return errCnt;
1675 }
1676
1677 //by marshmellow
1678 //attempt to identify a Sequence Terminator in ASK modulated raw wave
1679 bool DetectST(uint8_t buffer[], size_t *size, int *foundclock) {
1680 size_t bufsize = *size;
1681 //need to loop through all samples and identify our clock, look for the ST pattern
1682 uint8_t fndClk[] = {8,16,32,40,50,64,128};
1683 int clk = 0;
1684 int tol = 0;
1685 int i, j, skip, start, end, low, high, minClk, waveStart;
1686 bool complete = false;
1687 int tmpbuff[bufsize / 32]; //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured
1688 int waveLen[bufsize / 32]; // if clock is larger then we waste memory in array size that is not needed...
1689 size_t testsize = (bufsize < 512) ? bufsize : 512;
1690 int phaseoff = 0;
1691 high = low = 128;
1692 memset(tmpbuff, 0, sizeof(tmpbuff));
1693 memset(waveLen, 0, sizeof(waveLen));
1694
1695
1696 if ( getHiLo(buffer, testsize, &high, &low, 80, 80) == -1 ) {
1697 if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
1698 return false; //just noise
1699 }
1700 i = 0;
1701 j = 0;
1702 minClk = 255;
1703 // get to first full low to prime loop and skip incomplete first pulse
1704 while ((buffer[i] < high) && (i < bufsize))
1705 ++i;
1706 while ((buffer[i] > low) && (i < bufsize))
1707 ++i;
1708 skip = i;
1709
1710 // populate tmpbuff buffer with pulse lengths
1711 while (i < bufsize) {
1712 // measure from low to low
1713 while ((buffer[i] > low) && (i < bufsize))
1714 ++i;
1715 start= i;
1716 while ((buffer[i] < high) && (i < bufsize))
1717 ++i;
1718 //first high point for this wave
1719 waveStart = i;
1720 while ((buffer[i] > low) && (i < bufsize))
1721 ++i;
1722 if (j >= (bufsize/32)) {
1723 break;
1724 }
1725 waveLen[j] = i - waveStart; //first high to first low
1726 tmpbuff[j++] = i - start;
1727 if (i-start < minClk && i < bufsize) {
1728 minClk = i - start;
1729 }
1730 }
1731 // set clock - might be able to get this externally and remove this work...
1732 if (!clk) {
1733 for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) {
1734 tol = fndClk[clkCnt]/8;
1735 if (minClk >= fndClk[clkCnt]-tol && minClk <= fndClk[clkCnt]+1) {
1736 clk=fndClk[clkCnt];
1737 break;
1738 }
1739 }
1740 // clock not found - ERROR
1741 if (!clk) {
1742 if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
1743 return false;
1744 }
1745 } else tol = clk/8;
1746
1747 *foundclock = clk;
1748
1749 // look for Sequence Terminator - should be pulses of clk*(1 or 1.5), clk*2, clk*(1.5 or 2)
1750 start = -1;
1751 for (i = 0; i < j - 4; ++i) {
1752 skip += tmpbuff[i];
1753 if (tmpbuff[i] >= clk*1-tol && tmpbuff[i] <= (clk*2)+tol && waveLen[i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior
1754 if (tmpbuff[i+1] >= clk*2-tol && tmpbuff[i+1] <= clk*2+tol && waveLen[i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2
1755 if (tmpbuff[i+2] >= (clk*3)/2-tol && tmpbuff[i+2] <= clk*2+tol && waveLen[i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave
1756 if (tmpbuff[i+3] >= clk*1-tol && tmpbuff[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
1757 start = i + 3;
1758 break;
1759 }
1760 }
1761 }
1762 }
1763 }
1764 // first ST not found - ERROR
1765 if (start < 0) {
1766 if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
1767 return false;
1768 } else {
1769 if (g_debugMode==2) prnt("DEBUG STT: first STT found at: %d, j=%d",start, j);
1770 }
1771 if (waveLen[i+2] > clk*1+tol)
1772 phaseoff = 0;
1773 else
1774 phaseoff = clk/2;
1775
1776 // skip over the remainder of ST
1777 skip += clk*7/2; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point
1778
1779 // now do it again to find the end
1780 end = skip;
1781 for (i += 3; i < j - 4; ++i) {
1782 end += tmpbuff[i];
1783 if (tmpbuff[i] >= clk*1-tol && tmpbuff[i] <= (clk*2)+tol && waveLen[i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior
1784 if (tmpbuff[i+1] >= clk*2-tol && tmpbuff[i+1] <= clk*2+tol && waveLen[i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2
1785 if (tmpbuff[i+2] >= (clk*3)/2-tol && tmpbuff[i+2] <= clk*2+tol && waveLen[i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave
1786 if (tmpbuff[i+3] >= clk*1-tol && tmpbuff[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
1787 complete = true;
1788 break;
1789 }
1790 }
1791 }
1792 }
1793 }
1794 end -= phaseoff;
1795 //didn't find second ST - ERROR
1796 if (!complete) {
1797 if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting");
1798 return false;
1799 }
1800 if (g_debugMode==2) prnt("DEBUG STT: start of data: %d end of data: %d, datalen: %d, clk: %d, bits: %d, phaseoff: %d", skip, end, end-skip, clk, (end-skip)/clk, phaseoff);
1801 //now begin to trim out ST so we can use normal demod cmds
1802 start = skip;
1803 size_t datalen = end - start;
1804 // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock
1805 if ( clk - (datalen % clk) <= clk/8) {
1806 // padd the amount off - could be problematic... but shouldn't happen often
1807 datalen += clk - (datalen % clk);
1808 } else if ( (datalen % clk) <= clk/8 ) {
1809 // padd the amount off - could be problematic... but shouldn't happen often
1810 datalen -= datalen % clk;
1811 } else {
1812 if (g_debugMode==2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk);
1813 return false;
1814 }
1815 // if datalen is less than one t55xx block - ERROR
1816 if (datalen/clk < 8*4) {
1817 if (g_debugMode==2) prnt("DEBUG STT: datalen is less than 1 full t55xx block - quitting");
1818 return false;
1819 }
1820 size_t dataloc = start;
1821 if (buffer[dataloc-(clk*4)-(clk/8)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {
1822 //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start
1823 for ( i=0; i <= (clk/8); ++i ) {
1824 if ( buffer[dataloc - (clk*4) - i] <= low ) {
1825 dataloc -= i;
1826 break;
1827 }
1828 }
1829 }
1830
1831 size_t newloc = 0;
1832 i=0;
1833 if (g_debugMode==2) prnt("DEBUG STT: Starting STT trim - start: %d, datalen: %d ",dataloc, datalen);
1834
1835 // warning - overwriting buffer given with raw wave data with ST removed...
1836 while ( dataloc < bufsize-(clk/2) ) {
1837 //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part)
1838 if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+3]<high && buffer[dataloc+3]>low) {
1839 for(i=0; i < clk/2-tol; ++i) {
1840 buffer[dataloc+i] = high+5;
1841 }
1842 }
1843 for (i=0; i<datalen; ++i) {
1844 if (i+newloc < bufsize) {
1845 if (i+newloc < dataloc)
1846 buffer[i+newloc] = buffer[dataloc];
1847
1848 dataloc++;
1849 }
1850 }
1851 newloc += i;
1852 //skip next ST - we just assume it will be there from now on...
1853 if (g_debugMode==2) prnt("DEBUG STT: skipping STT at %d to %d", dataloc, dataloc+(clk*4));
1854 dataloc += clk*4;
1855 }
1856 *size = newloc;
1857 return true;
1858 }
Impressum, Datenschutz