gtdc/tdc_parser.c

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>

#include <glib.h>
#include <gdk/gdk.h>

#include "sockaddr.h"
#include "tcpsocket.h"

#include "tdc_parser.h"
#include "tdc_proto.h"
#include "tdc_store.h"
#include "tdc_variable.h"

static int socket_fd;
static gint socket_tag;
static void (*socket_status_cb)(int status);
static GByteArray *socket_stream;

static int tdcparser_send(struct tdc_pkt_header *pkt)
{
	return write(socket_fd, pkt, pkt->size);
}

void tdcparser_send_hello(void)
{
	int addr;
	for (addr = 1; addr < 8; addr++) {
		struct tdc_pkt_header pkt;
		pkt.cmd = (addr << 4) | TDC_HELLO;
		pkt.size = sizeof(pkt);

		tdcparser_send(&pkt);
	}
}

int tdcparser_send_getvars(int address)
{
	struct tdc_pkt_header pkt;
	pkt.cmd = (address << 4) | TDC_GETVARS;
	pkt.size = sizeof(pkt);

	return tdcparser_send(&pkt);
}

int tdcparser_send_getvalue(int address, int id)
{
	struct tdc_getvalue_request pkt;
	pkt.cmd = (address << 4) | TDC_GETVALUE;
	pkt.size = sizeof(pkt);
	pkt.id = (id & 0xFF);

	return tdcparser_send((struct tdc_pkt_header *)&pkt);
}

int tdcparser_send_setvalue(int address, int id)
{
	struct tdc_var *var = tdcstore_get_var(address, id);
	if (var == NULL)
		return -1;

	int datasize = (var->flags & TDC_SIZEMASK);

	struct tdc_setvalue_request *pkt;
	pkt = g_malloc0(sizeof(struct tdc_setvalue_request) + datasize);

	pkt->cmd = (address << 4) | TDC_SETVALUE;
	pkt->size = sizeof(pkt) + datasize;
	pkt->id = (id & 0xFF);
	memcpy(pkt->data, &var->data, datasize);

	int retval = tdcparser_send((struct tdc_pkt_header *)pkt);
	g_free(pkt);

	return retval;
}

int tdcparser_send_request(int address, int interval, uint32_t *bitmap)
{
	struct tdc_reqvalues_request pkt;
	pkt.cmd = (address << 4) | TDC_REQVALUES;
	pkt.size = sizeof(pkt);
	pkt.interval = interval;
	memcpy(pkt.varmap, bitmap, sizeof(pkt.varmap));

	return tdcparser_send((struct tdc_pkt_header *)&pkt);
}

static int check_size_helper(int available,
			int packet_size,
			int expected_base_size,
			int packet_data_size,
			const char *name)
{
	/* not enough data in buffer */
	if (available < expected_base_size) {
// 		printf("received incomplete %s\n", name);
		return 0;
	}

	/* retest, after we know that packet_data_size is valid */
	if (available < (expected_base_size + packet_data_size)) {
// 		printf("received incomplete %s\n", name);
		return 0;
	}

	/* packet has wrong size */
	if (packet_size != (expected_base_size + packet_data_size)) {
// 		printf("received invalid %s\n", name);
		return -1;
	}

// 	printf("received %s\n", name);
	return packet_size;
}

static int tdcparser_parse(void)
{
	int address = (socket_stream->data[0] & TDC_ADDRMASK) >> 4;

	int remove = 1;

	switch (socket_stream->data[0] & (TDC_REPLY | TDC_OPCODEMASK)) {
	case TDC_HELLO: {
		struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_HELLO");
		/* nothing to do */
	}	break;

	case (TDC_HELLO | TDC_REPLY): {
		struct tdc_hello_reply *pkt = (struct tdc_hello_reply *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_HELLO REPLY");
		if (remove <= 0)
			break;

		tdcstore_create_board(address, pkt->name);
		tdcstore_refresh_values(address, 250);
		tdcparser_send_getvars(address);
	}	break;

	case TDC_GETVARS: {
		struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_GETVARS");
		/* nothing to do */
	}	break;

	case (TDC_GETVARS | TDC_REPLY): {
		struct tdc_getvars_reply *pkt = (struct tdc_getvars_reply *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt),
					pkt->name_len, "TDC_GETVARS REPLY");

		if (remove <= 0)
			break;

		tdcstore_create_var(address, tdcvar_create(pkt->id, pkt->flags, pkt->name, pkt->name_len));
	}	break;

	case TDC_GETVALUE: {
		struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_GETVALUE");
		/* nothing to do */
	}	break;

	case (TDC_GETVALUE | TDC_REPLY): {
		struct tdc_getvalue_reply *pkt = (struct tdc_getvalue_reply *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_GETVALUE REPLY (1)");
		/* at this point: invalid packet */
		if (remove > 0)
			remove = 1;
		/* retry with more bytes */
		else if (remove == 0)
			break;

		/* base packet is complete, but exact size is wrong, get it from storage */
		struct tdc_var *var = tdcstore_get_var(address, pkt->id);
		if (var == NULL) {
			printf("unknown variable %d\n", pkt->id);
			remove = 1;
			break;
		}

		int varsize = (var->flags & TDC_SIZEMASK);
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), varsize, "TDC_GETVALUE REPLY (2)");
		if (remove <= 0)
			break;

		tdcstore_update_var(address, pkt->id, pkt->data, varsize);
	}	break;

	case TDC_SETVALUE: {
		struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_SETVALUE REPLY (1)");
		/* at this point: invalid packet */
		if (remove > 0)
			remove = 1;
		/* retry with more bytes */
		else if (remove == 0)
			break;

		/* base packet is complete, but exact size is wrong, get it from storage */
		struct tdc_var *var = tdcstore_get_var(address, pkt->id);
		if (var == NULL) {
			printf("unknown variable %d\n", pkt->id);
			remove = 1;
			break;
		}

		int varsize = (var->flags & TDC_SIZEMASK);
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), varsize, "TDC_SETVALUE REPLY (2)");
		if (remove <= 0)
			break;

		/* nothing to do */
	}	break;

	case (TDC_SETVALUE | TDC_REPLY):
		break;

	case TDC_REQVALUES: {
		struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_REQVALUES");
		/* nothing to do */
	}	break;

	case (TDC_REQVALUES | TDC_REPLY): {
		struct tdc_reqvalues_reply *pkt = (struct tdc_reqvalues_reply *)socket_stream->data;
		remove = check_size_helper(socket_stream->len, pkt->size, sizeof(*pkt), 0, "TDC_REQVALUES");
		if (remove <= 0)
			break;

		// TODO: output pkt->timestamp, pkt->cnt
	}	break;

	case TDC_TERMINAL:
	case (TDC_TERMINAL | TDC_REPLY):
		break;
	}

	if (remove != 0)
		g_byte_array_remove_range(socket_stream, 0, (remove > 0) ? remove : -remove);

	return remove;
}

static void tdcparser_read_cb(gpointer data,
				gint source,
				GdkInputCondition condition)
{
	char buf[256];
	int len = read(socket_fd, buf, sizeof(buf));

	if (len > 0) {
		g_byte_array_append(socket_stream, (guint8 *)buf, len);

		while (socket_stream->len >= 2) {
			if (tdcparser_parse() == 0)
				break;
		}

	} else {
		g_warning("gtdc:tdcparser_read_cb:read:%s", g_strerror(errno));
		tdcparser_disconnect();
	}
}

static void tdcparser_connect_cb(gpointer data,
				gint source,
				GdkInputCondition condition)
{
	gdk_input_remove(socket_tag);

	if (tcp_connect_error(source) < 0) {
		g_warning("gtdc:tdcparser_connect_cb:tcp_connect_error:%s", g_strerror(errno));

		socket_status_cb(STAT_DISCONNECTED);
		close(socket_fd);

	} else {
		socket_tag = gdk_input_add(socket_fd, GDK_INPUT_READ, tdcparser_read_cb, NULL);
		socket_status_cb(STAT_CONNECTED);

		if (socket_stream == NULL)
			socket_stream = g_byte_array_new();

		else if (socket_stream->len)
			g_byte_array_remove_range(socket_stream, 0, socket_stream->len);

		tdcstore_flush();
		tdcparser_send_hello();
	}
}

void tdcparser_connect(char *addr_str, void (*cb)(int status))
{
	struct sockaddr_in sa;
	parse_sockaddr(addr_str, &sa);

	socket_status_cb = cb;
	socket_status_cb(STAT_CONNECTING);

	socket_fd = tcp_connect_nonblock(&sa);
	socket_tag = gdk_input_add(socket_fd, GDK_INPUT_WRITE, tdcparser_connect_cb, NULL);
}

void tdcparser_disconnect(void)
{
	socket_status_cb(STAT_DISCONNECTED);
	gdk_input_remove(socket_tag);
	close(socket_fd);
}