1 files changed, 288 insertions, 0 deletions
diff --git a/libyhy.c b/libyhy.c
new file mode 100644
index 0000000..041b0b7
--- /dev/null
+++ b/libyhy.c
@@ -0,0 +1,288 @@
+#include "libyhy.h"
+#include <stdint.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <stdio.h>
+#define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#ifdef DEBUG
+	#define DBG(x) (x)
+#else
+	#define DBG(x)
+#endif
+
+#define DEFAULT_NODE_ID 0
+
+//TODO
+//0x AA substitution where applicable
+//Confirm checksum on receive
+//Handle status flags on return
+
+#pragma pack(push, 1)
+typedef struct {
+	uint16_t magic; //0xAABB
+	uint16_t csum_span;
+	uint16_t node;
+	uint16_t command;
+} yhy_transmit_t;
+
+typedef struct {
+	uint16_t magic; //0xAABB
+	uint16_t csum_span;
+	uint16_t node;
+	uint16_t command;
+	uint8_t status; //0 on success
+} yhy_response_t;
+#pragma pack(pop)
+
+enum /*_yhy_command*/ {
+	YHY_SET_BAUDRATE = 0x0101,
+		/* uint8_t baudrate_id
+		 * 0 -> 4800
+		 * 1 -> 9600
+		 * 2 -> 14400
+		 * 3 -> 19200
+		 * 4 -> 38800
+		 * 5 -> 38400
+		 * 6 -> 57600
+		 * 7 -> 115200
+		 */
+	YHY_SET_NODEID = 0x0102,
+		/* uint16_t nodeid */
+	YHY_GET_NODEID = 0x0103,
+		/* No payload */
+	YHY_READ_VERSION = 0x0104,
+		/* No payload */
+	YHY_BEEP = 0x0106,
+		/* uint8_t beeptime in 10ms increments */
+	YHY_LED = 0x0107,
+		/* uint8_t LED status bitfield
+		 * Bit 0 -> Green LED
+		 * Bit 1 -> Red LED
+		 */
+	YHY_ANTENNA = 0x010C,
+		/* uint8_t anntenna state
+		 *	* 0 -> Antenna off
+		 *	* 1 -> Antenna on
+		 */
+	YHY_MIFARE_REQUEST = 0x0201,
+		/* uint8_t command
+		 *	* 0x52 -> Wake up all cards (WUPA)
+		 *	* 0x26 -> Request idle cards (REQA)
+		 */
+	YHY_MIFARE_ANTICOLL = 0x0202,
+		/* No payload */
+	YHY_MIFARE_SELECT = 0x0203,
+		/* uint32_t Card UID */
+	YHY_MIFARE_HLTA = 0x0204,
+		/* No payload */
+	YHY_MIFARE_AUTH = 0x0207,
+		/* uint8_t auth_mode
+		 *	* 0x60 -> Key A
+		 *	* 0x61 -> Key B
+		 * uint8_t block
+		 * uint8_t[6] key
+		 */
+	YHY_MIFARE_READ = 0x0208,
+		/* Payload uint8_t block */
+	YHY_MIFARE_WRITE = 0x0209,
+		/* uint8_t block
+		 * uint8_t[16] data */
+	YHY_ULTRALIGHT_SELECT = 0x0212,
+		/* No payload */
+	YHY_ULTRALIGHT_WRITE = 0x0213
+		/* uint8_t page
+		 * uint8_t[4] data */
+};
+
+static void xxd(uint8_t* buf, size_t len){
+	size_t n;
+	for(n = 0; n < len; n++){
+		printf("%02X ", buf[n]);
+	}
+}
+
+static ssize_t yhy_send_command(int fd, uint16_t node, uint16_t cmd, uint8_t* payload, size_t len){
+	size_t n;
+	uint8_t checksum = 0x00;
+	yhy_transmit_t header = {
+		.magic = htobe16(0xAABB),
+		.csum_span = htole16(len + 5),
+		.node = node,
+		.command = htole16(cmd)
+	};
+
+	for(n = 4; n < sizeof(header); n++){
+		checksum ^= ((uint8_t*)(&header))[n];
+	}
+
+	for(n = 0; n < len; n++){
+		checksum ^= payload[n];
+	}
+	
+	DBG(printf("Out: "));
+	DBG(xxd((uint8_t*) &header, sizeof(header)));
+	DBG(xxd(payload, len));
+	DBG(xxd(&checksum, 1));
+	DBG(printf("\n"));
+
+	write(fd, &header, sizeof(header));
+	if(len){
+		write(fd, payload, len);
+	}
+	write(fd, &checksum, 1);
+	return 0;
+}
+
+static ssize_t yhy_receive_response(int fd, uint8_t* response, size_t max_len){
+	//maximum received data is probably firmware version, assume 1024 bytes
+	uint8_t recv_buffer[1024] = "";
+	yhy_response_t* resp = (yhy_response_t*) recv_buffer;
+	read(fd, recv_buffer, sizeof(yhy_response_t));
+
+	//UL_SELECT returns wrong length from device
+	if(resp->command == YHY_ULTRALIGHT_SELECT && resp->status == 0){
+		DBG(printf("Fixing UL_SELECT response length (was %d)\n", le16toh(resp->csum_span)));
+		resp->csum_span = htole16(0x0D);
+	}
+
+	uint16_t bytes_left = le16toh(resp->csum_span) - 5;
+	read(fd, recv_buffer + sizeof(yhy_response_t), bytes_left);
+
+	DBG(printf("In: "));
+	DBG(xxd(recv_buffer, sizeof(yhy_response_t)));
+	DBG(printf(" | "));
+	DBG(xxd(recv_buffer + sizeof(yhy_response_t), bytes_left));
+	DBG(printf("\n"));
+
+	if(bytes_left > 1 && response && max_len){
+		memcpy(response, recv_buffer + sizeof(yhy_response_t), MIN(bytes_left - 1, max_len));
+	}
+	return bytes_left - 1;
+}
+
+static ssize_t yhy_sync(int fd, uint16_t node, uint16_t cmd, uint8_t* payload, size_t payload_len, uint8_t* response, size_t response_len){
+	yhy_send_command(fd, node, cmd, payload, payload_len);
+	return yhy_receive_response(fd, response, response_len);
+}
+
+int yhy_open(char* device){
+	struct termios tty;
+	
+	int fd = open(device, O_RDWR);
+	if(fd < 0) {
+		return -1;
+	}
+
+	if(tcgetattr(fd, &tty)){
+		return -1;
+	}
+
+	cfsetospeed(&tty, B115200);
+	cfsetispeed(&tty, B115200);
+
+	tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+	tty.c_cflag |= (CLOCAL | CREAD);
+	tty.c_cflag &= ~PARENB;
+	tty.c_cflag &= ~CSTOPB;
+	tty.c_cflag &= ~CSIZE;
+	tty.c_cflag &= ~CRTSCTS;
+	tty.c_cflag |= CS8;
+	tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+
+	if(tcsetattr(fd, TCSAFLUSH, &tty)){
+		return -1;
+	}
+
+	return fd;
+}
+
+int yhy_sync_read_version(int fd, char* version, size_t max_len){
+	ssize_t bytes = yhy_sync(fd, DEFAULT_NODE_ID, YHY_READ_VERSION, NULL, 0, (uint8_t*) version, max_len);
+	if(bytes >= max_len){
+		version[max_len] = 0;
+	}
+	else{
+		version[bytes] = 0;
+	}
+	return bytes;
+}
+
+int yhy_sync_beep(int fd, uint8_t time){
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_BEEP, &time, 1, NULL, 0);
+}
+
+int yhy_sync_led(int fd, uint8_t led_bits){
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_LED, &led_bits, 1, NULL, 0);
+}
+
+int yhy_sync_antenna(int fd, uint8_t enable){
+	uint8_t status = enable ? 1 : 0;
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_ANTENNA, &status, 1, NULL, 0);
+}
+
+int yhy_sync_request(int fd, uint8_t wakeup, uint16_t* atqa){
+	*atqa = 0;
+
+	//Select WUPA or REQA
+	uint8_t wup = wakeup ? 0x52 : 0x26;
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_REQUEST, &wup, 1, (uint8_t*) atqa, 2);
+}
+
+int yhy_sync_anticoll(int fd, uint8_t* serial, size_t length){
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_ANTICOLL, NULL, 0, serial, length);
+}
+
+int yhy_sync_select(int fd, uint8_t* serial, size_t length, uint8_t* sak){
+	*sak = 0;
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_SELECT, serial, length, sak, 1);
+}
+
+int yhy_sync_hlta(int fd){
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_HLTA, NULL, 0, NULL, 0);
+}
+
+int yhy_sync_auth(int fd, uint8_t key_a, uint8_t block, uint8_t key[6]){
+	struct {
+		uint8_t mode;
+		uint8_t block;
+		uint8_t key[6];
+	} auth_payload = {
+		.mode = (key_a ? 0x60 : 0x61),
+		.block = block
+	};
+	memcpy(&(auth_payload.key), key, 6);
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_AUTH, (uint8_t*) (&auth_payload), sizeof(auth_payload), NULL, 0);
+}
+
+int yhy_sync_read(int fd, uint8_t block, uint8_t* data, size_t max_len){
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_READ, &block, 1, data, max_len);
+}
+
+int yhy_sync_write(int fd, uint8_t block, uint8_t data[16]){
+	struct {
+		uint8_t block;
+		uint8_t data[16];
+	} write_payload = {
+		.block = block
+	};
+	memcpy(&(write_payload.data), data, 16);
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_WRITE, (uint8_t*) (&write_payload), sizeof(write_payload), NULL, 0);
+}
+
+int yhy_sync_ulselect(int fd, uint8_t* serial, size_t length){
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_ULTRALIGHT_SELECT, NULL, 0, serial, length);
+}
+
+int yhy_sync_ulwrite(int fd, uint8_t block, uint8_t data[4]){
+	struct {
+		uint8_t block;
+		uint8_t data[4];
+	} write_payload = {
+		.block = block
+	};
+	memcpy(&(write_payload.data), data, 4);
+	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_ULTRALIGHT_WRITE, (uint8_t*) (&write_payload), sizeof(write_payload), NULL, 0);
+}