208 lines
5.8 KiB
C
208 lines
5.8 KiB
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;
|
|
|
|
/*
|
|
* transmit function
|
|
* returns:
|
|
* -1 : could not sent data (e.g. fifo full)
|
|
* >= 0 : bytes sent
|
|
*/
|
|
typedef uint32_t (txfunc_t)(struct tdc_pkt_header *head);
|
|
|
|
/* just eat the packet */
|
|
static uint32_t txdummy(struct tdc_pkt_header *head)
|
|
{
|
|
return head->size;
|
|
}
|
|
|
|
/* 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, struct tdc_pkt_header *head)
|
|
{
|
|
if (addr <= ARRAY_SIZE(transmit_table))
|
|
return transmit_table[addr](head);
|
|
|
|
return -1;
|
|
}
|
|
|
|
/*
|
|
* send all variable names & flags
|
|
* returns:
|
|
* 0 - success (*id = 0)
|
|
* -1 - not all data was sent, restart request later (*id has restart-point)
|
|
*/
|
|
int32_t tdc_get_vars(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 txerror = (tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply)) != reply->size);
|
|
free(reply);
|
|
|
|
/* transmit fifo is full, remember position (*id) and restart */
|
|
if (txerror)
|
|
return -1;
|
|
|
|
(*id)++;
|
|
value++;
|
|
}
|
|
*id = 0;
|
|
return 0;
|
|
}
|
|
|
|
int32_t tdc_get_value(uint32_t id)
|
|
{
|
|
struct tdc_value *value = &_tdc_value_table + id;
|
|
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 txerror = (tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply)) != reply->size);
|
|
free(reply);
|
|
|
|
return (txerror) ? -1 : 0;
|
|
}
|
|
|
|
void tdc_set_value(uint32_t id, uint8_t *data)
|
|
{
|
|
struct tdc_value *value = &_tdc_value_table + id;
|
|
uint32_t datalen = value->flags & TDC_SIZEMASK;
|
|
|
|
// TODO: atomic?
|
|
memcpy(value->data, data, datalen);
|
|
}
|
|
|
|
/*
|
|
* parses tdc data
|
|
* returns:
|
|
* -1 : data too short / error
|
|
* >= 0 : data read, come back if there is more
|
|
*/
|
|
int32_t tdc_parse_pkt(txfunc_t *txfunc, const uint8_t *data, uint32_t size)
|
|
{
|
|
// TODO: use a fifo, with a peek function?
|
|
|
|
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)
|
|
tdc_register_txfunc(TDC_ADDR0, txfunc);
|
|
|
|
/* all replys go to the HOST */
|
|
if (head->cmd & TDC_REPLY) {
|
|
// TODO: wrong returncode
|
|
return tdc_transmit(TDC_ADDR0, head);
|
|
}
|
|
|
|
/* forward this packet? */
|
|
if ((head->cmd & TDC_ADDRMASK) != TDC_ADDR1) {
|
|
uint32_t addr = (head->cmd & TDC_ADDRMASK) >> 4;
|
|
// TODO: wrong returncode
|
|
return tdc_transmit(addr, head);
|
|
}
|
|
|
|
/* parse the packet */
|
|
switch (head->cmd & TDC_OPCODEMASK) {
|
|
/* HELLO from HOST */
|
|
case TDC_HELLO:
|
|
tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
|
|
break;
|
|
|
|
case TDC_GETVARS: {
|
|
static uint32_t i;
|
|
|
|
/* tx-fifo is full, complete request later */
|
|
if (tdc_get_vars(&i) == -1)
|
|
return 0;
|
|
|
|
} break;
|
|
|
|
case TDC_GETVALUE: {
|
|
struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)data;
|
|
|
|
/* tx-fifo is full, retry request later */
|
|
if (tdc_get_value(pkt->id) == -1)
|
|
return 0;
|
|
|
|
} break;
|
|
|
|
case TDC_SETVALUE: {
|
|
struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)data;
|
|
tdc_set_value(pkt->id, pkt->data);
|
|
} break;
|
|
|
|
case TDC_REQVALUES: {
|
|
struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)data;
|
|
|
|
} break;
|
|
|
|
case TDC_TERMINAL:
|
|
// TODO: not possible?
|
|
break;
|
|
|
|
case TDC_USERDATA:
|
|
// TODO: currently not used
|
|
break;
|
|
};
|
|
|
|
return head->size;
|
|
}
|