X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/c85858f5247daf444cf2db070ba54df4f646d9f8..4c6ccc2b042d4c6821d579f2d9ee967cfd2cde18:/common/lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index a32d3b85..fbed6a9a 100644
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -170,6 +170,23 @@ uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_
 	return 0;
 }
 
+// search for given preamble in given BitStream and return success=1 or fail=0 and startIndex (where it was found)
+// does not look for a repeating preamble
+// em4x05/4x69 only sends preamble once, so look for it once in the first pLen bits
+// leave it generic so it could be reused later...
+bool onePreambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t size, size_t *startIdx) {
+	// Sanity check.  If preamble length is bigger than bitstream length.
+	if ( size <= pLen ) return false;
+	for (size_t idx = 0; idx < size - pLen; idx++) {
+		if (memcmp(BitStream+idx, preamble, pLen) == 0) {
+			if (g_debugMode) prnt("DEBUG: preamble found at %u", idx);
+			*startIdx = idx;
+			return true;
+		}
+	}
+	return false;
+}
+
 //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)
@@ -493,16 +510,16 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow
 	// in case you have junk or noise at the beginning of the trace...
 	uint8_t thresholdCnt = 0;
 	size_t waveSizeCnt = 0;
-	bool isAboveThreshold = dest[idx] >= threshold_value;
+	bool isAboveThreshold = dest[idx++] >= threshold_value;
 	for (; idx < size-20; idx++ ) {
 		if(dest[idx] < threshold_value && isAboveThreshold) {
 			thresholdCnt++;
-			if (thresholdCnt > 4 && waveSizeCnt < fchigh+1) break;			
+			if (thresholdCnt > 2 && waveSizeCnt < fchigh+1) break;			
 			isAboveThreshold = false;
 			waveSizeCnt = 0;
 		} else if (dest[idx] >= threshold_value && !isAboveThreshold) {
 			thresholdCnt++;
-			if (thresholdCnt > 4 && waveSizeCnt < fchigh+1) break;			
+			if (thresholdCnt > 2 && waveSizeCnt < fchigh+1) break;			
 			isAboveThreshold = true;
 			waveSizeCnt = 0;
 		} else {
@@ -1491,7 +1508,7 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
 
 	size_t numBits=0;
 	uint8_t curPhase = *invert;
-	size_t i, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
+	size_t i=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0;
 	uint8_t fc=0, fullWaveLen=0, tol=1;
 	uint16_t errCnt=0, waveLenCnt=0;
 	fc = countFC(dest, *size, 0);
@@ -1499,19 +1516,45 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert)
 	//PrintAndLog("DEBUG: FC: %d",fc);
 	*clock = DetectPSKClock(dest, *size, *clock);
 	if (*clock == 0) return -1;
+	// jump to modulating data by finding the first 2 threshold crossings (or first 1 waves)
+	// in case you have junk or noise at the beginning of the trace...
+	uint8_t thresholdCnt = 0;
+	size_t waveSizeCnt = 0;
+	uint8_t threshold_value = 123; //-5
+	bool isAboveThreshold = dest[i++] >= threshold_value;
+	for (; i < *size-20; i++ ) {
+		if(dest[i] < threshold_value && isAboveThreshold) {
+			thresholdCnt++;
+			if (thresholdCnt > 2 && waveSizeCnt < fc+1) break;			
+			isAboveThreshold = false;
+			waveSizeCnt = 0;
+		} else if (dest[i] >= threshold_value && !isAboveThreshold) {
+			thresholdCnt++;
+			if (thresholdCnt > 2 && waveSizeCnt < fc+1) break;			
+			isAboveThreshold = true;
+			waveSizeCnt = 0;
+		} else {
+			waveSizeCnt++;
+		}
+		if (thresholdCnt > 10) break;
+	}
+	if (g_debugMode == 2) prnt("DEBUG PSK: threshold Count reached at %u, count: %u",i, thresholdCnt);
+
+
 	int avgWaveVal=0, lastAvgWaveVal=0;
+	waveStart = i+1;
 	//find first phase shift
-	for (i=0; i<loopCnt; i++){
+	for (; i<loopCnt; i++){
 		if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){
 			waveEnd = i+1;
-			//PrintAndLog("DEBUG: waveEnd: %d",waveEnd);
+			if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u",waveEnd, waveStart);
 			waveLenCnt = waveEnd-waveStart;
-			if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+2)){ //not first peak and is a large wave but not out of whack
+			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
-				if (lastAvgWaveVal > 123) curPhase ^= 1;  //fudge graph 0 a little 123 vs 128
+				if (lastAvgWaveVal > threshold_value) curPhase ^= 1;  //fudge graph 0 a little 123 vs 128
 				break;
 			} 
 			waveStart = i+1;