+ return;
+}
+
+//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 fullWaveLen=0, waveLenCnt=0, avgWaveVal;
+ uint16_t errCnt=0, errCnt2=0;
+
+ *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc);
+ if (*clock == 0) return -1;
+ //if clock detect found firstfullwave...
+ uint16_t tol = fc/2;
+ if (firstFullWave == 0) {
+ //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);
+ }
+ //advance bits
+ numBits += (firstFullWave / *clock);
+ *startIdx = firstFullWave - (*clock * numBits)+2;
+ //set start of wave as clock align
+ lastClkBit = firstFullWave;
+ if (g_debugMode==2) prnt("DEBUG PSK: firstFullWave: %u, waveLen: %u, startIdx %i",firstFullWave,fullWaveLen, *startIdx);
+ 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++) {
+ //top edge of wave = start of new wave
+ if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]) {
+ if (waveStart == 0) {
+ waveStart = i+1;
+ waveLenCnt = 0;
+ avgWaveVal = dest[i+1];
+ } else { //waveEnd
+ waveEnd = i+1;
+ waveLenCnt = waveEnd-waveStart;
+ 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
+ curPhase ^= 1;
+ dest[numBits++] = curPhase;
+ lastClkBit += *clock;
+ } 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) {
+ 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;
+ }
+ }
+ avgWaveVal += dest[i+1];
+ }
+ *size = numBits;
+ return errCnt;
+}
+
+int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) {
+ int startIdx = 0;
+ return pskRawDemod_ext(dest, size, clock, invert, &startIdx);
+}
+
+//**********************************************************************************************
+//-----------------Tag format detection section-------------------------------------------------
+//**********************************************************************************************
+
+// by marshmellow
+// FSK Demod then try to locate an AWID ID
+int AWIDdemodFSK(uint8_t *dest, size_t *size) {
+ //make sure buffer has enough data
+ if (*size < 96*50) return -1;
+
+ if (justNoise(dest, *size)) return -2;
+
+ // FSK demodulator
+ *size = fskdemod(dest, *size, 50, 1, 10, 8); // fsk2a RF/50
+ if (*size < 96) return -3; //did we get a good demod?
+
+ uint8_t preamble[] = {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
+ if (*size != 96) return -5;
+ return (int)startIdx;
+}
+
+//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)
+{
+ //sanity checks
+ if (*size < 64) return 0;
+ if (BitStream[1]>1) return 0; //allow only 1s and 0s
+
+ // 111111111 bit pattern represent start of frame
+ // include 0 in front to help get start pos
+ uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1};
+ uint8_t errChk = 0;
+ uint8_t FmtLen = 10; // sets of 4 bits = end data
+ *startIdx = 0;
+ errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, startIdx);
+ if ( errChk == 0 || (*size != 64 && *size != 128) ) return 0;
+ if (*size == 128) FmtLen = 22; // 22 sets of 4 bits
+
+ //skip last 4bit parity row for simplicity
+ *size = removeParity(BitStream, *startIdx + sizeof(preamble), 5, 0, FmtLen * 5);
+ if (*size == 40) { // std em410x format
+ *hi = 0;
+ *lo = ((uint64_t)(bytebits_to_byte(BitStream, 8)) << 32) | (bytebits_to_byte(BitStream + 8, 32));
+ } else if (*size == 88) { // long em format
+ *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;
+}
+
+// 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;
+
+ 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 size for fdxb
+ //return start position
+ return (int)startIdx;
+}
+
+// by marshmellow
+// demod gProxIIDemod
+// error returns as -x
+// success returns start position in BitStream
+// BitStream must contain previously askrawdemod and biphasedemoded data
+int gProxII_Demod(uint8_t BitStream[], size_t *size) {
+ size_t startIdx=0;
+ uint8_t preamble[] = {1,1,1,1,1,0};
+
+ uint8_t errChk = preambleSearch(BitStream, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -3; //preamble not found
+ if (*size != 96) return -2; //should have found 96 bits
+ //check first 6 spacer bits to verify format
+ if (!BitStream[startIdx+5] && !BitStream[startIdx+10] && !BitStream[startIdx+15] && !BitStream[startIdx+20] && !BitStream[startIdx+25] && !BitStream[startIdx+30]){
+ //confirmed proper separator bits found
+ //return start position
+ return (int) startIdx;
+ }
+ return -5; //spacer bits not found - not a valid gproxII
+}
+
+// loop to get raw HID waveform then FSK demodulate the TAG ID from it
+int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo) {
+ if (justNoise(dest, *size)) return -1;
+
+ 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;
+ // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1
+ uint8_t preamble[] = {0,0,0,1,1,1,0,1};
+ // find bitstring in array
+ uint8_t errChk = preambleSearch(dest, preamble, sizeof(preamble), size, &startIdx);
+ if (errChk == 0) return -3; //preamble not found
+
+ numStart = startIdx + sizeof(preamble);
+ // final loop, go over previously decoded FSK data and manchester decode into usable tag ID
+ for (size_t idx = numStart; (idx-numStart) < *size - sizeof(preamble); idx+=2){
+ if (dest[idx] == dest[idx+1]){
+ return -4; //not manchester data
+ }
+ *hi2 = (*hi2<<1)|(*hi>>31);
+ *hi = (*hi<<1)|(*lo>>31);
+ //Then, shift in a 0 or one into low
+ if (dest[idx] && !dest[idx+1]) // 1 0
+ *lo=(*lo<<1)|1;
+ else // 0 1
+ *lo=(*lo<<1)|0;
+ }
+ return (int)startIdx;
projects / proxmark3-svn / blobdiff