gtdc/tdc_store.c

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

#include <glib.h>

#include "tdc_store.h"
#include "tdcproto.h"

#include "variable_tab.h"

#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))

static struct tdc_var * var_map[256];
static GByteArray *stream;

static void destroy_var(int id)
{
	g_free(var_map[id]);
	var_map[id] = NULL;
}

static void create_var(int id, uint32_t flags, char *name, int len)
{
	id &= 0xFF;

	if (var_map[id] != NULL)
		destroy_var(id);

	var_map[id] = g_malloc0(sizeof(struct tdc_var) + len + 1);
	var_map[id]->id = id;
	var_map[id]->flags = flags;

	strncpy(var_map[id]->name, name, len);
	var_map[id]->name[len] = '\0';

	vartab_add_var(var_map[id]);
}

static void update_var(int id, uint8_t *data, int len)
{
	memcpy(&(var_map[id & 0xFF]->data_uint8), data, len);
}

#if 0
static char * stream_print(void)
{
	int pos = 0, i = 0, j;
	char *buf = g_malloc(stream->len * 4 + 64);

	while (pos < stream->len) {
		i += sprintf(buf + i, "%04X: ", pos);
		for (j = 0; j < 16; j++) {
			if (pos + j < stream->len)
				i += sprintf(buf + i, "%02X", stream->data[pos + j]);
			else
				i += sprintf(buf + i, "  ");

			if (j % 2)
				buf[i++] = ' ';
		}

		for (j = 0; j < 16; j++) {
			if (pos + j < stream->len) {
				unsigned char val = stream->data[pos + j];
				if (val >= 0x20 && val < 0x80)
					buf[i++] = val;
				else
					buf[i++] = '.';
			} else {
				buf[i++] = ' ';
			}
		}

		pos += 16;
		buf[i++] = '\r';
		buf[i++] = '\n';
	}
	buf[i] = 0;
	return buf;
}
#endif

static int parse_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 parse(int fd)
{
	int address = (stream->data[0] & TDC_ADDRMASK) >> 4;

	int remove = 1;

	switch (stream->data[0] & (TDC_REPLY | TDC_OPCODEMASK)) {
	case TDC_HELLO: {
		struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)stream->data;
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), 0, "TDC_HELLO");
		/* do nothing, we'll never receive TDC_HELLOs */
	}	break;

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

		printf("ADDR%d: '%32s'\n", address, pkt->name);

		struct tdc_pkt_header pkt2;
		pkt2.cmd = (address << 4) | TDC_GETVARS;
		pkt2.size = sizeof(pkt2);
		write(fd, &pkt2, pkt2.size);

	}	break;

	case TDC_GETVARS: {
		struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)stream->data;
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), 0, "TDC_GETVARS");
		/* do nothing, we'll never receive TDC_GETVARSs */
	}	break;

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

		if (remove <= 0)
			break;

		create_var(pkt->id, pkt->flags, pkt->name, pkt->name_len);
		printf("create_var(%d, 0x%x, '%s', %d)\n",
			pkt->id, var_map[pkt->id]->flags, var_map[pkt->id]->name, pkt->name_len);
	}	break;

	case TDC_GETVALUE: {
		struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)stream->data;
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), 0, "TDC_GETVALUE");
		/* do nothing, we'll never receive TDC_GETVALUEs */
	}	break;

	case (TDC_GETVALUE | TDC_REPLY): {
		struct tdc_getvalue_reply *pkt = (struct tdc_getvalue_reply *)stream->data;

		/* check base packet */
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), 0, "TDC_GETVALUE REPLY");
		if (remove <= 0)
			break;

		if (var_map[pkt->id & 0xFF] == NULL) {
			printf("unknown variable %d\n", pkt->id);
			break;
		}

		int varsize = (var_map[pkt->id & 0xFF]->flags & TDC_SIZEMASK);
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), varsize, "TDC_GETVALUE REPLY");
		// TODO: adjust remove on error (valid reply with invalid data size)

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

	case TDC_SETVALUE: {
		struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)stream->data;
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), 0, "TDC_SETVALUE");
		if (remove <= 0)
			break;

		if (var_map[pkt->id & 0xFF] == NULL) {
			printf("unknown variable %d\n", pkt->id);
			break;
		}

		int varsize = (var_map[pkt->id & 0xFF]->flags & TDC_SIZEMASK);
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), varsize, "TDC_GETVALUE REPLY");
		// TODO: adjust remove on error (valid reply with invalid data size)

		/* do nothing, we'll never receive TDC_SETVALUEs */
	}	break;

	case (TDC_SETVALUE | TDC_REPLY):
		break;

	case TDC_REQVALUES: {
		struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)stream->data;
		remove = parse_size_helper(stream->len, pkt->size, sizeof(*pkt), 0, "TDC_REQVALUES");
		/* do nothing, we'll never receive TDC_REQVALUESs */
	}	break;

	case (TDC_REQVALUES | TDC_REPLY): {
		struct tdc_reqvalues_reply *pkt = (struct tdc_reqvalues_reply *)stream->data;
		remove = parse_size_helper(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(stream, 0, (remove > 0) ? remove : -remove);

	return remove;
}

int tdc_parse_data(int fd)
{
	char buf[256];
	int len = read(fd, buf, sizeof(buf));

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

		while (stream->len >= 2) {
			if (parse(fd) == 0)
				break;
		}
	}

	return len;
}

void tdc_flush_vars(void)
{
	int i;
	for (i = 0; i < ARRAY_SIZE(var_map); i++) {
		if (var_map[i] == NULL)
			continue;

		destroy_var(i);
	}

	if (stream->len)
		g_byte_array_remove_range(stream, 0, stream->len);
}

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

		write(fd, &pkt, pkt.size);
	}
}

void tdc_init(void)
{
	stream = g_byte_array_new();
}