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