/***************************************************************************
 *   sam7fc - Telemetrie Handling                                          *
 *                                                                         *
 *   Copyright (C) 02/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; version 2 of the License                *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include "board.h"		// ARRAY_SIZE()
#include "at91_pitc.h"
#include "telemetrie.h"
#include "tdc_proto.h"
#include "memalloc.h"
#include "fifo.h"

#define TDC_OWN_ADDRESS		TDC_ADDR1

/* extern symbols, defined in ldscript */
extern struct tdc_value _tdc_value_table;
extern struct tdc_value _tdc_value_table_end;

/* max. 8x 32 = 256 variables */
static uint32_t tdc_varmap[8];

/* array of devices, that are used to reach address X */
static struct comm_device *routing_table[8];

/*
 * returns:
 * -1: on routing error
 *  0: no space left in txfifo (caller should retry)
 * >0: success
 */
int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head)
{
	if (addr >= ARRAY_SIZE(routing_table) || !routing_table[addr])
		return -1;

	int32_t retval = fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
	if (routing_table[addr]->trigger_tx)
		routing_table[addr]->trigger_tx();

	return retval;
}

static int32_t tdc_get_vars(void)
{
	/* restart point */
	static uint32_t id;

	struct tdc_value *value = &_tdc_value_table + id;

	while (value < &_tdc_value_table_end) {
		uint32_t datalen = strlen(value->name);

		struct tdc_getvars_reply *reply = alloc(sizeof(struct tdc_getvars_reply) + datalen);
		reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVARS;
		reply->size = sizeof(struct tdc_getvars_reply) + datalen;
		reply->id = (id & 0xFF);
		reply->flags = value->flags;
		reply->name_len = datalen;
		memcpy(reply->name, value->name, datalen);

		uint32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
		free(reply);

		/* push routing error(-1) and retry(0) */
		if (ret <= 0)
			return ret;

		id++;
		value++;
	}

	/* dump complete, reset restart point */
	id = 0;
	return 1;
}

static int32_t tdc_get_value(uint32_t id)
{
	struct tdc_value *value = &_tdc_value_table + id;
	if (value >= &_tdc_value_table_end)
		return -1;

	uint32_t datalen = value->flags & TDC_SIZEMASK;

	struct tdc_getvalue_reply *reply = alloc(sizeof(struct tdc_getvalue_reply) + datalen);
	reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVALUE;
	reply->size = sizeof(struct tdc_getvalue_reply) + datalen;
	reply->id = id;
	memcpy(reply->data, value->data, datalen);

	int32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
	free(reply);

	return ret;
}

static int32_t tdc_set_value(uint32_t id, uint8_t *data, uint32_t data_size)
{
	struct tdc_value *value = &_tdc_value_table + id;
	if (value >= &_tdc_value_table_end)
		return -1;

	uint32_t len = value->flags & TDC_SIZEMASK;

	if (len != data_size)
		return -1;

	// TODO: atomic?
	memcpy(value->data, data, len);
	return len;
}

static uint32_t tdc_timer_cb(struct pitc_timer *timer)
{
	uint32_t i, j;
	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++) {
		uint32_t bitmask = tdc_varmap[i];

		for (j = 0; j < 32; j++) {
			if (!bitmask)
				break;

			if (bitmask & 0x01) {
				if (tdc_get_value(i * 32 + j) < 0)
					tdc_varmap[i] &= ~(1 << j);
			}
			bitmask >>= 1;
		}
	}
	return PITC_RESTART_TIMER;
}

static struct pitc_timer tdc_timer = {
	.func = tdc_timer_cb,
};

static int32_t tdc_setup_timer(uint32_t interval, uint32_t *varmap)
{
	memcpy(tdc_varmap, varmap, sizeof(tdc_varmap));

	uint32_t i;
	uint32_t tmp = 0;
	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++)
		tmp |= tdc_varmap[i];

	if ((interval > 0) && (tmp != 0)) {
		tdc_timer.interval = interval;
		pitc_schedule_timer(&tdc_timer);

	} else {
		pitc_remove_timer(&tdc_timer);
	}
	return 1;
}

static const struct tdc_hello_reply hello_reply = {
	.cmd = TDC_REPLY | TDC_OWN_ADDRESS | TDC_HELLO,
	.size = sizeof(struct tdc_hello_reply),
	.name = "sam7fc-v0.01",
};

void tdc_register_device(uint32_t addr, struct comm_device *device)
{
	if (addr < ARRAY_SIZE(routing_table))
		routing_table[addr] = device;
}

struct tdc_pkt_header * tdc_alloc_fullpkt(struct comm_device *device, uint32_t size)
{
	struct tdc_pkt_header *head = alloc(size);

	/* peek the whole packet */
	uint32_t len = fifo_peek(device->rxfifo, (char *)head, size);
	if (len != size) {
		free(head);
		head = NULL;
	}

	return head;
}

static int32_t tdc_receive(struct comm_device *device)
{
	struct tdc_pkt_header tmp_head;
	struct tdc_pkt_header *head = &tmp_head;

	/* peek the header, return retry(0) if not enough bytes are available */
	uint32_t len = fifo_peek(device->rxfifo, (char *)head, sizeof(tmp_head));
	if (len != sizeof(tmp_head))
		return 0;

	/* assume an error, remove one byte from fifo */
	uint32_t used_bytes = 1;
	int32_t ret = -1;

	/* remember the device as path to the host */
	if ((head->cmd & (TDC_REPLY | TDC_OPCODEMASK)) == TDC_HELLO) {
		tdc_register_device(TDC_ADDR0, device);
	}

	/* reply packets / forward packets */
	if (head->cmd & TDC_REPLY || (head->cmd & TDC_ADDRMASK) != TDC_OWN_ADDRESS) {
		/* peek complete packet, return retry(0) if not enough bytes are available */
		head = tdc_alloc_fullpkt(device, head->size);
		if (head == NULL)
			return 0;

		/* reply packets go to ADDR0, forwards to others */
		uint32_t addr = (head->cmd & TDC_REPLY) ? TDC_ADDR0 : ((head->cmd & TDC_ADDRMASK) >> 4);

		used_bytes = head->size;
		ret = tdc_transmit(addr, head);

	} else {
		/* parse cmd */
		switch (head->cmd & TDC_OPCODEMASK) {
		case TDC_HELLO: {
			/* check packet size */
			struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
			if (pkt->size != sizeof(*pkt))
				break;

			/* send reply */
			ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
			used_bytes = pkt->size;
		}	break;

		case TDC_GETVARS: {
			struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
			if (pkt->size != sizeof(*pkt))
				break;

			/* send reply */
			ret = tdc_get_vars();
			used_bytes = pkt->size;
		}	break;

		case TDC_GETVALUE: {
			struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
			if (pkt->size != sizeof(*pkt))
				break;

			/* peek complete packet, return retry(0) if not enough bytes are available */
			head = tdc_alloc_fullpkt(device, head->size);
			if (head != NULL) {
				pkt = (struct tdc_getvalue_request *)head;
				ret = tdc_get_value(pkt->id);
				used_bytes = pkt->size;

			} else {
				ret = 0;
			}
		}	break;

		case TDC_SETVALUE: {
			struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
			if (pkt->size < sizeof(*pkt) +1 || pkt->size > sizeof(*pkt) +8)
				break;

			/* peek complete packet, return retry(0) if not enough bytes are available */
			head = tdc_alloc_fullpkt(device, head->size);
			if (head != NULL) {
				pkt = (struct tdc_setvalue_request *)head;
				ret = tdc_set_value(pkt->id, pkt->data, pkt->size - sizeof(*pkt));
				used_bytes = pkt->size;

			} else {
				ret = 0;
			}
		}	break;

		case TDC_REQVALUES: {
			struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
			if (pkt->size != sizeof(*pkt))
				break;

			/* peek complete packet, return retry(0) if not enough bytes are available */
			head = tdc_alloc_fullpkt(device, head->size);
			if (head != NULL) {
				pkt = (struct tdc_reqvalues_request *)head;
				ret = tdc_setup_timer(pkt->interval, pkt->varmap);
				used_bytes = pkt->size;

			} else {
				ret = 0;
			}
		}	break;

		}
	}

	/* on success(>0) or routing error(-1) remove the packet */
	if (ret != 0) {
		/* remove bytes from fifo */
		fifo_remove(device->rxfifo, used_bytes);
	}

	/* free allocated memory */
	if (head != NULL && head != &tmp_head)
		free(head);

	return ret;
}

void tdc_check(void)
{
	uint32_t i;
	for (i = 0; i < ARRAY_SIZE(routing_table); i++) {
		if (routing_table[i] != NULL) {
			tdc_receive(routing_table[i]);
			// TODO: handle retry
		}
	}
}

void tdc_init(void)
{
	uint32_t count = &_tdc_value_table_end - &_tdc_value_table;
	printf("found %ld TDC variables\n\r", count);
}