From 1d216bc4c5f336ba531762bde73996fc4fb3d056 Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Mon, 4 Feb 2008 22:53:49 +0100 Subject: [PATCH] WIP: telemetrie --- include/telemetrie.h | 118 +++++++++++++++++++++++++ ldscript.ld | 9 +- src/telemetrie.c | 200 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 325 insertions(+), 2 deletions(-) create mode 100644 include/telemetrie.h create mode 100644 src/telemetrie.c diff --git a/include/telemetrie.h b/include/telemetrie.h new file mode 100644 index 0000000..26d07dd --- /dev/null +++ b/include/telemetrie.h @@ -0,0 +1,118 @@ +#ifndef TELEMETRIE_H_ +#define TELEMETRIE_H_ + +/* + * 0: this is a request (host -> board) + * 1: this is a reply (board -> host) + */ +#define TDC_DIR 0x80 +#define TDC_REPLY TDC_DIR + +/* + * TDC_DIR = 0: destination address + * TDC_DIR = 1: source address + */ +#define TDC_ADDRMASK 0x70 +#define TDC_ADDR0 0x00 // host (dynamic!, sends to interface of last hello) +#define TDC_ADDR1 0x10 // flightcontrol +#define TDC_ADDR2 0x20 // missioncontrol +#define TDC_ADDR3 0x30 // videocontrol +#define TDC_ADDR4 0x40 +#define TDC_ADDR5 0x50 +#define TDC_ADDR6 0x60 +#define TDC_ADDR7 0x70 + +#define TDC_OPCODEMASK 0x0F +#define TDC_HELLO 0x00 // sets the path/interface to the host, reply is a info string +#define TDC_GETVARS 0x01 // request variable names, many replies +#define TDC_GETVALUE 0x02 // get one value, one reply +#define TDC_SETVALUE 0x03 // set one value, no reply +#define TDC_REQVALUES 0x04 // registers a periodic update, timed replies +#define TDC_TERMINAL 0x05 // stdout data + +#define TDC_USERDATA 0x0F // user-defined data e.g. between boards + +struct tdc_pkt_header { + uint8_t cmd; // TDC_* + uint8_t size; // bytes after size +} __attribute__ ((packed)); + +struct tdc_hello_reply { + uint8_t cmd; + uint8_t size; + char name[32]; // name of device, version string +} __attribute__ ((packed)); + +struct tdc_getvars_reply { + uint8_t cmd; + uint8_t size; + uint8_t id; // variable ID (max 256 vars / board) + uint32_t flags; // variable parameters (type, size, ro/rw) + uint8_t name[0]; // variable name, excluding '\0' +} __attribute__ ((packed)); + +struct tdc_getvalue_request { + uint8_t cmd; + uint8_t size; + uint8_t id; // variable ID (max 256 vars / board) +} __attribute__ ((packed)); + +struct tdc_getvalue_reply { + uint8_t cmd; + uint8_t size; + uint8_t id; // variable ID (max 256 vars / board) + uint8_t data[0]; // variable data 1-8 bytes +} __attribute__ ((packed)); + +struct tdc_setvalue_request { + uint8_t cmd; + uint8_t size; + uint8_t id; // variable ID (max 256 vars / board) + uint8_t data[0]; // variable data 1-8 bytes +} __attribute__ ((packed)); + +struct tdc_reqvalues_request { + uint8_t cmd; + uint8_t size; + uint16_t interval; // interval in ms + uint32_t varmap[8]; // bitmap of variables (32 * 8 = 256) +} __attribute__ ((packed)); + +struct tdc_reqvalues_reply { + uint8_t cmd; + uint8_t size; + uint32_t timestamp; // internal jiffie count + uint8_t cnt; // number of variables +} __attribute__ ((packed)); + + +struct tdc_value { + const void *ptr; + const uint8_t *name; + uint32_t flags; +}; + +#define TDC_SIZEMASK 0x0F +#define TDC_UNSIGNED 0x00 +#define TDC_SIGNED 0x10 +#define TDC_FP 0x20 + +#define TDC_VALUE(var, desc, type, flags) \ + type * tdc_check_##var(void) { return (&var); } \ + static struct tdc_value __attribute__((used, section(".tdc_value"))) \ + tdc_value_##var = { &var, desc, sizeof(type) | flags }; \ + tdc_value_##var = tdc_value_##var; + +#define TDC_UINT8(var, desc) TDC_VALUE(var, desc, uint8_t, TDC_UNSIGNED) +#define TDC_UINT16(var, desc) TDC_VALUE(var, desc, uint16_t, TDC_UNSIGNED) +#define TDC_UINT32(var, desc) TDC_VALUE(var, desc, uint32_t, TDC_UNSIGNED) +#define TDC_UINT64(var, desc) TDC_VALUE(var, desc, uint64_t, TDC_UNSIGNED) +#define TDC_INT8(var, desc) TDC_VALUE(var, desc, int8_t, TDC_SIGNED) +#define TDC_INT16(var, desc) TDC_VALUE(var, desc, int16_t, TDC_SIGNED) +#define TDC_INT32(var, desc) TDC_VALUE(var, desc, int32_t, TDC_SIGNED) +#define TDC_INT64(var, desc) TDC_VALUE(var, desc, int64_t, TDC_SIGNED) +#define TDC_FLOAT(var, desc) TDC_VALUE(var, desc, float, TDC_FP) +#define TDC_DOUBLE(var, desc) TDC_VALUE(var, desc, double, TDC_FP) + + +#endif /*TELEMETRIE_H_*/ diff --git a/ldscript.ld b/ldscript.ld index 225c407..fd5aeb9 100644 --- a/ldscript.ld +++ b/ldscript.ld @@ -19,6 +19,11 @@ SECTIONS *(.pio_isr) _pio_isr_table_end = .; + . = ALIGN(4); + _tdc_value_table = .; + *(.tdc_value) + _tdc_value_table_end = .; + . = ALIGN(4); *(.rodata) *(.rodata.*) @@ -51,11 +56,11 @@ SECTIONS .stab.index 0 : { *(.stab.index) } .stab.indexstr 0 : { *(.stab.indexstr) } .comment 0 : { *(.comment) } - + /* DWARF 1 */ .debug 0 : { *(.debug) } .line 0 : { *(.line) } - + /* GNU DWARF 1 extensions */ .debug_srcinfo 0 : { *(.debug_srcinfo) } .debug_sfnames 0 : { *(.debug_sfnames) } diff --git a/src/telemetrie.c b/src/telemetrie.c new file mode 100644 index 0000000..7ccfc30 --- /dev/null +++ b/src/telemetrie.c @@ -0,0 +1,200 @@ +/*************************************************************************** + * 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 +#include +#include +#include "board.h" // ARRAY_SIZE() +#include "telemetrie.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; +} + + +static void * alloc(uint32_t size) +{ + return NULL; +} + +static void free(void *p) +{ +} + +/* +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; +}