1 files changed, 242 insertions, 0 deletions
diff --git a/libtwn3.c b/libtwn3.c
new file mode 100644
index 0000000..e07fbb5
--- /dev/null
+++ b/libtwn3.c
@@ -0,0 +1,242 @@
+#include <stdio.h>
+#include <stdint.h>
+
+#define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#define DEFAULT_NODE 0xFF
+
+#ifdef DEBUG
+	#define DBG(x) (x)
+	static void xxd(uint8_t* buf, size_t len){
+		size_t n;
+		for(n = 0; n < len; n++){
+			printf("%02X ", buf[n]);
+		}
+	}
+#else
+	#define DBG(x)
+#endif
+
+#include "libtwn3.h"
+
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <string.h>
+#include <ctype.h>
+
+#pragma pack(push, 1)
+typedef struct /*_twn3_hdr_t*/ {
+	uint8_t stx; //start of transmission, fixed 0x02
+	uint8_t station;
+	uint8_t length; //bytes in data segment
+} twn3_hdr_t;
+
+typedef struct /*_twn3_trl_t*/ {
+	uint8_t bcc; //xor over all bytes between stx and etx
+	uint8_t etx; //fixed 0x03
+} twn3_trl_t;
+#pragma pack(pop)
+
+int twn3_open(char* device){
+	int fd = open(device, O_RDWR | O_CLOEXEC);
+	struct termios tty;
+	uint8_t recv_buf[1024];
+	ssize_t bytes;
+
+	if(fd < 0){
+		return -1;
+	}
+
+	if(tcgetattr(fd, &tty)){
+		return -1;
+	}
+
+	cfsetospeed(&tty, B9600);
+	cfsetispeed(&tty, B9600);
+	tty.c_iflag = IGNBRK | IGNPAR;
+	tty.c_oflag = 0;
+	tty.c_cflag |= CS8;
+
+	tty.c_lflag = 0;
+	tty.c_cc[VMIN] = 0;
+	tty.c_cc[VTIME] = 3;
+
+	if(tcsetattr(fd, TCSAFLUSH, &tty)){
+		return -1;
+	}
+
+	tcflush(fd, TCIOFLUSH);
+
+	//the default config is for the reader to start in continuous read mode
+	//send two cancel commands to return to a known mode, at least one should produce '?'
+	twn3_send_command(fd, 0, (uint8_t*) "..", 2);
+	//then, read until the timeout elapses to clear the buffer.
+	//this is necessary because the default protocol does not have clear message boundaries
+	bytes = twn3_receive_response(fd, RX_FLAG_UNTIL_TIMEOUT, recv_buf, sizeof(recv_buf));
+	if(bytes <= 3 || recv_buf[bytes - 3] != '?'
+			|| recv_buf[bytes - 2] != '\r'
+			|| recv_buf[bytes - 1] != '\n'){
+		DBG(printf("Invalid initial response (%ld bytes): ", bytes));
+		DBG(xxd(recv_buf, bytes));
+		DBG(printf("\n"));
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+ssize_t twn3_send_command(int fd, uint8_t flags, uint8_t* data, size_t len){
+	size_t n;
+	twn3_hdr_t hdr = {
+		.stx = 0x02,
+		.station = DEFAULT_NODE,
+		.length = len
+	};
+	twn3_trl_t trl = {
+		.etx = 0x03
+	};
+
+	DBG(printf("Out: "));
+	if(flags & TX_FLAG_BINARY){
+		//calculate checksum
+		trl.bcc = hdr.station ^ hdr.length;
+		for(n = 0; n < len; n++){
+			trl.bcc ^= data[n];
+		}
+
+		DBG(xxd((uint8_t*) &hdr, sizeof(hdr)));
+
+		write(fd, &hdr, sizeof(hdr));
+	}
+
+	DBG(xxd(data, len));
+	write(fd, data, len);
+
+	if(flags & TX_FLAG_BINARY){
+		DBG(xxd((uint8_t*) &trl, sizeof(trl)));
+		write(fd, &trl, sizeof(trl));
+	}
+	DBG(printf("\n"));
+	return 0;
+}
+
+ssize_t twn3_receive_response(int fd, uint8_t flags, uint8_t* response, size_t max_length){
+	uint8_t recv_buffer[1024] = "";
+	ssize_t status, bytes = 0;
+
+	//TODO handle buffer overrun
+	//TODO handle binary protocol
+
+	while(1){
+		status = read(fd, recv_buffer + bytes, sizeof(recv_buffer) - bytes);
+		if(status < 0){
+			return -1;
+		}
+		if(status){
+			bytes += status;
+			if(!(flags & RX_FLAG_UNTIL_TIMEOUT) && bytes > 2){
+				//check if the last two buffer bytes are a newline
+				if(recv_buffer[bytes - 2] == '\r'
+						&& recv_buffer[bytes - 1] == '\n'){
+					break;
+				}
+			}
+		}
+		else if(flags & RX_FLAG_UNTIL_TIMEOUT){
+			break;
+		}
+	}
+
+	DBG(printf("In%s: ", (flags & RX_FLAG_UNTIL_TIMEOUT) ? " (timeout)" : ""));
+	DBG(xxd(recv_buffer, bytes));
+	DBG(printf("\n"));
+	if(response && max_length){
+		memcpy(response, recv_buffer, MIN(bytes, max_length));
+	}
+	return bytes;
+}
+
+ssize_t twn3_sync_command(int fd, uint8_t flags, uint8_t* cmd, size_t cmd_len, uint8_t* response, size_t max_length){
+	twn3_send_command(fd, flags, cmd, cmd_len);
+	return twn3_receive_response(fd, flags, response, max_length);
+}
+
+int twn3_sync_read_version(int fd, uint8_t flags, char* version, size_t max_length){
+	ssize_t bytes = twn3_sync_command(fd, flags, (uint8_t*) "v", 1, (uint8_t*) version, max_length);
+	if(bytes >= max_length){
+		version[max_length] = 0;
+	}
+	else{
+		//strip newlines
+		while(bytes > 0 && !isprint(version[bytes - 1])){
+			version[bytes - 1] = 0;
+			bytes--;
+		}
+	}
+	return bytes;
+}
+
+int twn3_sync_read_eeprom(int fd, uint8_t flags, uint8_t reg, uint8_t* value){
+	char data[6];
+	ssize_t bytes;
+	snprintf(data, sizeof(data) - 1, "re%02X", reg);
+	
+	bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 4, (uint8_t*) data, 4);
+	if(bytes){
+		data[2] = 0;
+		if(value){
+			*value = strtoul(data, NULL, 16);
+		}
+		return 0;
+	}
+	return -1;
+}
+
+int twn3_sync_write_eeprom(int fd, uint8_t flags, uint8_t reg, uint8_t value){
+	char data[8];
+	ssize_t bytes;
+	snprintf(data, sizeof(data) - 1, "we%02X%02X", reg, value);
+
+	if(reg > 0x14){
+		return -1;
+	}
+
+	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'){
+		data[2] = 0;
+		bytes = strtoul(data, NULL, 16);
+		return (bytes == value) ? 0 : -1;
+	}
+	return -1;
+}
+
+int twn3_sync_storekey(int fd, uint8_t flags, uint8_t keyid, uint8_t key[6]){
+	char data[20];
+	snprintf(data, sizeof(data) - 1, "wm%02X%02X%02X%02X%02X%02X%02X",
+			keyid, key[0], key[1],
+			key[2], key[3],
+			key[4], key[5]);
+
+	if(keyid > 31){
+		return -1;
+	}
+
+	twn3_sync_command(fd, flags, (uint8_t*) data, 16, NULL, 0);
+	return 0;
+}
+
+int twn3_sync_antenna(int fd, uint8_t flags, uint8_t enable){
+	uint8_t data[5];
+	ssize_t bytes;
+
+	if(enable){
+		bytes = twn3_sync_command(fd, flags, (uint8_t*) "pon", 3, data, sizeof(data));
+	}
+	else{
+		bytes = twn3_sync_command(fd, flags, (uint8_t*) "poff", 4, data, sizeof(data));
+	}
+
+	return (bytes > 2 && data[0] == 'P') ? 0 : -1;
+}