//-----------------------------------------------------------------------------
#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--------------------------------------------
// 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;
}
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) {
//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];
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, 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
//need to loop through all samples and identify our clock, look for the ST pattern
int clk = 0;
int tol = 0;
- int j, high, low, skip, start, end, minClk=255;
+ 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 / 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
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;
}
-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
- if (samples[i]+fc < 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 - demodulate PSK1 wave
//uses wave lengths (# Samples)
int pskRawDemod_ext(uint8_t dest[], size_t *size, int *clock, int *invert, int *startIdx) {
if (*size < 170) return -1;
uint8_t curPhase = *invert;
+ uint8_t fc=0;
size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
- uint16_t fc=0, fullWaveLen=0, waveLenCnt=0, avgWaveVal, tol=1;
+ uint16_t fullWaveLen=0, waveLenCnt=0, avgWaveVal;
uint16_t errCnt=0, errCnt2=0;
- fc = countFC(dest, *size, 1);
- if ((fc >> 8) == 10) return -1; //fsk found - quit
- fc = fc & 0xFF;
- if (fc!=2 && fc!=4 && fc!=8) return -1;
- *clock = DetectPSKClock(dest, *size, *clock);
+ *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc);
if (*clock == 0) return -1;
-
- //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 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;
- if (waveLenCnt > fc){
+ 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;
}
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;
projects / proxmark3-svn / blobdiff