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