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 "telemetrie.h"
#include "memalloc.h"

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

typedef uint32_t (txfunc_t)(const uint8_t *data, uint32_t size);

/* just eat the packet */
static uint32_t txdummy(const uint8_t *data, uint32_t size)
{
	return -1;
}

/* table of txfuncs for routing/forwarding */
static txfunc_t *transmit_table[8] = {
	&txdummy, &txdummy, &txdummy, &txdummy,
	&txdummy, &txdummy, &txdummy, &txdummy,
};

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",
};

void tdc_register_txfunc(uint32_t addr, txfunc_t *txfunc)
{
	if (addr <= ARRAY_SIZE(transmit_table))
		transmit_table[addr] = txfunc;
}

int32_t tdc_transmit(uint32_t addr, const uint8_t *data, uint32_t size)
{
	if (addr <= ARRAY_SIZE(transmit_table))
		return transmit_table[addr](data, size);

	return -1;
}

/*
void *blub = NULL;
func(&blub);
*/
int32_t tdc_get_vars(void **privdata)
{
	struct tdc_value **value = (struct tdc_value **)privdata;

	if (*value == NULL)
		*value = &_tdc_value_table;

	uint32_t id = (*value - &_tdc_value_table) / sizeof(struct tdc_value);

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

		int32_t ret = transmit_table[TDC_ADDR0]((const uint8_t *)reply, reply->size);
		free(reply);

		if (ret != reply->size)
			return -1;

		id++;
		(*value)++;
	}

	/* TODO: all done */
	(*value) = NULL;
	return 0;
}

#if 0

static int32_t tdc_get_values(uint8_t *buf, uint32_t size, void **key)
{
	if (*key == NULL)
		*key = &_tdc_value_table;

	uint32_t id = (*key - &_tdc_value_table) / sizeof(struct tdc_value);
	uint32_t pos = 0;

	while (*key < &_tdc_value_table_end) {
		struct tdc_value *tmp = (struct tdc_value *)*key;
		uint32_t datalen = (tmp->flags & TDC_SIZEMASK);

		/* fits in buffer? */
		if ((size - pos) < (2 + 1 + datalen))
			return (pos > 0) ? pos : -1;

		buf[pos++] = TDC_VALUEOP;
		buf[pos++] = 1 + datalen;
		buf[pos++] = id++;

		memcpy(buf + pos, tmp->ptr, datalen);
		pos += datalen;

		*key += sizeof(struct tdc_value);
	}
	return pos;
}
#endif

/*
 * parses tdc data
 */
int32_t tdc_parse_pkt(txfunc_t *txfunc, const uint8_t *data, uint32_t size)
{
	struct tdc_pkt_header *head = (struct tdc_pkt_header *)data;

	if (size < sizeof(struct tdc_pkt_header))
		return -1;

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

	/* all replys go to the HOST */
	if (head->cmd & TDC_REPLY) {
		return transmit_table[TDC_ADDR0](data, head->size);
	}

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

	/* parse the packet */
	switch (head->cmd & TDC_OPCODEMASK) {
	/* HELLO from HOST */
	case TDC_HELLO:
		transmit_table[TDC_ADDR0]((const uint8_t *)&hello_reply, sizeof(hello_reply));
		break;

	case TDC_GETVARS:
		// TODO: exec func
		break;

	case TDC_GETVALUE:
		// TODO: exec func
		break;

	case TDC_SETVALUE:
		// TODO: exec func
		break;

	case TDC_REQVALUES:
		// TODO: exec func
		break;

	case TDC_TERMINAL:
		// TODO: not possible?
		break;

	case TDC_USERDATA:
		// TODO: currently not used
		break;
	};

	return head->size;
}