common/lfdemod.c

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