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