Implement single-tag select

3 files changed, 102 insertions, 24 deletions

diff --git a/libtwn3.c b/libtwn3.c
index e9a70bc..03e56c0 100644
--- a/libtwn3.c
+++ b/libtwn3.c
@@ -37,6 +37,27 @@ typedef struct /*_twn3_trl_t*/ {
 } twn3_trl_t;
 #pragma pack(pop)
 
+static inline uint8_t hex_nibble(char in){
+	if(in <= 0x39){
+		return in & 0x0F;
+	}
+	return (in & ~(0x20)) - 55;
+}
+
+static inline size_t decode_ascii_inplace(uint8_t* data, size_t bytes){
+	size_t u;
+	for(u = 0; bytes > 2 * u; u++){
+		if(!isprint(data[u * 2])){
+			break;
+		}
+		if(!isxdigit(data[u * 2]) || !isxdigit(data[u * 2 + 1])){
+			return -1;
+		}
+		data[u] = (hex_nibble(data[u * 2]) << 4) | hex_nibble(data[u * 2 + 1]);
+	}
+	return u;
+}
+
 int twn3_open(char* device){
 	int fd = open(device, O_RDWR | O_CLOEXEC);
 	struct termios tty;
@@ -247,39 +268,80 @@ int twn3_sync_antenna(int fd, uint8_t flags, uint8_t enable){
 	return (bytes > 2 && data[0] == 'P') ? 0 : -1;
 }
 
-int twn3_sync_select_first(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length);
+int twn3_sync_select_first(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length){
+	char data[30];
+	size_t u = 0;
+	ssize_t bytes = twn3_sync_command(fd, flags, (uint8_t*) "s", 1, (uint8_t*) data, sizeof(data));
+
+	*type = tag_none;
+	if(bytes > 2 && data[0] != 'N'){
+		*type = tag_unknown;
+
+		u = decode_ascii_inplace((uint8_t*) data, bytes - 2);
+
+		//TBD
+		if(u % 2){
+			//type tag prepended
+			*type = data[0];
+			memcpy(serial, data + 1, MIN(*length, u - 1));
+			*length = u - 1;
+		}
+		else{
+			memcpy(serial, data, MIN(*length, u));
+			*length = u;
+		}
+	}
+	return (bytes > 0) ? 0 : -1;
+}
+
 int twn3_sync_list(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length);
 int twn3_sync_select(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length);
-int twn3_sync_auth(int fd, uint8_t flags, uint8_t key_a, uint8_t block, uint8_t key[6]);
-int twn3_sync_auth_stored(int fd, uint8_t flags, uint8_t key_a, uint8_t block, uint8_t key_id);
 
-static inline uint8_t hex_nibble(char in){
-	if(in <= 39){
-		return in & 0x0F;
+int twn3_sync_auth(int fd, uint8_t flags, uint8_t key_a, uint8_t sector, uint8_t key[6]){
+	char data[20];
+	ssize_t bytes = 0;
+
+	snprintf(data, sizeof(data) - 1, "l%02X%02X%02X%02X%02X%02X%02X%02X\r", sector, key_a ? 0xAA : 0xBB,
+			key[0], key[1], key[2], key[3], key[4], key[5]);
+
+	if(sector > 0x3F){
+		return -1;
 	}
-	if(in <= 46){
-		return in - 55;
+
+	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 17, (uint8_t*) data, sizeof(data));
+	return (bytes > 0 && data[0] == 'L') ? 0 : -1;
+}
+
+int twn3_sync_auth_stored(int fd, uint8_t flags, uint8_t key_a, uint8_t sector, uint8_t key_id){
+	char data[10];
+	ssize_t bytes = 0;
+	uint8_t key = (key_a) ? (key_id + 0x10) : (key_id + 0x30);
+
+	snprintf(data, sizeof(data) - 1, "l%02X%02X\r", sector, key);
+
+	if(sector > 0x3F || key_id > 31){
+		return -1;
 	}
-	return in - 87;
+
+	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 6, (uint8_t*) data, sizeof(data));
+	return (bytes > 0 && data[0] == 'L') ? 0 : -1;
 }
 
 int twn3_sync_read(int fd, uint8_t flags, uint8_t block, uint8_t* values, size_t max_len){
 	char data[40];
-	ssize_t bytes;
-	unsigned u;
+	ssize_t bytes = 0;
+	size_t u;
 	snprintf(data, sizeof(data) - 1, "r%02X", block);
 
 	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 3, (uint8_t*) data, sizeof(data));
 	if(bytes > 0){
 		if(bytes > 10){
-			//convert ascii hex representation
-			for(u = 0; u < max_len && bytes > 2 * u; u++){
-				if(!isxdigit(data[u * 2]) || !isxdigit(data[(u * 2) + 1])){
-					return -1;
-				}
+			u = decode_ascii_inplace((uint8_t*) data, bytes - 2);
+			memcpy(values, data, MIN(u, max_len));
 
-				values[u] = (hex_nibble(data[u * 2]) << 4) | hex_nibble(data[(u * 2) + 1]);
-			}
+			DBG(printf("Decoded %ld bytes of data\n", u));
+			DBG(xxd((uint8_t*) values, u));
+			DBG(printf("\n"));
 			return u;
 		}
 		else{
@@ -291,7 +353,7 @@ int twn3_sync_read(int fd, uint8_t flags, uint8_t block, uint8_t* values, size_t
 
 int twn3_sync_write(int fd, uint8_t flags, uint8_t block, uint8_t value[16]){
 	char data[40];
-	ssize_t bytes;
+	ssize_t bytes = 0;
 	snprintf(data, sizeof(data) - 1, "w%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
 			block,
 			value[0], value[1], value[2], value[3],
diff --git a/libtwn3.h b/libtwn3.h
index b6a8b93..91b3406 100644
--- a/libtwn3.h
+++ b/libtwn3.h
@@ -3,6 +3,7 @@
 #include <inttypes.h>
 
 typedef enum /*_twn3_tag_type*/ {
+	tag_none = 0x00,
 	tag_mf_light = 0x01,
 	tag_mf_classic_1k = 0x02,
 	tag_mf_classic_4l = 0x03,
diff --git a/twn3_test.c b/twn3_test.c
index ca10574..734201e 100644
--- a/twn3_test.c
+++ b/twn3_test.c
@@ -12,8 +12,8 @@ int main(int argc, char** argv){
 	char data[1024];
 	unsigned u;
 
-	int device = twn3_open("/dev/ttyACM0");
-	
+	int device = twn3_open("/dev/ttyACM3");
+
 	printf("Device is at fd %d\n", device);
 	if(device < 0){
 		printf("%s\n", strerror(errno));
@@ -33,7 +33,7 @@ int main(int argc, char** argv){
 	}
 
 	//write some userdata
-	twn3_sync_write_eeprom(device, 0, 19, 0xAA);
+	//twn3_sync_write_eeprom(device, 0, 19, 0xAA);
 
 	//set ExtendID bit
 	//twn3_sync_write_eeprom(device, 0, 5, 0x05);
@@ -42,8 +42,23 @@ int main(int argc, char** argv){
 	//twn3_sync_restart(device, 0);
 
 	//store a key
-	uint8_t key[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
-	twn3_sync_storekey(device, 0, 12, key);
+	//uint8_t key[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+	//twn3_sync_storekey(device, 0, 12, key);
+	
+	//wait for a single tag
+	int status;
+	uint8_t type = tag_none;
+	uint8_t serial[20];
+	size_t serial_length = sizeof(serial);
+	for(status = twn3_sync_select_first(device, 0, &type, serial, &serial_length);
+			status == 0;
+			status = twn3_sync_select_first(device, 0, &type, serial, &serial_length)){
+		if(type != tag_none){
+			printf("Tag with UID length %lu found, type %02X\n", serial_length, type);
+			break;
+		}
+		serial_length = sizeof(serial);
+	}
 
 	close(device);
 	return EXIT_SUCCESS;