#define prnt dummy
#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;
- }
- return justNoise1;
+//test samples are not just noise
+uint8_t justNoise(uint8_t *bits, size_t size) {
+ #define THRESHOLD 123
+ uint8_t val = 1;
+ for(size_t idx=0; idx < size && val ;idx++)
+ val = bits[idx] < THRESHOLD;
+ return val;
}
//by marshmellow
{
uint32_t parityWd = 0;
size_t j = 0, bitCnt = 0;
- for (int word = 0; word < (bLen); word+=pLen){
+ for (int word = 0; word < (bLen); word += pLen){
for (int bit=0; bit < pLen; bit++){
parityWd = (parityWd << 1) | BitStream[startIdx+word+bit];
BitStream[j++] = (BitStream[startIdx+word+bit]);
j--; // 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
- default: //test parity
- if (parityTest(parityWd, pLen, pType) == 0) return 0; break;
+ 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
+ default: if (parityTest(parityWd, pLen, pType) == 0) { return 0; } break; //test parity
}
- bitCnt+=(pLen-1);
+ bitCnt += (pLen-1);
parityWd = 0;
}
// if we got here then all the parities passed
//search for given preamble in given BitStream and return success=1 or fail=0 and startIndex and length
uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx)
{
- uint8_t foundCnt=0;
- for (int idx=0; idx < *size - pLen; idx++){
+ // Sanity check. If preamble length is bigger than bitstream length.
+ if ( *size <= pLen ) return 0;
+
+ uint8_t foundCnt = 0;
+ for (int idx = 0; idx < *size - pLen; idx++){
if (memcmp(BitStream+idx, preamble, pLen) == 0){
//first index found
foundCnt++;
//by marshmellow
//takes 1s and 0s and searches for EM410x format - output EM ID
-uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo)
+int Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_t *hi, uint64_t *lo)
{
//no arguments needed - built this way in case we want this to be a direct call from "data " cmds in the future
// otherwise could be a void with no arguments
//set defaults
uint32_t i = 0;
- if (BitStream[1]>1) return 0; //allow only 1s and 0s
+ if (BitStream[1]>1) return -1; //allow only 1s and 0s
// 111111111 bit pattern represent start of frame
// include 0 in front to help get start pos
uint8_t FmtLen = 10;
*startIdx = 0;
errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, startIdx);
- if (errChk == 0 || *size < 64) return 0;
+ if (errChk == 0 ) return -4;
+ if (*size < 64) return -3;
if (*size > 64) FmtLen = 22;
*startIdx += 1; //get rid of 0 from preamble
idx = *startIdx + 9;
for (i=0; i<FmtLen; i++){ //loop through 10 or 22 sets of 5 bits (50-10p = 40 bits or 88 bits)
parityBits = bytebits_to_byte(BitStream+(i*5)+idx,5);
//check even parity - quit if failed
- if (parityTest(parityBits, 5, 0) == 0) return 0;
+ if (parityTest(parityBits, 5, 0) == 0) return -5;
//set uint64 with ID from BitStream
for (uint8_t ii=0; ii<4; ii++){
*hi = (*hi << 1) | (*lo >> 63);
smplCnt++;
} else { //transition
if ((BinStream[i] >= high && !waveHigh) || (BinStream[i] <= low && waveHigh)){
+
if (smplCnt > clk-(clk/4)-1) { //full clock
- if (smplCnt > clk + (clk/4)+1) { //too many samples
+ if (smplCnt > clk + (clk/4)+2) { //too many samples
errCnt++;
if (g_debugMode==2) prnt("DEBUG ASK: Modulation Error at: %u", i);
- BinStream[bitCnt++]=7;
+ BinStream[bitCnt++] = 7;
} else if (waveHigh) {
BinStream[bitCnt++] = invert;
BinStream[bitCnt++] = invert;
//by marshmellow
void askAmp(uint8_t *BitStream, size_t size)
{
- for(size_t i = 1; i<size; i++){
- if (BitStream[i]-BitStream[i-1]>=30) //large jump up
- BitStream[i]=255;
- else if(BitStream[i]-BitStream[i-1]<=-20) //large jump down
- BitStream[i]=0;
+ uint8_t last = 128;
+ for(size_t i = 1; i < size; ++i){
+ if (BitStream[i]-BitStream[i-1] >= 30) //large jump up
+ last = 255;
+ else if(BitStream[i-1] - BitStream[i] >= 20) //large jump down
+ last = 0;
+
+ BitStream[i] = last;
}
- return;
}
//by marshmellow
if (*clk==0 || start < 0) return -3;
if (*invert != 1) *invert = 0;
if (amp==1) askAmp(BinStream, *size);
- if (g_debugMode==2) prnt("DEBUG ASK: clk %d, beststart %d", *clk, start);
+ if (g_debugMode==2) prnt("DEBUG ASK: size %d, clk %d, beststart %d", *size, *clk, start);
uint8_t initLoopMax = 255;
if (initLoopMax > *size) initLoopMax = *size;
errCnt = cleanAskRawDemod(BinStream, size, *clk, *invert, high, low);
if (askType) //askman
return manrawdecode(BinStream, size, 0);
- else //askraw
- return errCnt;
+ //askraw
+ return errCnt;
}
if (g_debugMode==2) prnt("DEBUG ASK: Weak Wave Detected - using weak wave demod");
*size = bitnum;
return errCnt;
}
-
//by marshmellow
//take 10 and 01 and manchester decode
//run through 2 times and take least errCnt
-int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert)
-{
- uint16_t bitnum=0, MaxBits = 512, errCnt = 0;
- size_t i, ii;
+int manrawdecode(uint8_t * BitStream, size_t *size, uint8_t invert){
+ uint16_t bitnum = 0, MaxBits = 512, errCnt = 0;
+ size_t i, k;
uint16_t bestErr = 1000, bestRun = 0;
if (*size < 16) return -1;
//find correct start position [alignment]
- for (ii=0;ii<2;++ii){
- for (i=ii; i<*size-3; i+=2)
- if (BitStream[i]==BitStream[i+1])
+ for (k=0; k < 2; ++k){
+ for (i=k; i<*size-3; i += 2)
+ if (BitStream[i] == BitStream[i+1])
errCnt++;
- if (bestErr>errCnt){
- bestErr=errCnt;
- bestRun=ii;
+ if (bestErr > errCnt){
+ bestErr = errCnt;
+ bestRun = k;
}
errCnt=0;
}
//decode
- for (i=bestRun; i < *size-3; i+=2){
- if(BitStream[i] == 1 && (BitStream[i+1] == 0)){
- BitStream[bitnum++]=invert;
- } else if((BitStream[i] == 0) && BitStream[i+1] == 1){
- BitStream[bitnum++]=invert^1;
+ for (i=bestRun; i < *size-3; i += 2){
+ if (BitStream[i] == 1 && (BitStream[i+1] == 0)){
+ BitStream[bitnum++] = invert;
+ } else if ((BitStream[i] == 0) && BitStream[i+1] == 1){
+ BitStream[bitnum++] = invert^1;
} else {
- BitStream[bitnum++]=7;
+ BitStream[bitnum++] = 7;
}
- if(bitnum>MaxBits) break;
+ if (bitnum>MaxBits) break;
}
*size=bitnum;
return bestErr;
{
if (justNoise(dest, *size)) return -1;
- size_t numStart=0, size2=*size, startIdx=0;
+ size_t numStart=0, size2 = *size, startIdx=0;
// FSK demodulator
*size = fskdemod(dest, size2,50,1,10,8); //fsk2a
if (*size < 96*2) return -2;
*hi2 = (*hi2<<1)|(*hi>>31);
*hi = (*hi<<1)|(*lo>>31);
//Then, shift in a 0 or one into low
+ *lo <<= 1;
if (dest[idx] && !dest[idx+1]) // 1 0
- *lo=(*lo<<1)|1;
+ *lo |= 1;
else // 0 1
- *lo=(*lo<<1)|0;
+ *lo |= 0;
}
return (int)startIdx;
}
{
if (justNoise(dest, *size)) return -1;
- size_t numStart=0, size2=*size, startIdx=0;
+ size_t numStart=0, size2 = *size, startIdx=0;
// FSK demodulator
*size = fskdemod(dest, size2,50,1,10,8); //fsk2a
if (*size < 96) return -2;
int VikingDemod_AM(uint8_t *dest, size_t *size) {
//make sure buffer has data
if (*size < 64*2) return -2;
-
size_t startIdx = 0;
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};
uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
if ( checkCalc != 0xA8 ) return -5;
if (*size != 64) return -6;
//return start position
- return (int) startIdx;
+ return (int)startIdx;
}
+// by iceman
+// find Visa2000 preamble in already demoded data
+int Visa2kDemod_AM(uint8_t *dest, size_t *size) {
+ if (*size < 96*2) return -1; //make sure buffer has data
+ size_t startIdx = 0;
+ 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};
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -2; //preamble not found
+ if (*size != 96) return -3; //wrong demoded size
+ //return start position
+ return (int)startIdx;
+}
+// by iceman
+// find Noralsy preamble in already demoded data
+int NoralsyDemod_AM(uint8_t *dest, size_t *size) {
+ if (*size < 96*2) return -1; //make sure buffer has data
+ size_t startIdx = 0;
+ uint8_t preamble[] = {1,0,1,1,1,0,1,1,0,0,0,0};
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -2; //preamble not found
+ if (*size != 96) return -3; //wrong demoded size
+ //return start position
+ return (int)startIdx;
+}
// find presco preamble 0x10D in already demoded data
int PrescoDemod(uint8_t *dest, size_t *size) {
- //make sure buffer has data
- if (*size < 64*2) return -2;
-
+ if (*size < 128*2) return -1; //make sure buffer has data
size_t startIdx = 0;
- uint8_t preamble[] = {1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0};
+ uint8_t preamble[] = {0,0,0,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0};
uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
- if (errChk == 0) return -4; //preamble not found
+ if (errChk == 0) return -2; //preamble not found
+ if (*size != 128) return -3; //wrong demoded size
//return start position
- return (int) startIdx;
+ return (int)startIdx;
}
// Ask/Biphase Demod then try to locate an ISO 11784/85 ID
// BitStream must contain previously askrawdemod and biphasedemoded data
-int FDXBdemodBI(uint8_t *dest, size_t *size)
-{
- //make sure buffer has enough data
- if (*size < 128) return -1;
-
+int FDXBdemodBI(uint8_t *dest, size_t *size) {
+ if (*size < 128*2) return -1; //make sure buffer has enough data
size_t startIdx = 0;
uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,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 demoded size
+ //return start position
+ return (int)startIdx;
+}
+// ASK/Diphase fc/64 (inverted Biphase)
+// Note: this i s not a demod, this is only a detection
+// the parameter *dest needs to be demoded before call
+// 0xFFFF preamble, 64bits
+int JablotronDemod(uint8_t *dest, size_t *size){
+ if (*size < 64*2) return -1; //make sure buffer has enough data
+ size_t startIdx = 0;
+ uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0};
uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
if (errChk == 0) return -2; //preamble not found
+ if (*size != 64) return -3; // wrong demoded size
+
+ uint8_t checkchksum = 0;
+ for (int i=16; i < 56; i += 8) {
+ checkchksum += bytebits_to_byte(dest+startIdx+i,8);
+ }
+ checkchksum ^= 0x3A;
+ uint8_t crc = bytebits_to_byte(dest+startIdx+56, 8);
+ if ( checkchksum != crc ) return -5;
return (int)startIdx;
}
*size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
if (*size < 128) return -2; //did we get a good demod?
- uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
+ uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1};
size_t startIdx = 0;
uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
if (errChk == 0) return -4; //preamble not found
return (int)startIdx;
}
-// find presco preamble 0x10D in already demoded data
+// find nedap preamble in already demoded data
int NedapDemod(uint8_t *dest, size_t *size) {
//make sure buffer has data
if (*size < 128) return -3;
size_t startIdx = 0;
+ //uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,0,0,0,1};
uint8_t preamble[] = {1,1,1,1,1,1,1,1,1,0};
uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
if (errChk == 0) return -4; //preamble not found
- //return start position
+ return (int) startIdx;
+}
+
+// Find IDTEC PSK1, RF Preamble == 0x4944544B, Demodsize 64bits
+// by iceman
+int IdteckDemodPSK(uint8_t *dest, size_t *size) {
+ //make sure buffer has data
+ if (*size < 64*2) return -1;
+ size_t startIdx = 0;
+ uint8_t preamble[] = {0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,0,0,1,0,0,1,0,1,1};
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -2; //preamble not found
+ if (*size != 64) return -3; // wrong demoded size
return (int) startIdx;
}
if (clockFnd>0) {
clkCnt = clockFnd;
clkEnd = clockFnd+1;
+ } else {
+ clkCnt=1;
}
- else clkCnt=1;
//test each valid clock from smallest to greatest to see which lines up
- for(; clkCnt < clkEnd; clkCnt++){
- if (clk[clkCnt] <= 32){
+ for(; clkCnt < clkEnd; clkCnt++) {
+ if (clk[clkCnt] <= 32) {
tol=1;
- }else{
+ } else {
tol=0;
}
//if no errors allowed - keep start within the first clock
- if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt]<128) loopCnt=clk[clkCnt]*2;
- bestErr[clkCnt]=1000;
+ if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt]<128)
+ loopCnt = clk[clkCnt] * 2;
+
+ bestErr[clkCnt] = 1000;
+
//try lining up the peaks by moving starting point (try first few clocks)
for (ii=0; ii < loopCnt; ii++){
if (dest[ii] < peak && dest[ii] > low) continue;
- errCnt=0;
+ errCnt = 0;
// now that we have the first one lined up test rest of wave array
loopEnd = ((size-ii-tol) / clk[clkCnt]) - 1;
for (i=0; i < loopEnd; ++i){
}
//if we found no errors then we can stop here and a low clock (common clocks)
// this is correct one - return this clock
- if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d",clk[clkCnt],errCnt,ii,i);
- if(errCnt==0 && clkCnt<7) {
+ if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d", clk[clkCnt], errCnt, ii, i);
+ if (errCnt==0 && clkCnt<7) {
if (!clockFnd) *clock = clk[clkCnt];
return ii;
}
//if we found errors see if it is lowest so far and save it as best run
- if(errCnt<bestErr[clkCnt]){
- bestErr[clkCnt]=errCnt;
- bestStart[clkCnt]=ii;
+ if (errCnt < bestErr[clkCnt]) {
+ bestErr[clkCnt] = errCnt;
+ bestStart[clkCnt] = ii;
}
}
}
- uint8_t iii;
- uint8_t best=0;
- for (iii=1; iii<clkEnd; ++iii){
- if (bestErr[iii] < bestErr[best]){
- if (bestErr[iii] == 0) bestErr[iii]=1;
+ uint8_t k;
+ uint8_t best = 0;
+ for (k=1; k < clkEnd; ++k){
+ if (bestErr[k] < bestErr[best]){
+ if (bestErr[k] == 0) bestErr[k]=1;
// current best bit to error ratio vs new bit to error ratio
- if ( (size/clk[best])/bestErr[best] < (size/clk[iii])/bestErr[iii] ){
- best = iii;
+ if ( (size/clk[best])/bestErr[best] < (size/clk[k])/bestErr[k] ){
+ best = k;
}
}
- 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]);
+ if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d", clk[k], bestErr[k], clk[best], bestStart[best]);
}
if (!clockFnd) *clock = clk[best];
return bestStart[best];
projects / proxmark3-svn / blobdiff