Implement read/write

2 files changed, 73 insertions, 7 deletions

diff --git a/libtwn3.c b/libtwn3.c
index 4d862cd..e9a70bc 100644
--- a/libtwn3.c
+++ b/libtwn3.c
@@ -163,6 +163,7 @@ ssize_t twn3_sync_command(int fd, uint8_t flags, uint8_t* cmd, size_t cmd_len, u
 }
 
 int twn3_sync_restart(int fd, uint8_t flags){
+	//TODO stop auto-started continuous mode
 	ssize_t bytes = twn3_sync_command(fd, flags, (uint8_t*) "x", 1, NULL, 0);
 	return (bytes >= 2) ? 0 : -1;
 }
@@ -184,11 +185,11 @@ int twn3_sync_read_version(int fd, uint8_t flags, char* version, size_t max_leng
 
 int twn3_sync_read_eeprom(int fd, uint8_t flags, uint8_t reg, uint8_t* value){
 	char data[6];
-	ssize_t bytes;
+	ssize_t bytes = 0;
 	snprintf(data, sizeof(data) - 1, "re%02X", reg);
 	
 	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 4, (uint8_t*) data, 4);
-	if(bytes){
+	if(bytes > 0){
 		data[2] = 0;
 		if(value){
 			*value = strtoul(data, NULL, 16);
@@ -200,7 +201,7 @@ int twn3_sync_read_eeprom(int fd, uint8_t flags, uint8_t reg, uint8_t* value){
 
 int twn3_sync_write_eeprom(int fd, uint8_t flags, uint8_t reg, uint8_t value){
 	char data[8];
-	ssize_t bytes;
+	ssize_t bytes = 0;
 	snprintf(data, sizeof(data) - 1, "we%02X%02X", reg, value);
 
 	if(reg > 0x14){
@@ -209,7 +210,7 @@ int twn3_sync_write_eeprom(int fd, uint8_t flags, uint8_t reg, uint8_t value){
 
 	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 6, (uint8_t*) data, 4);
 	//failure may return `F\r\n`, which is also valid hex - genius!
-	if(bytes && data[1] != '\r'){
+	if(bytes > 0 && data[1] != '\r'){
 		data[2] = 0;
 		bytes = strtoul(data, NULL, 16);
 		return (bytes == value) ? 0 : -1;
@@ -234,7 +235,7 @@ int twn3_sync_storekey(int fd, uint8_t flags, uint8_t keyid, uint8_t key[6]){
 
 int twn3_sync_antenna(int fd, uint8_t flags, uint8_t enable){
 	uint8_t data[5];
-	ssize_t bytes;
+	ssize_t bytes = 0;
 
 	if(enable){
 		bytes = twn3_sync_command(fd, flags, (uint8_t*) "pon", 3, data, sizeof(data));
@@ -245,3 +246,66 @@ 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_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;
+	}
+	if(in <= 46){
+		return in - 55;
+	}
+	return in - 87;
+}
+
+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;
+	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;
+				}
+
+				values[u] = (hex_nibble(data[u * 2]) << 4) | hex_nibble(data[(u * 2) + 1]);
+			}
+			return u;
+		}
+		else{
+			return 0;
+		}
+	}
+	return -1;
+}
+
+int twn3_sync_write(int fd, uint8_t flags, uint8_t block, uint8_t value[16]){
+	char data[40];
+	ssize_t bytes;
+	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],
+			value[4], value[5], value[6], value[7],
+			value[8], value[9], value[10], value[11],
+			value[12], value[13], value[14], value[15]);
+
+	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 19, (uint8_t*) data, sizeof(data));
+	if(bytes > 0){
+		if(data[1] != '\r'){
+			return 0;
+		}
+		return 1;
+	}
+	return -1;
+}
+
diff --git a/twn3_test.c b/twn3_test.c
index cb4fb3a..ca10574 100644
--- a/twn3_test.c
+++ b/twn3_test.c
@@ -10,7 +10,6 @@
 
 int main(int argc, char** argv){
 	char data[1024];
-	ssize_t bytes;
 	unsigned u;
 
 	int device = twn3_open("/dev/ttyACM0");
@@ -37,7 +36,10 @@ int main(int argc, char** argv){
 	twn3_sync_write_eeprom(device, 0, 19, 0xAA);
 
 	//set ExtendID bit
-	twn3_sync_write_eeprom(device, 0, 5, 0x05);
+	//twn3_sync_write_eeprom(device, 0, 5, 0x05);
+
+	//restart device
+	//twn3_sync_restart(device, 0);
 
 	//store a key
 	uint8_t key[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};