razzor
/
sam7fc
1
0
Fork 0
Code Issues Pull Requests Releases Activity
sam7fc/src/telemetrie.c

339 lines
9.1 KiB
C
Raw Blame History

/***************************************************************************
* 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);
}
Reference in New Issue View Git Blame Copy Permalink