fixed ISO 14443A anticollision

git-svn-id: https://svn.gnumonks.org/trunk/librfid@1988 e0336214-984f-0b4b-a45f-81c69e1f0ede

2 files changed, 49 insertions, 23 deletions

diff --git a/src/rfid_asic_rc632.c b/src/rfid_asic_rc632.c
index 0d6824c..0560d09 100644
--- a/src/rfid_asic_rc632.c
+++ b/src/rfid_asic_rc632.c
@@ -875,7 +875,7 @@ rc632_iso14443a_transceive_acf(struct rfid_asic_handle *handle,
 	int ret;
 	u_int8_t rx_buf[64];
 	u_int8_t rx_len = sizeof(rx_buf);
-	u_int8_t rx_align = 0, tx_last_bits, tx_bytes;
+	u_int8_t rx_align = 0, tx_last_bits, tx_bytes, tx_bytes_total;
 	u_int8_t boc;
 	u_int8_t error_flag;
 	*bit_of_col = ISO14443A_BITOFCOL_NONE;
@@ -899,14 +899,14 @@ rc632_iso14443a_transceive_acf(struct rfid_asic_handle *handle,
 	if (ret < 0)
 		return ret;
 
-	tx_last_bits = acf->nvb & 0x0f;	/* lower nibble indicates bits */
-	tx_bytes = acf->nvb >> 4;
+	tx_last_bits = acf->nvb & 0x07;	/* lower nibble indicates bits */
+	tx_bytes = ( acf->nvb >> 4 ) & 0x07;
 	if (tx_last_bits) {
-		tx_bytes++;
-		rx_align = (tx_last_bits+1) % 8;/* rx frame complements tx */
+		tx_bytes_total = tx_bytes+1;
+		rx_align = tx_last_bits & 0x07; /* rx frame complements tx */
 	}
-
-	//rx_align = 8 - tx_last_bits;/* rx frame complements tx */
+	else
+		tx_bytes_total = tx_bytes;
 
 	/* set RxAlign and TxLastBits*/
 	ret = rc632_reg_write(handle, RC632_REG_BIT_FRAMING,
@@ -914,7 +914,7 @@ rc632_iso14443a_transceive_acf(struct rfid_asic_handle *handle,
 	if (ret < 0)
 		return ret;
 
-	ret = rc632_transceive(handle, (u_int8_t *)acf, tx_bytes,
+	ret = rc632_transceive(handle, (u_int8_t *)acf, tx_bytes_total,
 				rx_buf, &rx_len, 0x32, 0);
 	if (ret < 0)
 		return ret;
@@ -923,8 +923,10 @@ rc632_iso14443a_transceive_acf(struct rfid_asic_handle *handle,
 	acf->uid_bits[tx_bytes-2] = (
 		  (acf->uid_bits[tx_bytes-2] & (0xff >> (8-tx_last_bits)))
 		| rx_buf[0]);
+	
 	/* copy the rest */
-	memcpy(&acf->uid_bits[tx_bytes+1-2], &rx_buf[1], rx_len-1);
+	if(rx_len)
+	    memcpy(&acf->uid_bits[tx_bytes-1], &rx_buf[1], rx_len-1);
 
 	/* determine whether there was a collission */
 	ret = rc632_reg_read(handle, RC632_REG_ERROR_FLAG, &error_flag);
diff --git a/src/rfid_layer2_iso14443a.c b/src/rfid_layer2_iso14443a.c
index 4716018..2c4cb1e 100644
--- a/src/rfid_layer2_iso14443a.c
+++ b/src/rfid_layer2_iso14443a.c
@@ -23,6 +23,8 @@
 #include <unistd.h>
 #include <string.h>
 #include <errno.h>
+ 
+// #define DEBUG_LIBRFID
 
 #include <librfid/rfid.h>
 #include <librfid/rfid_layer2.h>
@@ -32,6 +34,8 @@
 
 #define TIMEOUT 1236
 
+unsigned long randctx[4]={0x22d4a017,0x773a1f44,0xc39e1460,0x9cde8801};
+
 /* Transceive a 7-bit short frame */
 int
 iso14443a_transceive_sf(struct rfid_layer2_handle *handle,
@@ -80,20 +84,38 @@ iso14443a_code_nvb_bits(unsigned char *nvb, unsigned int bits)
 	return 0;
 }
 
+int random_bit(void)
+{
+    unsigned long e;
+
+    e = randctx[0];    
+    randctx[0]=randctx[1];
+    randctx[1]=(randctx[2]<<19) + (randctx[2]>>13) + randctx[3];
+    randctx[2]=randctx[3] ^ randctx[0];
+    randctx[3]=e+randctx[1];
+    
+    return randctx[1]&1;
+}
+
 /* first bit is '1', second bit '2' */
 static void
-set_bit_in_field(unsigned char *bitfield, unsigned int bit)
+rnd_toggle_bit_in_field(unsigned char *bitfield, unsigned int size, unsigned int bit)
 {
-	unsigned int byte_count = bit / 8;
-	unsigned int bit_count = bit % 8;
-
-	DEBUGP("bitfield=%p, byte_count=%u, bit_count=%u\n",
-			bitfield, byte_count, bit_count);
-	DEBUGP("%p = 0x%02x\n", (bitfield+byte_count), *(bitfield+byte_count));
-	*(bitfield+byte_count) |= 1 << (bit_count-1);
-	DEBUGP("%p = 0x%02x\n", (bitfield+byte_count), *(bitfield+byte_count));
+    unsigned int byte,rnd;
+
+    if(bit && (bit <= (size*8)) )
+    {
+	rnd=random_bit();
+	
+        DEBUGP("xor'ing bit %u with %u\n",bit,rnd);
+        bit--;
+        byte=bit/8;
+        bit=rnd<<(bit%8);
+        bitfield[byte] ^= bit;
+    }
 }
 
+
 static int
 iso14443a_anticol(struct rfid_layer2_handle *handle)
 {
@@ -122,7 +144,7 @@ iso14443a_anticol(struct rfid_layer2_handle *handle)
 		return ret;
 	}
 	h->state = ISO14443A_STATE_ATQA_RCVD;
-
+	
 	DEBUGP("ATQA: 0x%02x 0x%02x\n", *aqptr, *(aqptr+1));
 
 	if (!atqa->bf_anticol) {
@@ -150,18 +172,20 @@ cascade:
 	ret = iso14443a_transceive_acf(handle, &acf, &bit_of_col);
 	if (ret < 0)
 		return ret;
-	DEBUGP("bit_of_col = %u\n", bit_of_col);
 	
 	while (bit_of_col != ISO14443A_BITOFCOL_NONE) {
-		set_bit_in_field(&acf.uid_bits[0], bit_of_col-16);
+		DEBUGP("collision at pos %u\n", bit_of_col);
+
 		iso14443a_code_nvb_bits(&acf.nvb, bit_of_col);
+		rnd_toggle_bit_in_field(acf.uid_bits, sizeof(acf.uid_bits), bit_of_col);
+		DEBUGP("acf: nvb=0x%02X uid_bits=%s\n",acf.nvb,rfid_hexdump(acf.uid_bits,sizeof(acf.uid_bits)));
 		ret = iso14443a_transceive_acf(handle, &acf, &bit_of_col);
-		DEBUGP("bit_of_col = %u\n", bit_of_col);
 		if (ret < 0)
 			return ret;
 	}
 
 	iso14443a_code_nvb_bits(&acf.nvb, 7*8);
+
 	ret = iso14443a_transceive(handle, RFID_14443A_FRAME_REGULAR,
 				   (unsigned char *)&acf, 7, 
 				   (unsigned char *) &sak, &rx_len,
@@ -325,7 +349,7 @@ iso14443a_init(struct rfid_reader_handle *rh)
 		return NULL;
 
 	memset(h, 0, sizeof(*h));
-
+	
 	h->l2 = &rfid_layer2_iso14443a;
 	h->rh = rh;
 	h->priv.iso14443a.state = ISO14443A_STATE_NONE;