sam7fc/src/telemetrie.c

/***************************************************************************
 *   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 "memalloc.h"
#include "fifo.h"

/* 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];

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;

	return fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
}

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;
		reply->flags = value->flags;
		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_getvars_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)
{
	struct tdc_value *value = &_tdc_value_table + id;
	if (value >= &_tdc_value_table_end)
		return -1;

	uint32_t datalen = value->flags & TDC_SIZEMASK;

	// TODO: atomic?
	memcpy(value->data, data, datalen);

	return 1;
}

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 tmp = tdc_varmap[i];

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

			if (tmp & 0x01) {
				if (tdc_get_value(i * 32 + j) <= 0)
					return PITC_REMOVE_TIMER;
			}

			tmp >>= 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));

	if (interval > 0) {
		tdc_timer.interval = interval;
		// TODO: timer already running
		pitc_schedule_timer(&tdc_timer);

	} else {
		// TODO: timer stop
	}
	return 1;
}

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

/*
 * returns:
 * -1: on routing error
 *  0: no space left in txfifo (caller should retry)
 * >0: success
 */
static int32_t tdc_parse_packet(struct tdc_pkt_header *head)
{
	/* all replys go to the HOST */
	if (head->cmd & TDC_REPLY)
		return tdc_transmit(TDC_ADDR0, head);

	/* forward this packet? */
	if ((head->cmd & TDC_ADDRMASK) != TDC_ADDR1) {
		uint32_t addr = (head->cmd & TDC_ADDRMASK) >> 4;
		return tdc_transmit(addr, head);
	}

	int32_t ret = -1;

	/* parse the packet */
	switch (head->cmd & TDC_OPCODEMASK) {
	case TDC_HELLO:
		/* answer the hello */
		ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
		break;

	case TDC_GETVARS:
		ret = tdc_get_vars();
		break;

	case TDC_GETVALUE: {
		struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
		ret = tdc_get_value(pkt->id);
	}	break;

	case TDC_SETVALUE: {
		struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
		ret = tdc_set_value(pkt->id, pkt->data);
	}	break;

	case TDC_REQVALUES: {
		struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
		ret = tdc_setup_timer(pkt->interval, pkt->varmap);
	}	break;
	};

	/*
	 * on succes(>0) return size of request,
	 * and push retry(0) and routing error(-1) up
	 */
	return (ret > 0) ? head->size : ret;
}

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

void tdc_receive(struct comm_device *device)
{
	while (1) {
		/* peek the header */
		struct tdc_pkt_header tmp_head;
		uint32_t len = fifo_peek(device->rxfifo, (char *)&tmp_head, sizeof(tmp_head));
		if (len != sizeof(tmp_head))
			return;

		/* peek the whole packet */
		struct tdc_pkt_header *head = alloc(tmp_head.size);
		len = fifo_peek(device->rxfifo, (char *)head, tmp_head.size);
		if (len != tmp_head.size)
			return;

		/* if it's a hello-request, remember the device as path to the host */
		if ((head->cmd & (TDC_OPCODEMASK & TDC_DIR)) == TDC_HELLO)
			tdc_register_device(TDC_ADDR0, device);

		/* parse packet, remove data if no restart is needed */
		int32_t ret = tdc_parse_packet(head);
		free(head);

		/* some tx-fifo was full(0), return to caller and let it retry */
		if (!ret)
			return;

		/* success(>0) or routing error(-1) -> remove the packet */
		fifo_remove(device->rxfifo, tmp_head.size);
	}
}