X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/6f36848f9ecb5f6a1131a9bf4def5b3e2e05a415..b3ec0b3d4f5e26a266c70a46d4aaafbfa2110731:/common/lfdemod.c

diff --git a/common/lfdemod.c b/common/lfdemod.c
index 801d651b..c56301c0 100644
--- a/common/lfdemod.c
+++ b/common/lfdemod.c
@@ -35,12 +35,14 @@
 #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, ...){}
 #ifndef ON_DEVICE
@@ -81,40 +83,35 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t fuzzHi
 // 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;
 }
 
@@ -307,15 +304,17 @@ uint32_t manchesterEncode2Bytes(uint16_t datain) {
 
 //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;
@@ -1684,6 +1683,7 @@ uint8_t Em410xDecode(uint8_t *BitStream, size_t *size, size_t *startIdx, uint32_
 		*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;
@@ -1700,6 +1700,8 @@ int FDXBdemodBI(uint8_t *dest, size_t *size) {
 
 	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;
 }