#include <string.h> // for memset, memcmp and size_t
#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;
}
-void getNextLow(uint8_t samples[], size_t size, int low, int *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, int *i) {
+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) {
- int i=0, start, waveStart;
+ size_t i=0, firstLow, firstHigh;
size_t testsize = (size < 512) ? size : 512;
if ( getHiLo(samples, testsize, high, low, 80, 80) == -1 ) {
// populate tmpbuff buffer with pulse lengths
while (i < size) {
// measure from low to low
- getNextLow(samples, size, *low, &i);
- start = i;
-
+ firstLow = i;
//find first high point for this wave
getNextHigh(samples, size, *high, &i);
- waveStart = i;
+ firstHigh = i;
getNextLow(samples, size, *low, &i);
- if (*waveCnt >= (size/32))
+ if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK))
break;
- highToLowWaveLen[*waveCnt] = i - waveStart; //first high to first low
- lowToLowWaveLen[*waveCnt] = i - start;
+ highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low
+ lowToLowWaveLen[*waveCnt] = i - firstLow;
*waveCnt += 1;
- if (i-start < *minClk && i < size) {
- *minClk = i - start;
+ if (i-firstLow < *minClk && i < size) {
+ *minClk = i - firstLow;
}
}
return true;
//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...
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
//**********************************************************************************************
// 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) {
+bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) {
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
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=0, j, skip, start, end, low, high, minClk=255;
+ int j, high, low, skip, start, end, 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...
+ 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;
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;
+
+ 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");
// 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
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 == 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;
+ 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 errCnt=0, errCnt2=0;
+
fc = countFC(dest, *size, 1);
- uint8_t fc2 = fc >> 8;
- if (fc2 == 10) return -1; //fsk found - quit
+ if ((fc >> 8) == 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);
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);
+ i = findModStart(dest, *size, 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];
- }
+ 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
} else { //waveEnd
waveEnd = i+1;
waveLenCnt = waveEnd-waveStart;
- lastAvgWaveVal = avgWaveVal/waveLenCnt;
if (waveLenCnt > fc){
- //PrintAndLog("DEBUG: avgWaveVal: %d, waveSum: %d",lastAvgWaveVal,avgWaveVal);
//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
*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;
}
projects / proxmark3-svn / blobdiff