339 lines
9.1 KiB
C
339 lines
9.1 KiB
C
/***************************************************************************
|
|
* 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);
|
|
}
|