//-----------------------------------------------------------------------------
#include <string.h> // for memset, memcmp and size_t
+#include "lfdemod.h"
#include <stdint.h> // for uint_32+
#include <stdbool.h> // for bool
+#include "parity.h" // for parity test
//**********************************************************************************************
//---------------------------------Utilities Section--------------------------------------------
//**********************************************************************************************
+#define LOWEST_DEFAULT_CLOCK 32
+#define FSK_PSK_THRESHOLD 123
//to allow debug print calls when used not on device
void dummy(char *fmt, ...){}
#endif
uint8_t justNoise(uint8_t *BitStream, size_t size) {
- static const uint8_t THRESHOLD = 123;
//test samples are not just noise
uint8_t justNoise1 = 1;
for(size_t idx=0; idx < size && justNoise1 ;idx++){
- justNoise1 = BitStream[idx] < THRESHOLD;
+ justNoise1 = BitStream[idx] < FSK_PSK_THRESHOLD;
}
return justNoise1;
}
if (BitStream[i] > *high) *high = BitStream[i];
if (BitStream[i] < *low) *low = BitStream[i];
}
- if (*high < 123) return -1; // just noise
+ if (*high < FSK_PSK_THRESHOLD) return -1; // just noise
*high = ((*high-128)*fuzzHi + 12800)/100;
*low = ((*low-128)*fuzzLo + 12800)/100;
return 1;
// by marshmellow
// pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType
// returns 1 if passed
-uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) {
- uint8_t ans = 0;
- for (uint8_t i = 0; i < bitLen; i++){
- ans ^= ((bits >> i) & 1);
- }
- if (g_debugMode) prnt("DEBUG: ans: %d, ptype: %d, bits: %08X",ans,pType,bits);
- return (ans == pType);
+bool parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) {
+ return oddparity32(bits) ^ pType;
}
// by marshmellow
-// takes a array of binary values, start position, length of bits per parity (includes parity bit),
-// Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run)
+// takes a array of binary values, start position, length of bits per parity (includes parity bit - MAX 32),
+// Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run)
size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) {
uint32_t parityWd = 0;
- size_t j = 0, bitCnt = 0;
+ size_t bitCnt = 0;
for (int word = 0; word < (bLen); word+=pLen) {
for (int bit=0; bit < pLen; bit++) {
+ if (word+bit >= bLen) break;
parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
- BitStream[j++] = (BitStream[startIdx+word+bit]);
+ BitStream[bitCnt++] = (BitStream[startIdx+word+bit]);
}
if (word+pLen > bLen) break;
- j--; // overwrite parity with next data
+ bitCnt--; // overwrite parity with next data
// if parity fails then return 0
switch (pType) {
- case 3: if (BitStream[j]==1) {return 0;} break; //should be 0 spacer bit
- case 2: if (BitStream[j]==0) {return 0;} break; //should be 1 spacer bit
+ case 3: if (BitStream[bitCnt]==1) {return 0;} break; //should be 0 spacer bit
+ case 2: if (BitStream[bitCnt]==0) {return 0;} break; //should be 1 spacer bit
default: if (parityTest(parityWd, pLen, pType) == 0) {return 0;} break; //test parity
}
- bitCnt+=(pLen-1);
parityWd = 0;
}
// if we got here then all the parities passed
- //return ID start index and size
+ //return size
return bitCnt;
}
}
// find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup.
-size_t findModStart(uint8_t dest[], size_t size, uint8_t threshold_value, uint8_t expWaveSize) {
+size_t findModStart(uint8_t dest[], size_t size, uint8_t expWaveSize) {
size_t i = 0;
size_t waveSizeCnt = 0;
uint8_t thresholdCnt = 0;
- bool isAboveThreshold = dest[i++] >= threshold_value;
+ bool isAboveThreshold = dest[i++] >= FSK_PSK_THRESHOLD;
for (; i < size-20; i++ ) {
- if(dest[i] < threshold_value && isAboveThreshold) {
+ if(dest[i] < FSK_PSK_THRESHOLD && isAboveThreshold) {
thresholdCnt++;
if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
isAboveThreshold = false;
waveSizeCnt = 0;
- } else if (dest[i] >= threshold_value && !isAboveThreshold) {
+ } else if (dest[i] >= FSK_PSK_THRESHOLD && !isAboveThreshold) {
thresholdCnt++;
if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break;
isAboveThreshold = true;
return i;
}
+int getClosestClock(int testclk) {
+ uint8_t fndClk[] = {8,16,32,40,50,64,128};
+
+ for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++)
+ if (testclk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && testclk <= fndClk[clkCnt]+1)
+ return fndClk[clkCnt];
+
+ return 0;
+}
+
+void getNextLow(uint8_t samples[], size_t size, int low, size_t *i) {
+ while ((samples[*i] > low) && (*i < size))
+ *i+=1;
+}
+
+void getNextHigh(uint8_t samples[], size_t size, int high, size_t *i) {
+ while ((samples[*i] < high) && (*i < size))
+ *i+=1;
+}
+
+// load wave counters
+bool loadWaveCounters(uint8_t samples[], size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) {
+ size_t i=0, firstLow, firstHigh;
+ size_t testsize = (size < 512) ? size : 512;
+
+ if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) {
+ if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
+ return false; //just noise
+ }
+
+ // get to first full low to prime loop and skip incomplete first pulse
+ getNextHigh(samples, size, *high, &i);
+ getNextLow(samples, size, *low, &i);
+ *skip = i;
+
+ // populate tmpbuff buffer with pulse lengths
+ while (i < size) {
+ // measure from low to low
+ firstLow = i;
+ //find first high point for this wave
+ getNextHigh(samples, size, *high, &i);
+ firstHigh = i;
+
+ getNextLow(samples, size, *low, &i);
+
+ if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK))
+ break;
+
+ highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low
+ lowToLowWaveLen[*waveCnt] = i - firstLow;
+ *waveCnt += 1;
+ if (i-firstLow < *minClk && i < size) {
+ *minClk = i - firstLow;
+ }
+ }
+ return true;
+}
+
+size_t pskFindFirstPhaseShift(uint8_t samples[], size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) {
+ uint16_t loopCnt = (size+3 < 4096) ? size : 4096; //don't need to loop through entire array...
+
+ uint16_t avgWaveVal=0, lastAvgWaveVal=0;
+ size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave;
+ for (; i<loopCnt; i++) {
+ // find peak // was "samples[i] + fc" but why? must have been used to weed out some wave error... removed..
+ if (samples[i] < samples[i+1] && samples[i+1] >= samples[i+2]){
+ waveEnd = i+1;
+ if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart);
+ waveLenCnt = waveEnd-waveStart;
+ if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack
+ lastAvgWaveVal = avgWaveVal/(waveLenCnt);
+ firstFullWave = waveStart;
+ *fullWaveLen = waveLenCnt;
+ //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
+ if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1;
+ return firstFullWave;
+ }
+ waveStart = i+1;
+ avgWaveVal = 0;
+ }
+ avgWaveVal += samples[i+2];
+ }
+ return 0;
+}
+
//by marshmellow
//amplify based on ask edge detection - not accurate enough to use all the time
void askAmp(uint8_t *BitStream, size_t size) {
//by marshmellow
//encode binary data into binary manchester
-//NOTE: BitStream must have double the size available in memory to do the swap
+//NOTE: BitStream must have triple the size of "size" available in memory to do the swap
int ManchesterEncode(uint8_t *BitStream, size_t size) {
- size_t modIdx=size, i=0;
- if (size>modIdx) return -1;
+ //allow up to 4K out (means BitStream must be at least 2048+4096 to handle the swap)
+ size = (size>2048) ? 2048 : size;
+ size_t modIdx = size;
+ size_t i;
for (size_t idx=0; idx < size; idx++){
BitStream[idx+modIdx++] = BitStream[idx];
BitStream[idx+modIdx++] = BitStream[idx]^1;
}
- for (; i<(size*2); i++){
+ for (i=0; i<(size*2); i++){
BitStream[i] = BitStream[i+size];
}
return i;
// by marshmellow
// to help detect clocks on heavily clipped samples
// based on count of low to low
-int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low, int *clock) {
- uint8_t fndClk[] = {8,16,32,40,50,64,128};
+int DetectStrongAskClock(uint8_t dest[], size_t size, int high, int low, int *clock) {
size_t startwave;
size_t i = 100;
size_t minClk = 255;
int shortestWaveIdx = 0;
// get to first full low to prime loop and skip incomplete first pulse
- while ((dest[i] < high) && (i < size))
- ++i;
- while ((dest[i] > low) && (i < size))
- ++i;
+ getNextHigh(dest, size, high, &i);
+ getNextLow(dest, size, low, &i);
// loop through all samples
while (i < size) {
// measure from low to low
- while ((dest[i] > low) && (i < size))
- ++i;
startwave = i;
- while ((dest[i] < high) && (i < size))
- ++i;
- while ((dest[i] > low) && (i < size))
- ++i;
+
+ getNextHigh(dest, size, high, &i);
+ getNextLow(dest, size, low, &i);
//get minimum measured distance
if (i-startwave < minClk && i < size) {
minClk = i - startwave;
}
}
// set clock
- if (g_debugMode==2) prnt("DEBUG ASK: detectstrongASKclk smallest wave: %d",minClk);
- for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) {
- if (minClk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && minClk <= fndClk[clkCnt]+1) {
- *clock = fndClk[clkCnt];
- return shortestWaveIdx;
- }
- }
- return 0;
+ if (g_debugMode==2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d",minClk);
+ *clock = getClosestClock(minClk);
+ if (*clock == 0)
+ return 0;
+
+ return shortestWaveIdx;
}
// by marshmellow
//by marshmellow
//detect nrz clock by reading #peaks vs no peaks(or errors)
-int DetectNRZClock_ext(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) {
+int DetectNRZClock(uint8_t dest[], size_t size, int clock, size_t *clockStartIdx) {
size_t i=0;
uint8_t clk[]={8,16,32,40,50,64,100,128,255};
size_t loopCnt = 4096; //don't need to loop through entire array...
//get high and low peak
int peak, low;
- if (getHiLo(dest, loopCnt, &peak, &low, 75, 75) < 1) return 0;
+ if (getHiLo(dest, loopCnt, &peak, &low, 90, 90) < 1) return 0;
int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low);
size_t ii;
uint16_t smplCnt = 0;
int16_t peakcnt = 0;
int16_t peaksdet[] = {0,0,0,0,0,0,0,0};
- uint16_t maxPeak = 255;
- bool firstpeak = false;
- //test for large clipped waves
- for (i=0; i<loopCnt; i++){
- if (dest[i] >= peak || dest[i] <= low){
- if (!firstpeak) continue;
+ uint16_t minPeak = 255;
+ bool firstpeak = true;
+ //test for large clipped waves - ignore first peak
+ for (i=0; i<loopCnt; i++) {
+ if (dest[i] >= peak || dest[i] <= low) {
+ if (firstpeak) continue;
smplCnt++;
} else {
- firstpeak=true;
- if (smplCnt > 6 ){
- if (maxPeak > smplCnt){
- maxPeak = smplCnt;
- //prnt("maxPk: %d",maxPeak);
- }
+ firstpeak = false;
+ if (smplCnt > 0) {
+ if (minPeak > smplCnt && smplCnt > 7) minPeak = smplCnt;
peakcnt++;
- //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt);
- smplCnt=0;
+ if (g_debugMode == 2) prnt("DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d",minPeak,smplCnt,peakcnt);
+ smplCnt = 0;
}
}
}
+ if (minPeak < 8) return 0;
bool errBitHigh = 0;
bool bitHigh = 0;
uint8_t ignoreCnt = 0;
size_t bestStart[]={0,0,0,0,0,0,0,0,0};
peakcnt=0;
//test each valid clock from smallest to greatest to see which lines up
- for(clkCnt=0; clkCnt < 8; ++clkCnt){
+ for(clkCnt=0; clkCnt < 8; ++clkCnt) {
//ignore clocks smaller than smallest peak
- if (clk[clkCnt] < maxPeak - (clk[clkCnt]/4)) continue;
+ if (clk[clkCnt] < minPeak - (clk[clkCnt]/4)) continue;
//try lining up the peaks by moving starting point (try first 256)
- for (ii=20; ii < loopCnt; ++ii){
- if ((dest[ii] >= peak) || (dest[ii] <= low)){
+ for (ii=20; ii < loopCnt; ++ii) {
+ if ((dest[ii] >= peak) || (dest[ii] <= low)) {
peakcnt = 0;
bitHigh = false;
ignoreCnt = 0;
lastBit += clk[clkCnt];
}
//else if not a clock bit and no peaks
- } else if (dest[i] < peak && dest[i] > low){
- if (ignoreCnt==0){
+ } else if (dest[i] < peak && dest[i] > low) {
+ if (ignoreCnt==0) {
bitHigh=false;
if (errBitHigh==true) peakcnt--;
errBitHigh=false;
}
int iii=7;
uint8_t best=0;
- for (iii=7; iii > 0; iii--){
+ for (iii=7; iii > 0; iii--) {
if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) {
if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) {
best = iii;
}
- } else if (peaksdet[iii] > peaksdet[best]){
+ } else if (peaksdet[iii] > peaksdet[best]) {
best = iii;
}
- 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);
+ if (g_debugMode==2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],minPeak, clk[best], lowestTransition);
}
*clockStartIdx = bestStart[best];
return clk[best];
}
-int DetectNRZClock(uint8_t dest[], size_t size, int clock) {
- size_t bestStart=0;
- return DetectNRZClock_ext(dest, size, clock, &bestStart);
-}
-
//by marshmellow
//countFC is to detect the field clock lengths.
//counts and returns the 2 most common wave lengths
best3=i;
}
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]);
+ if (fcLens[i]==0) break;
}
if (fcLens[best1]==0) return 0;
uint8_t fcH=0, fcL=0;
// TODO: take top 3 answers and compare to known Field clocks to get top 2
uint16_t fcs = (((uint16_t)fcH)<<8) | fcL;
- if (fskAdj) return fcs;
- return fcLens[best1];
+ if (fskAdj) return fcs;
+ return (uint16_t)fcLens[best2] << 8 | fcLens[best1];
}
//by marshmellow
//detect psk clock by reading each phase shift
// a phase shift is determined by measuring the sample length of each wave
-int DetectPSKClock_ext(uint8_t dest[], size_t size, int clock, int *firstPhaseShift) {
+int DetectPSKClock(uint8_t dest[], size_t size, int clock, size_t *firstPhaseShift, uint8_t *curPhase, uint8_t *fc) {
uint8_t clk[]={255,16,32,40,50,64,100,128,255}; //255 is not a valid clock
uint16_t loopCnt = 4096; //don't need to loop through entire array...
if (size == 0) return 0;
- if (size<loopCnt) loopCnt = size-20;
+ if (size+3<loopCnt) loopCnt = size-20;
+
+ uint16_t fcs = countFC(dest, size, 0);
+ *fc = fcs & 0xFF;
+ if (g_debugMode==2) prnt("DEBUG PSK: FC: %d, FC2: %d",*fc, fcs>>8);
+ if ((fcs>>8) == 10 && *fc == 8) return -1;
+ if (*fc!=2 && *fc!=4 && *fc!=8) return -1;
//if we already have a valid clock quit
size_t i=1;
if (clk[i] == clock) return clock;
size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0;
- uint8_t clkCnt, fc=0, fullWaveLen=0, tol=1;
- uint16_t peakcnt=0, errCnt=0, waveLenCnt=0;
+
+ uint8_t clkCnt, tol=1;
+ uint16_t peakcnt=0, errCnt=0, waveLenCnt=0, fullWaveLen=0;
uint16_t bestErr[]={1000,1000,1000,1000,1000,1000,1000,1000,1000};
uint16_t peaksdet[]={0,0,0,0,0,0,0,0,0};
- fc = countFC(dest, size, 0);
- if (fc!=2 && fc!=4 && fc!=8) return -1;
- if (g_debugMode==2) prnt("DEBUG PSK: FC: %d",fc);
- //find first full wave
- for (i=160; i<loopCnt; i++){
- if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){
- if (waveStart == 0) {
- waveStart = i+1;
- //prnt("DEBUG: waveStart: %d",waveStart);
- } else {
- waveEnd = i+1;
- //prnt("DEBUG: waveEnd: %d",waveEnd);
- waveLenCnt = waveEnd-waveStart;
- if (waveLenCnt > fc){
- firstFullWave = waveStart;
- fullWaveLen=waveLenCnt;
- break;
- }
- waveStart=0;
- }
- }
+ //find start of modulating data in trace
+ i = findModStart(dest, size, *fc);
+
+ firstFullWave = pskFindFirstPhaseShift(dest, size, curPhase, i, *fc, &fullWaveLen);
+ if (firstFullWave == 0) {
+ // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
+ // so skip a little to ensure we are past any Start Signal
+ firstFullWave = 160;
+ fullWaveLen = 0;
}
+
*firstPhaseShift = firstFullWave;
if (g_debugMode ==2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen);
//test each valid clock from greatest to smallest to see which lines up
- for(clkCnt=7; clkCnt >= 1 ; clkCnt--){
+ for(clkCnt=7; clkCnt >= 1 ; clkCnt--) {
+ tol = *fc/2;
lastClkBit = firstFullWave; //set end of wave as clock align
waveStart = 0;
errCnt=0;
} else { //waveEnd
waveEnd = i+1;
waveLenCnt = waveEnd-waveStart;
- if (waveLenCnt > fc){
+ if (waveLenCnt > *fc){
//if this wave is a phase shift
- 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);
+ 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);
if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit
peakcnt++;
lastClkBit+=clk[clkCnt];
} else { //phase shift before supposed to based on clock
errCnt++;
}
- } else if (i+1 > lastClkBit + clk[clkCnt] + tol + fc){
+ } else if (i+1 > lastClkBit + clk[clkCnt] + tol + *fc){
lastClkBit+=clk[clkCnt]; //no phase shift but clock bit
}
waveStart=i+1;
return clk[best];
}
-int DetectPSKClock(uint8_t dest[], size_t size, int clock) {
- int firstPhaseShift = 0;
- return DetectPSKClock_ext(dest, size, clock, &firstPhaseShift);
-}
+//int DetectPSKClock(uint8_t dest[], size_t size, int clock) {
+// size_t firstPhaseShift = 0;
+// uint8_t curPhase = 0;
+// return DetectPSKClock_ext(dest, size, clock, &firstPhaseShift, &curPhase);
+//}
//by marshmellow
//detects the bit clock for FSK given the high and low Field Clocks
//**********************************************************************************************
// 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...
-bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, int i) {
- for (; i < buffSize - 4; ++i) {
- *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...
- if (lowToLowWaveLen[i] >= clk*1-tol && lowToLowWaveLen[i] <= (clk*2)+tol && highToLowWaveLen[i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior
- 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
- 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
- if (lowToLowWaveLen[i+3] >= clk*1-tol && lowToLowWaveLen[i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
- *stStopLoc = i + 3;
+bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) {
+ if (buffSize < *i+4) return false;
+
+ for (; *i < buffSize - 4; *i+=1) {
+ *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...
+ if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior
+ 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
+ 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
+ if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit
+ *stStopLoc = *i + 3;
return true;
}
}
bool DetectST_ext(uint8_t buffer[], size_t *size, int *foundclock, size_t *ststart, size_t *stend) {
size_t bufsize = *size;
//need to loop through all samples and identify our clock, look for the ST pattern
- uint8_t fndClk[] = {8,16,32,40,50,64,128};
int clk = 0;
int tol = 0;
- int i, j, skip, start, end, low, high, minClk, waveStart;
+ int j=0, high, low, skip=0, start=0, end=0, minClk=255;
+ size_t i = 0;
//probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow]
- 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
- int waveLen[bufsize / 32]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed...
- size_t testsize = (bufsize < 512) ? bufsize : 512;
+ int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // 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
+ int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed...
+ //size_t testsize = (bufsize < 512) ? bufsize : 512;
int phaseoff = 0;
high = low = 128;
memset(tmpbuff, 0, sizeof(tmpbuff));
+ memset(waveLen, 0, sizeof(waveLen));
- if ( getHiLo(buffer, testsize, &high, &low, 80, 80) == -1 ) {
- if (g_debugMode==2) prnt("DEBUG STT: just noise detected - quitting");
- return false; //just noise
- }
- i = 0;
- j = 0;
- minClk = 255;
- // get to first full low to prime loop and skip incomplete first pulse
- while ((buffer[i] < high) && (i < bufsize))
- ++i;
- while ((buffer[i] > low) && (i < bufsize))
- ++i;
- skip = i;
-
- // populate tmpbuff buffer with pulse lengths
- while (i < bufsize) {
- // measure from low to low
- while ((buffer[i] > low) && (i < bufsize))
- ++i;
- start= i;
- while ((buffer[i] < high) && (i < bufsize))
- ++i;
- //first high point for this wave
- waveStart = i;
- while ((buffer[i] > low) && (i < bufsize))
- ++i;
- if (j >= (bufsize/32)) {
- break;
- }
- waveLen[j] = i - waveStart; //first high to first low
- tmpbuff[j++] = i - start;
- if (i-start < minClk && i < bufsize) {
- minClk = i - start;
- }
- }
+ if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false;
// set clock - might be able to get this externally and remove this work...
- if (!clk) {
- for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) {
- tol = fndClk[clkCnt]/8;
- if (minClk >= fndClk[clkCnt]-tol && minClk <= fndClk[clkCnt]+1) {
- clk=fndClk[clkCnt];
- break;
- }
- }
- // clock not found - ERROR
- if (!clk) {
- if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
- return false;
- }
- } else tol = clk/8;
-
+ clk = getClosestClock(minClk);
+ // clock not found - ERROR
+ if (clk == 0) {
+ if (g_debugMode==2) prnt("DEBUG STT: clock not found - quitting");
+ return false;
+ }
*foundclock = clk;
- i=0;
- if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, i)) {
+
+ tol = clk/8;
+ if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) {
// first ST not found - ERROR
if (g_debugMode==2) prnt("DEBUG STT: first STT not found - quitting");
return false;
} else {
- if (g_debugMode==2) prnt("DEBUG STT: first STT found at: %d, j=%d",start, j);
+ if (g_debugMode==2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j);
}
if (waveLen[i+2] > clk*1+tol)
phaseoff = 0;
// now do it again to find the end
int dummy1 = 0;
end = skip;
- if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, i+3)) {
+ i+=3;
+ if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) {
//didn't find second ST - ERROR
if (g_debugMode==2) prnt("DEBUG STT: second STT not found - quitting");
return false;
return false;
}
size_t dataloc = start;
- if (buffer[dataloc-(clk*4)-(clk/8)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {
+ if (buffer[dataloc-(clk*4)-(clk/4)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) {
//we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start
- for ( i=0; i <= (clk/8); ++i ) {
+ for ( i=0; i <= (clk/4); ++i ) {
if ( buffer[dataloc - (clk*4) - i] <= low ) {
dataloc -= i;
break;
// warning - overwriting buffer given with raw wave data with ST removed...
while ( dataloc < bufsize-(clk/2) ) {
//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)
- if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+3]<high && buffer[dataloc+3]>low) {
+ if (buffer[dataloc]<high && buffer[dataloc]>low && buffer[dataloc+clk/4]<high && buffer[dataloc+clk/4]>low) {
for(i=0; i < clk/2-tol; ++i) {
buffer[dataloc+i] = high+5;
}
- } //test for single sample outlier (high between two lows) in the case of very strong waves
- if (buffer[dataloc] >= high && buffer[dataloc+2] <= low) {
- buffer[dataloc] = buffer[dataloc+2];
- buffer[dataloc+1] = buffer[dataloc+2];
+ } //test for small spike outlier (high between two lows) in the case of very strong waves
+ if (buffer[dataloc] > low && buffer[dataloc+clk/4] <= low) {
+ for(i=0; i < clk/4; ++i) {
+ buffer[dataloc+i] = buffer[dataloc+clk/4];
+ }
}
if (firstrun) {
*stend = dataloc;
return DetectST_ext(buffer, size, foundclock, &ststart, &stend);
}
+//by marshmellow
+//take 11 10 01 11 00 and make 01100 ... miller decoding
+//check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row
+//decodes miller encoded binary
+//NOTE askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as!
+int millerRawDecode(uint8_t *BitStream, size_t *size, int invert) {
+ if (*size < 16) return -1;
+ uint16_t MaxBits = 512, errCnt = 0;
+ size_t i, bitCnt=0;
+ uint8_t alignCnt = 0, curBit = BitStream[0], alignedIdx = 0;
+ uint8_t halfClkErr = 0;
+ //find alignment, needs 4 1s or 0s to properly align
+ for (i=1; i < *size-1; i++) {
+ alignCnt = (BitStream[i] == curBit) ? alignCnt+1 : 0;
+ curBit = BitStream[i];
+ if (alignCnt == 4) break;
+ }
+ // for now error if alignment not found. later add option to run it with multiple offsets...
+ if (alignCnt != 4) {
+ if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bitstream is not miller or your data does not have a 101 in it");
+ return -1;
+ }
+ alignedIdx = (i-1) % 2;
+ for (i=alignedIdx; i < *size-3; i+=2) {
+ halfClkErr = (uint8_t)((halfClkErr << 1 | BitStream[i]) & 0xFF);
+ if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) {
+ errCnt++;
+ BitStream[bitCnt++] = 7;
+ continue;
+ }
+ BitStream[bitCnt++] = BitStream[i] ^ BitStream[i+1] ^ invert;
+
+ if (bitCnt > MaxBits) break;
+ }
+ *size = bitCnt;
+ return errCnt;
+}
+
//by marshmellow
//take 01 or 10 = 1 and 11 or 00 = 0
//check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010
// by marshmellow - demodulate NRZ wave - requires a read with strong signal
// peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak
-int nrzRawDemod_ext(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) {
+int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) {
if (justNoise(dest, *size)) return -1;
- *clk = DetectNRZClock(dest, *size, *clk);
+ size_t clkStartIdx = 0;
+ *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx);
if (*clk==0) return -2;
size_t i, gLen = 4096;
if (gLen>*size) gLen = *size-20;
*size = numBits;
return 0;
}
-int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert) {
- int startIdx = 0;
- return nrzRawDemod_ext(dest, size, clk, invert, &startIdx);
-}
//translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq])
size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) {
if (fchigh==0) fchigh=10;
if (fclow==0) fclow=8;
//set the threshold close to 0 (graph) or 128 std to avoid static
- uint8_t threshold_value = 123;
size_t preLastSample = 0;
size_t LastSample = 0;
size_t currSample = 0;
if ( size < 1024 ) return 0; // not enough samples
//find start of modulating data in trace
- idx = findModStart(dest, size, threshold_value, fchigh);
+ idx = findModStart(dest, size, fchigh);
// Need to threshold first sample
- if(dest[idx] < threshold_value) dest[0] = 0;
+ if(dest[idx] < FSK_PSK_THRESHOLD) dest[0] = 0;
else dest[0] = 1;
last_transition = idx;
// (could also be fc/5 && fc/7 for fsk1 = 4-9)
for(; idx < size; idx++) {
// threshold current value
- if (dest[idx] < threshold_value) dest[idx] = 0;
+ if (dest[idx] < FSK_PSK_THRESHOLD) dest[idx] = 0;
else dest[idx] = 1;
// Check for 0->1 transition
//by marshmellow - demodulate PSK1 wave
//uses wave lengths (# Samples)
int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *startIdx) {
- if (size == 0) return -1;
- uint16_t loopCnt = 4096; //don't need to loop through entire array...
- if (*size<loopCnt) loopCnt = *size;
+ if (*size < 170) return -1;
- size_t numBits=0;
uint8_t curPhase = *invert;
- size_t i=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
- uint16_t fc=0, fullWaveLen=0, tol=1;
- uint16_t errCnt=0, waveLenCnt=0, errCnt2=0;
- fc = countFC(dest, *size, 1);
- uint8_t fc2 = fc >> 8;
- if (fc2 == 10) return -1; //fsk found - quit
- fc = fc & 0xFF;
- if (fc!=2 && fc!=4 && fc!=8) return -1;
- //PrintAndLog("DEBUG: FC: %d",fc);
- *clock = DetectPSKClock(dest, *size, *clock);
+ uint8_t fc=0;
+ size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
+ uint16_t fullWaveLen=0, waveLenCnt=0, avgWaveVal;
+ uint16_t errCnt=0, errCnt2=0;
+
+ *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc);
if (*clock == 0) return -1;
-
- //find start of modulating data in trace
- uint8_t threshold_value = 123; //-5
- i = findModStart(dest, *size, threshold_value, fc);
-
- //find first phase shift
- int avgWaveVal=0, lastAvgWaveVal=0;
- waveStart = i;
- for (; i<loopCnt; i++) {
- // find peak
- if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
- waveEnd = i+1;
- if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u",waveEnd, waveStart);
- waveLenCnt = waveEnd-waveStart;
- if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+3)){ //not first peak and is a large wave but not out of whack
- lastAvgWaveVal = avgWaveVal/(waveLenCnt);
- firstFullWave = waveStart;
- fullWaveLen=waveLenCnt;
- //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting)
- if (lastAvgWaveVal > threshold_value) curPhase ^= 1;
- break;
- }
-
- waveStart = i+1;
- avgWaveVal = 0;
- }
- avgWaveVal += dest[i+2];
- }
+ //if clock detect found firstfullwave...
+ uint16_t tol = fc/2;
if (firstFullWave == 0) {
- // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
- // so skip a little to ensure we are past any Start Signal
- firstFullWave = 160;
- memset(dest, curPhase, firstFullWave / *clock);
+ //find start of modulating data in trace
+ i = findModStart(dest, *size, fc);
+ //find first phase shift
+ firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen);
+ if (firstFullWave == 0) {
+ // no phase shift detected - could be all 1's or 0's - doesn't matter where we start
+ // so skip a little to ensure we are past any Start Signal
+ firstFullWave = 160;
+ memset(dest, curPhase, firstFullWave / *clock);
+ } else {
+ memset(dest, curPhase^1, firstFullWave / *clock);
+ }
} else {
memset(dest, curPhase^1, firstFullWave / *clock);
}
if (g_debugMode==2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit,(unsigned int) fc);
waveStart = 0;
dest[numBits++] = curPhase; //set first read bit
- for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){
+ for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++) {
//top edge of wave = start of new wave
- if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
+ if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]) {
if (waveStart == 0) {
waveStart = i+1;
waveLenCnt = 0;
} else { //waveEnd
waveEnd = i+1;
waveLenCnt = waveEnd-waveStart;
- lastAvgWaveVal = avgWaveVal/waveLenCnt;
- if (waveLenCnt > fc){
- //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
+ if (waveLenCnt > fc) {
//this wave is a phase shift
//PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc);
- if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit
+ if (i+1 >= lastClkBit + *clock - tol) { //should be a clock bit
curPhase ^= 1;
dest[numBits++] = curPhase;
lastClkBit += *clock;
- } else if (i < lastClkBit+10+fc){
+ } else if (i < lastClkBit+10+fc) {
//noise after a phase shift - ignore
} else { //phase shift before supposed to based on clock
errCnt++;
dest[numBits++] = 7;
}
- } else if (i+1 > lastClkBit + *clock + tol + fc){
+ } else if (i+1 > lastClkBit + *clock + tol + fc) {
lastClkBit += *clock; //no phase shift but clock bit
dest[numBits++] = curPhase;
} else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often)
errCnt2++;
if(errCnt2 > 101) return errCnt2;
+ avgWaveVal += dest[i+1];
+ continue;
}
avgWaveVal = 0;
waveStart = i+1;
*hi = (bytebits_to_byte(BitStream, 24));
*lo = ((uint64_t)(bytebits_to_byte(BitStream + 24, 32)) << 32) | (bytebits_to_byte(BitStream + 24 + 32, 32));
} else {
+ if (g_debugMode) prnt("Error removing parity: %u", *size);
return 0;
}
return 1;
uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
if (errChk == 0) return -2; //preamble not found
+ if (*size != 128) return -3; //wrong size for fdxb
+ //return start position
return (int)startIdx;
}
}
if (*size != 64 && *size != 224) return -2;
if (*invert==1)
- for (size_t i = startidx; i < *size; i++)
+ for (size_t i = startidx; i < *size + startidx; i++)
bitStream[i] ^= 1;
return (int) startidx;
return (int) startIdx;
}
-
// by iceman
// find Visa2000 preamble in already demoded data
int Visa2kDemod_AM(uint8_t *dest, size_t *size) {
projects / proxmark3-svn / blobdiff