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