From 12cf1472f159b5aaf26d871d88f684f1f46cf856 Mon Sep 17 00:00:00 2001
From: cbdev <cb@cbcdn.com>
Date: Thu, 24 Aug 2023 20:13:51 +0200
Subject: Implement single-tag select

---
 libtwn3.c | 100 ++++++++++++++++++++++++++++++++++++++++++++++++++------------
 1 file changed, 81 insertions(+), 19 deletions(-)

(limited to 'libtwn3.c')

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],
-- 
cgit v1.2.3