path: root/libtwn3.c

#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)

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 0;
		}
		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;
	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_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;
}

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 = 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 > 0){
		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 = 0;
	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 > 0 && 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 = 0;

	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;
}

int twn3_sync_select_first(int fd, uint8_t flags, uint8_t* type, uint8_t* uid, 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);

		//currently, all uids are either 4, 7, or 10 bytes
		if(u == 5 || u == 8 || u == 11){
			//type tag prepended
			*type = data[0];
			memcpy(uid, data + 1, MIN(*length, u - 1));
			*length = u - 1;
		}
		else{
			memcpy(uid, 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* uid, size_t* length);
int twn3_sync_select(int fd, uint8_t flags, uint8_t* type, uint8_t* uid, size_t* length);

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;
	}

	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;
	}

	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 = 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){
			u = decode_ascii_inplace((uint8_t*) data, bytes - 2);
			memcpy(values, data, MIN(u, max_len));

			DBG(printf("Decoded %ld bytes of data\n", u));
			DBG(xxd((uint8_t*) values, u));
			DBG(printf("\n"));
			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 = 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],
			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, 35, (uint8_t*) data, sizeof(data));
	if(bytes > 0){
		if(data[1] != '\r'){
			return 0;
		}
		return 1;
	}
	return -1;
}