eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

failed approach

Browse Source
This commit is contained in:
Jakob Meier
2020-11-19 18:24:03 +01:00
parent 349897d878
commit 6c23b00c22
324 changed files with 57839 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
gomspace/libgscsp/src/bindings/python/pygscsp.c Normal file
View File

@ -0,0 +1,61 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <Python.h>
#include <gs/csp/drivers/i2c/i2c.h>
#if PY_MAJOR_VERSION == 3
#define IS_PY3
#endif
/* gs_error_t gs_csp_i2c_init(uint8_t device, uint8_t csp_addr); */
static PyObject* pygscsp_csp_i2c_init(PyObject *self, PyObject *args) {
uint8_t device;
uint8_t csp_addr;
if (!PyArg_ParseTuple(args, "BB", &device, &csp_addr)) {
Py_RETURN_NONE;
}
return Py_BuildValue("i", gs_csp_i2c_init(device, csp_addr));
}
static PyMethodDef methods[] = {
{"i2c_init", pygscsp_csp_i2c_init, METH_VARARGS, ""},
/* sentinel */
{NULL, NULL, 0, NULL}
};
#ifdef IS_PY3
static struct PyModuleDef moduledef = {
PyModuleDef_HEAD_INIT,
"libgscsp_py3",
NULL,
-1,
methods,
NULL,
NULL,
NULL,
NULL
};
#endif
#ifdef IS_PY3
PyMODINIT_FUNC PyInit_libgscsp_py3(void) {
#else
PyMODINIT_FUNC initlibgscsp_py2(void) {
#endif
#ifdef IS_PY3
PyObject* m = PyModule_Create(&moduledef);
#else
Py_InitModule("libgscsp_py2", methods);
#endif
#ifdef IS_PY3
return m;
#endif
}

23
gomspace/libgscsp/src/clock.c Normal file
View File

@ -0,0 +1,23 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
/**
@file
Required by libcsp.
Proto-typed in ./libcsp/include/csp/arch/csp_clock.h, but with different argumet!
__attribute__((weak)) extern void clock_get_time(csp_timestamp_t * time);
__attribute__((weak)) extern void clock_set_time(csp_timestamp_t * time);
*/
#include <csp/arch/csp_clock.h>
#include <gs/util/clock.h>
void clock_get_time(csp_timestamp_t * time)
{
gs_clock_get_time((gs_timestamp_t*)time);
}
void clock_set_time(csp_timestamp_t * time)
{
gs_clock_set_time((gs_timestamp_t*)time);
}

652
gomspace/libgscsp/src/commands.c Normal file
View File

@ -0,0 +1,652 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <math.h>
#include <stdlib.h>
#include <limits.h>
#include <gs/csp/error.h>
#include <csp/csp.h>
#include <csp/csp_cmp.h>
#include <csp/csp_endian.h>
#include <gs/util/string.h>
#include <gs/util/gosh/command.h>
#include <gs/util/hexdump.h>
#include <gs/util/base16.h>
#include <gs/util/log.h>
#include <gs/util/clock.h>
static gs_error_t parse_node_timeout(gs_command_context_t *ctx, int node_index, uint8_t * node, int timeout_index, uint32_t * timeout)
{
gs_error_t error = GS_OK;
if (node) {
*node = csp_get_address();
if (ctx->argc > node_index) {
error = gs_string_to_uint8(ctx->argv[node_index], node);
}
}
if (timeout && (error == GS_OK)) {
*timeout = 1000;
if (ctx->argc > timeout_index) {
error = gs_string_to_uint32(ctx->argv[timeout_index], timeout);
}
}
return error;
}
static int cmd_ping(gs_command_context_t *ctx)
{
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 2, &timeout);
if (res) {
return res;
}
uint32_t size = 1;
if ((ctx->argc > 3) && (gs_string_to_uint32(ctx->argv[3], &size) != GS_OK)) {
return GS_ERROR_ARG;
}
uint32_t options = CSP_O_NONE;
if (ctx->argc > 4) {
const char * features = ctx->argv[4];
if (strchr(features, 'r'))
options |= CSP_O_RDP;
if (strchr(features, 'x'))
options |= CSP_O_XTEA;
if (strchr(features, 'h'))
options |= CSP_O_HMAC;
if (strchr(features, 'c'))
options |= CSP_O_CRC32;
}
printf("Ping node %u, timeout %" PRIu32 ", size %" PRIu32 ": options: 0x%" PRIx32 " ... ", node, timeout, size, options);
const uint64_t start = gs_clock_get_nsec();
const int time = csp_ping(node, timeout, size, options);
const uint64_t stop = gs_clock_get_nsec();
const float elapsed = (((float)(stop - start)) / 1E6);
if (time < 0) {
printf("timeout after %.03f ms\r\n", elapsed);
return GS_ERROR_TIMEOUT;
}
printf("reply in %.03f ms\r\n", elapsed);
return GS_OK;
}
static int cmd_ps(gs_command_context_t *ctx)
{
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 2, &timeout);
if (res) {
return res;
}
csp_ps(node, timeout);
return GS_OK;
}
static int cmd_memfree(gs_command_context_t *ctx)
{
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 2, &timeout);
if (res) {
return res;
}
csp_memfree(node, timeout);
return GS_OK;
}
static int cmd_reboot(gs_command_context_t *ctx)
{
if (ctx->argc < 2) {
return GS_ERROR_ARG;
}
uint8_t node;
int res = parse_node_timeout(ctx, 1, &node, 0, NULL);
if (res) {
return res;
}
csp_reboot(node);
return GS_OK;
}
static int cmd_shutdown(gs_command_context_t *ctx)
{
if (ctx->argc < 2) {
return GS_ERROR_ARG;
}
uint8_t node;
int res = parse_node_timeout(ctx, 1, &node, 0, NULL);
if (res) {
return res;
}
csp_shutdown(node);
return GS_OK;
}
static int cmd_buf_free(gs_command_context_t *ctx)
{
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 2, &timeout);
if (res) {
return res;
}
csp_buf_free(node, timeout);
return GS_OK;
}
static int cmd_uptime(gs_command_context_t *ctx)
{
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 2, &timeout);
if (res) {
return res;
}
csp_uptime(node, timeout);
return GS_OK;
}
#ifdef CSP_DEBUG
static int cmd_csp_route_print_table(gs_command_context_t *ctx)
{
csp_route_print_table();
return GS_OK;
}
static int cmd_csp_route_print_interfaces(gs_command_context_t *ctx)
{
csp_route_print_interfaces();
return GS_OK;
}
static int cmd_csp_conn_print_table(gs_command_context_t *ctx)
{
csp_conn_print_table();
return GS_OK;
}
#endif
#if CSP_USE_RDP
static int cmd_csp_rdp_set_opt(gs_command_context_t *ctx)
{
if (ctx->argc < 7) {
return GS_ERROR_ARG;
}
int res;
uint32_t window_size;
if ((res = gs_string_to_uint32(ctx->argv[1], &window_size))) {
return res;
}
uint32_t conn_timeout;
if ((res = gs_string_to_uint32(ctx->argv[2], &conn_timeout))) {
return res;
}
uint32_t packet_timeout;
if ((res = gs_string_to_uint32(ctx->argv[3], &packet_timeout))) {
return res;
}
uint32_t delayed_acks;
if ((res = gs_string_to_uint32(ctx->argv[4], &delayed_acks))) {
return res;
}
uint32_t ack_timeout;
if ((res = gs_string_to_uint32(ctx->argv[5], &ack_timeout))) {
return res;
}
uint32_t ack_delay_count;
if ((res = gs_string_to_uint32(ctx->argv[6], &ack_delay_count))) {
return res;
}
printf("Setting arguments to: window size %" PRIu32 ", conn timeout %" PRIu32 ", packet timeout %" PRIu32 ", delayed acks %" PRIu32 ", ack timeout %" PRIu32 ", ack delay count %" PRIu32 "\r\n",
window_size, conn_timeout, packet_timeout, delayed_acks, ack_timeout, ack_delay_count);
csp_rdp_set_opt(window_size, conn_timeout, packet_timeout, delayed_acks, ack_timeout, ack_delay_count);
return GS_OK;
}
#endif
static int cmd_cmp_ident(gs_command_context_t *ctx)
{
uint8_t node;
uint32_t timeout;
int ret = parse_node_timeout(ctx, 1, &node, 2, &timeout);
if (ret) {
return ret;
}
struct csp_cmp_message msg;
ret = csp_cmp_ident(node, timeout, &msg);
if (ret != CSP_ERR_NONE) {
printf("CSP error: %d\r\n", ret);
return gs_csp_error(ret);;
}
printf("Hostname: %s\r\n", msg.ident.hostname);
printf("Model: %s\r\n", msg.ident.model);
printf("Revision: %s\r\n", msg.ident.revision);
printf("Date: %s\r\n", msg.ident.date);
printf("Time: %s\r\n", msg.ident.time);
return GS_OK;
}
static int cmd_cmp_route_set(gs_command_context_t *ctx)
{
if (ctx->argc != 6)
return GS_ERROR_ARG;
uint8_t node = atoi(ctx->argv[1]);
uint32_t timeout = atoi(ctx->argv[2]);
printf("Sending route_set to node %"PRIu8" timeout %"PRIu32"\r\n", node, timeout);
struct csp_cmp_message msg;
msg.route_set.dest_node = atoi(ctx->argv[3]);
msg.route_set.next_hop_mac = atoi(ctx->argv[4]);
strncpy(msg.route_set.interface, ctx->argv[5], 10);
printf("Dest_node: %u, next_hop_mac: %u, interface %s\r\n", msg.route_set.dest_node, msg.route_set.next_hop_mac, msg.route_set.interface);
int ret = csp_cmp_route_set(node, timeout, &msg);
if (ret != CSP_ERR_NONE) {
printf("CSP error: %d\r\n", ret);
return gs_csp_error(ret);
}
return GS_OK;
}
static int cmd_cmp_ifc(gs_command_context_t *ctx) {
uint8_t node;
uint32_t timeout;
char * interface;
if (ctx->argc > 4 || ctx->argc < 3)
return GS_ERROR_ARG;
node = atoi(ctx->argv[1]);
interface = ctx->argv[2];
if (ctx->argc < 4)
timeout = 1000;
else
timeout = atoi(ctx->argv[3]);
struct csp_cmp_message msg;
strncpy(msg.if_stats.interface, interface, CSP_CMP_ROUTE_IFACE_LEN);
printf("Requesting interface stats for interface %s\r\n", interface);
int ret = csp_cmp_if_stats(node, timeout, &msg);
if (ret != CSP_ERR_NONE) {
printf("CSP error: %d\r\n", ret);
return gs_csp_error(ret);
}
msg.if_stats.tx = csp_ntoh32(msg.if_stats.tx);
msg.if_stats.rx = csp_ntoh32(msg.if_stats.rx);
msg.if_stats.tx_error = csp_ntoh32(msg.if_stats.tx_error);
msg.if_stats.rx_error = csp_ntoh32(msg.if_stats.rx_error);
msg.if_stats.drop = csp_ntoh32(msg.if_stats.drop);
msg.if_stats.autherr = csp_ntoh32(msg.if_stats.autherr);
msg.if_stats.frame = csp_ntoh32(msg.if_stats.frame);
msg.if_stats.txbytes = csp_ntoh32(msg.if_stats.txbytes);
msg.if_stats.rxbytes = csp_ntoh32(msg.if_stats.rxbytes);
msg.if_stats.irq = csp_ntoh32(msg.if_stats.irq);
printf("%-5s tx: %05"PRIu32" rx: %05"PRIu32" txe: %05"PRIu32" rxe: %05"PRIu32"\r\n"
" drop: %05"PRIu32" autherr: %05"PRIu32 " frame: %05"PRIu32"\r\n"
" txb: %"PRIu32" rxb: %"PRIu32"\r\n\r\n",
msg.if_stats.interface, msg.if_stats.tx, msg.if_stats.rx, msg.if_stats.tx_error, msg.if_stats.rx_error, msg.if_stats.drop,
msg.if_stats.autherr, msg.if_stats.frame, msg.if_stats.txbytes, msg.if_stats.rxbytes);
return GS_OK;
}
static int cmd_cmp_peek(gs_command_context_t *ctx)
{
if ((ctx->argc != 4) && (ctx->argc != 5))
return GS_ERROR_ARG;
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 4, &timeout);
if (res) {
return res;
}
uint32_t addr;
if (gs_string_to_uint32(ctx->argv[2], &addr)) {
return GS_ERROR_ARG;
}
uint32_t len;
if (gs_string_to_uint32(ctx->argv[3], &len)) {
return GS_ERROR_ARG;
}
if (len > CSP_CMP_PEEK_MAX_LEN) {
return GS_ERROR_RANGE;
}
printf("Dumping mem from node %u addr 0x%"PRIx32" len %"PRIx32" timeout %"PRIu32"\r\n", node, addr, len, timeout);
struct csp_cmp_message msg;
msg.peek.addr = csp_hton32(addr);
msg.peek.len = len;
int ret = csp_cmp_peek(node, timeout, &msg);
if (ret != CSP_ERR_NONE) {
printf("CSP error: %d\r\n", ret);
return gs_csp_error(ret);
}
gs_hexdump_addr(msg.peek.data, len, GS_TYPES_UINT2PTR(addr));
return GS_OK;
}
static int cmd_cmp_poke(gs_command_context_t *ctx)
{
if ((ctx->argc != 4) && (ctx->argc != 5))
return GS_ERROR_ARG;
uint8_t node;
uint32_t timeout;
int res = parse_node_timeout(ctx, 1, &node, 4, &timeout);
if (res) {
return res;
}
uint32_t addr;
if (gs_string_to_uint32(ctx->argv[2], &addr)) {
return GS_ERROR_ARG;
}
unsigned char data[CSP_CMP_POKE_MAX_LEN];
uint32_t len = base16_decode(ctx->argv[3], data);
if (len > CSP_CMP_PEEK_MAX_LEN) {
printf("Max length is: %u\r\n", CSP_CMP_PEEK_MAX_LEN);
return GS_ERROR_RANGE;
}
printf("Writing to mem at node %u addr 0x%"PRIx32" len %"PRIx32" timeout %"PRIu32"\r\n", node, addr, len, timeout);
gs_hexdump_addr(data, len, GS_TYPES_UINT2PTR(addr));
struct csp_cmp_message msg;
msg.poke.addr = csp_hton32(addr);
msg.poke.len = len;
memcpy(msg.poke.data, data, CSP_CMP_POKE_MAX_LEN);
int ret = csp_cmp_poke(node, timeout, &msg);
if (ret != CSP_ERR_NONE) {
printf("CSP error: %d\r\n", ret);
return gs_csp_error(ret);
}
return GS_OK;
}
static int cmd_cmp_clock(gs_command_context_t *ctx, uint32_t node, uint32_t timeout, const gs_timestamp_t * set)
{
char tbuf[GS_CLOCK_ISO8601_BUFFER_LENGTH];
struct csp_cmp_message msg;
memset(&msg, 0, sizeof(msg));
if (set) {
gs_clock_to_iso8601_string(set, tbuf, sizeof(tbuf));
printf("Set time: %s (%" PRIu32 ".%09" PRIu32 " sec)\r\n", tbuf, set->tv_sec, set->tv_nsec);
msg.clock.tv_sec = csp_hton32(set->tv_sec);
msg.clock.tv_nsec = csp_hton32(set->tv_nsec);
}
gs_timestamp_t t1, t2;
gs_clock_get_time(&t1);
int ret = csp_cmp_clock(node, timeout, &msg);
if (ret != CSP_ERR_NONE) {
return gs_csp_error(ret);
}
gs_clock_get_time(&t2);
/* Calculate round-trip time */
const int64_t rtt = ((uint64_t)t2.tv_sec * 1000000000 + t2.tv_nsec) - ((uint64_t)t1.tv_sec * 1000000000 + t1.tv_nsec);
gs_timestamp_t timestamp;
timestamp.tv_sec = csp_ntoh32(msg.clock.tv_sec);
timestamp.tv_nsec = csp_ntoh32(msg.clock.tv_nsec);
gs_clock_to_iso8601_string(&timestamp, tbuf, sizeof(tbuf));
printf("Get time: %s (%" PRIu32 ".%09" PRIu32 " sec)\r\n", tbuf, timestamp.tv_sec, timestamp.tv_nsec);
/* Calculate time difference to local clock. This takes the round-trip
* into account, but assumes a symmetrical link */
const int64_t remote = (uint64_t)timestamp.tv_sec * 1000000000 + timestamp.tv_nsec;
const int64_t local = (uint64_t)t1.tv_sec * 1000000000 + t1.tv_nsec + rtt / 2;
const double diff = (remote - local) / 1000000.0;
printf("Remote is %f ms %s local time\r\n", fabs(diff), diff > 0 ? "ahead of" : "behind");
return GS_OK;
}
static int cmd_cmp_clock_get(gs_command_context_t *ctx)
{
if (ctx->argc < 2) {
return GS_ERROR_ARG;
}
uint32_t node;
if (gs_string_to_uint32(ctx->argv[1], &node) != GS_OK) {
return GS_ERROR_ARG;
}
uint32_t timeout = 1000;
if (ctx->argc > 2) {
if (gs_string_to_uint32(ctx->argv[2], &timeout) != GS_OK) {
return GS_ERROR_ARG;
}
}
return cmd_cmp_clock(ctx, node, timeout, NULL);
}
static int cmd_cmp_clock_set(gs_command_context_t *ctx)
{
if (ctx->argc < 3) {
return GS_ERROR_ARG;
}
uint32_t node;
if (gs_string_to_uint32(ctx->argv[1], &node) != GS_OK) {
return GS_ERROR_ARG;
}
gs_timestamp_t ts;
if (gs_clock_from_string(ctx->argv[2], &ts) != GS_OK) {
return GS_ERROR_ARG;
}
uint32_t timeout = 1000;
if (ctx->argc > 3) {
if (gs_string_to_uint32(ctx->argv[3], &timeout) != GS_OK) {
return GS_ERROR_ARG;
}
}
return cmd_cmp_clock(ctx, node, timeout, &ts);
}
static int cmd_cmp_clock_sync(gs_command_context_t *ctx)
{
if (ctx->argc < 2) {
return GS_ERROR_ARG;
}
uint32_t node;
if (gs_string_to_uint32(ctx->argv[1], &node) != GS_OK) {
return GS_ERROR_ARG;
}
uint32_t timeout = 1000;
if (ctx->argc > 2) {
if (gs_string_to_uint32(ctx->argv[2], &timeout) != GS_OK) {
return GS_ERROR_ARG;
}
}
gs_timestamp_t ts;
gs_clock_get_time(&ts);
return cmd_cmp_clock(ctx, node, timeout, &ts);
}
static const gs_command_t GS_COMMAND_SUB cmp_clock_commands[] = {
{
.name = "get",
.help = "Get clock on <node>",
.usage = "<node> [timeout]",
.handler = cmd_cmp_clock_get,
},
{
.name = "set",
.help = "Set time of <node>",
.usage = "<node> <sec.nsec|YYYY-MM-DDTHH:MM:SSZ> [timeout]",
.handler = cmd_cmp_clock_set,
},
{
.name = "sync",
.help = "Sync/set time of <node> to time of this node",
.usage = "<node> [timeout]",
.handler = cmd_cmp_clock_sync,
}
};
static const gs_command_t GS_COMMAND_SUB cmp_commands[] = {
{
.name = "ident",
.help = "Node id",
.usage = "[node] [timeout]",
.handler = cmd_cmp_ident,
},{
.name = "route_set",
.help = "Update table",
.usage = "<node> <timeout> <addr> <mac> <ifstr>",
.handler = cmd_cmp_route_set,
},{
.name = "ifc",
.help = "Remote IFC",
.usage = "<node> <interface> [timeout]",
.handler = cmd_cmp_ifc,
},{
.name = "peek",
.help = "Show remote memory",
.usage = "<node> <addr> <len> [timeout]",
.handler = cmd_cmp_peek,
},{
.name = "poke",
.help = "Modify remote memory",
.usage = "<node> <addr> <base16_data> [timeout]",
.handler = cmd_cmp_poke,
},{
.name = "clock",
.help = "Get/set clock",
.chain = GS_COMMAND_INIT_CHAIN(cmp_clock_commands),
}
};
static const gs_command_t GS_COMMAND_ROOT csp_commands[] = {
{
.name = "ping",
.help = "csp: Ping",
.usage = "[node] [timeout] [size] [opt: r|x|h|c]",
.handler = cmd_ping,
},{
.name = "rps",
.help = "csp: Remote ps",
.usage = "[node] [timeout]",
.handler = cmd_ps,
},{
.name = "memfree",
.help = "csp: Memory free",
.usage = "[node] [timeout]",
.handler = cmd_memfree,
},{
.name = "buffree",
.help = "csp: Buffer free",
.usage = "[node] [timeout]",
.handler = cmd_buf_free,
},{
.name = "reboot",
.help = "csp: Reboot",
.usage = "<node>",
.handler = cmd_reboot,
},{
.name = "shutdown",
.help = "csp: Shutdown",
.usage = "<node>",
.handler = cmd_shutdown,
},{
.name = "uptime",
.help = "csp: Uptime",
.usage = "[node] [timeout]",
.handler = cmd_uptime,
},{
.name = "cmp",
.help = "csp: Management",
.chain = GS_COMMAND_INIT_CHAIN(cmp_commands),
},
#ifdef CSP_DEBUG
{
.name = "route",
.help = "csp: Show routing table",
.handler = cmd_csp_route_print_table,
},{
.name = "ifc",
.help = "csp: Show interfaces",
.handler = cmd_csp_route_print_interfaces,
},{
.name = "conn",
.help = "csp: Show connection table",
.handler = cmd_csp_conn_print_table,
},
#endif
#if CSP_USE_RDP
{
.name = "rdpopt",
.help = "csp: Set RDP options",
.handler = cmd_csp_rdp_set_opt,
.usage = "<window size> <conn timeout> <packet timeout> <delayed ACKs> <ACK timeout> <ACK delay count>"
},
#endif
};
gs_error_t gs_csp_register_commands(void)
{
return GS_COMMAND_REGISTER(csp_commands);
}

22
gomspace/libgscsp/src/conn.c Normal file
View File

@ -0,0 +1,22 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/csp.h>
#include <../lib/libcsp/src/csp_conn.h> // internal libcsp header
size_t gs_csp_conn_get_open(void)
{
size_t open = 0;
size_t max_connections;
const csp_conn_t * connections = csp_conn_get_array(&max_connections);
if (connections) {
for (unsigned int i = 0; i < max_connections; ++i) {
if (connections[i].state != CONN_CLOSED) {
++open;
}
}
}
// csp_conn_print_table();
return open;
}

91
gomspace/libgscsp/src/csp.c Normal file
View File

@ -0,0 +1,91 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/csp.h>
#include <gs/csp/log.h>
#include <gs/util/check.h>
#include "local.h"
void gs_csp_conf_get_defaults_embedded(gs_csp_conf_t * conf)
{
static const gs_csp_conf_t defaults = {
.use_gs_log = true,
.use_command_line_options = true,
.csp_buffer_size = 256, // typical MTU size is 256
.csp_buffers = 10, // in case of RDP connections, must be > RDP Windows size
.address = 1,
.hostname = "hostname",
.model = "model",
.revision = "revision",
};
*conf = defaults;
#if GS_CSP_COMMAND_LINE_SUPPORT
conf->address = gs_csp_command_line_get_address();
#endif
}
void gs_csp_conf_get_defaults_server(gs_csp_conf_t * conf)
{
gs_csp_conf_get_defaults_embedded(conf);
conf->csp_buffer_size = 512;
conf->csp_buffers = 400;
}
gs_error_t gs_csp_init(const gs_csp_conf_t * conf)
{
GS_CHECK_ARG(conf != NULL);
if (conf->use_gs_log) {
gs_csp_log_init();
}
int res = csp_buffer_init(conf->csp_buffers, conf->csp_buffer_size);
if (res != CSP_ERR_NONE) {
log_error("%s: csp_buffer_init(buffers: %u, size: %u) failed, CSP error: %d, error: %d",
__FUNCTION__, (unsigned int) conf->csp_buffers, (unsigned int) conf->csp_buffer_size, res, gs_csp_error(res));
return gs_csp_error(res);
}
csp_set_hostname(conf->hostname);
csp_set_model(conf->model);
csp_set_revision(conf->revision);
uint8_t csp_address = conf->address;
#if GS_CSP_COMMAND_LINE_SUPPORT
if (gs_csp_command_line_is_address_set()) {
csp_address = gs_csp_command_line_get_address();
}
#endif
res = csp_init(csp_address);
if (res != CSP_ERR_NONE) {
log_error("%s: csp_init(address: %u) failed, CSP error: %d, error: %d",
__FUNCTION__, conf->address, res, gs_csp_error(res));
return gs_csp_error(res);
}
#if GS_CSP_COMMAND_LINE_SUPPORT
if (conf->use_command_line_options) {
gs_error_t error = gs_csp_command_line_configure_interfaces();
if (error) {
log_error("%s: gs_csp_command_line_configure_interfaces() failed, error: %d",
__FUNCTION__, error);
return error;
}
}
#endif
return GS_OK;
}
bool gs_csp_is_address_valid(uint8_t address)
{
if (address < 1) {
return false;
}
if (address >= 33) {
return false;
}
return true;
}

106
gomspace/libgscsp/src/drivers/can/can.c Normal file
View File

@ -0,0 +1,106 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/csp/drivers/can/can.h>
#include <csp/csp.h>
#include <csp/interfaces/csp_if_can.h>
#include <gs/util/error.h>
#include <gs/util/check.h>
#include <gs/util/log.h>
#include <gs/util/drivers/can/can.h>
#define NO_OF_CAN_CHANNELS 2
#define MAX_NAME_LENGTH 10 // It says in csp_types.h, that it should be below 10 bytes
// change default log group
#define LOG_DEFAULT gs_can_log
typedef struct {
// self reference device handle
uint8_t can_ch;
// CSP interface name
char interface_name[MAX_NAME_LENGTH];
// CSP interface
csp_iface_t interface;
} gs_csp_can_interface_t;
static gs_csp_can_interface_t csp_can_interfaces[NO_OF_CAN_CHANNELS];
static void gs_csp_can_rxdata_callback_isr(int hdl,
uint32_t canMsgId,
bool extendedMsgId,
const void * data,
size_t data_size,
uint32_t nowMs,
void * user_data,
gs_context_switch_t * cswitch)
{
csp_can_rx(&csp_can_interfaces[hdl].interface, canMsgId, data, data_size, &cswitch->task_woken);
}
// Required by libcsp
int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc)
{
return gs_can_send_extended(((gs_csp_can_interface_t *)interface->driver)->can_ch, id, data, dlc, 1000);
}
gs_error_t gs_csp_can_init2(uint8_t device, uint8_t csp_addr, uint32_t mtu, const char * name, bool set_default_route, csp_iface_t ** csp_if)
{
GS_CHECK_HANDLE(device < NO_OF_CAN_CHANNELS);
gs_csp_can_interface_t * interface = &csp_can_interfaces[device];
// Register/subscribe to CAN frames for CSP
const uint32_t can_id = CFP_MAKE_DST(csp_get_address());
const uint32_t can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
log_debug("%s(%u): id=0x%" PRIx32 ", mask=0x%" PRIx32, __FUNCTION__, device, can_id, can_mask);
if (gs_string_empty(name)) {
name = GS_CSP_CAN_DEFAULT_IF_NAME;
}
if (strlen(name) >= MAX_NAME_LENGTH) {
return GS_ERROR_ARG;
}
if (csp_iflist_get_by_name(name)) {
log_error("%s(%u): name: [%s] - already exists", __FUNCTION__, device, name);
return GS_ERROR_EXIST;
}
// hook CAN into CSP
GS_STRNCPY(interface->interface_name, name);
interface->interface.name = interface->interface_name;
interface->interface.nexthop = csp_can_tx;
interface->interface.mtu = mtu;
interface->interface.driver = interface;
csp_iflist_add(&interface->interface);
if (csp_if) {
*csp_if = &interface->interface;
}
gs_error_t error = gs_can_set_extended_filter_mask(0, can_id, can_mask, gs_csp_can_rxdata_callback_isr, NULL);
if (error) {
log_error("%s: gs_can_set_extended_filter_mask() failed, error: %s", __FUNCTION__, gs_error_string(error));
return error;
}
error = gs_can_start(device);
if (error) {
log_error("%s: gs_can_start() failed, error: %s", __FUNCTION__, gs_error_string(error));
return error;
}
if (set_default_route) {
// Route all to CAN
csp_rtable_set(0, 0, &interface->interface, CSP_NODE_MAC);
}
return GS_OK;
}
gs_error_t gs_csp_can_init(uint8_t device, uint8_t csp_addr, uint32_t mtu, const char * name, csp_iface_t ** csp_if)
{
return gs_csp_can_init2(device, csp_addr, mtu, name, true, csp_if);
}

77
gomspace/libgscsp/src/drivers/i2c/i2c.c Normal file
View File

@ -0,0 +1,77 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/csp.h>
#include <gs/csp/error.h>
#include <gs/csp/drivers/i2c/i2c.h>
#include <csp/interfaces/csp_if_i2c.h>
#include <gs/util/drivers/i2c/master.h>
#include <gs/util/drivers/i2c/slave.h>
#include <gs/util/types.h>
#if !defined(__linux__)
#include <gs/embed/freertos.h>
#endif
#define E_FAIL -19 // The CSP I2C driver evaluates any other value than -1 as fail
#define I2C_FRAME_OVERHEAD (sizeof(i2c_frame_t) - sizeof(((i2c_frame_t *)0)->data))
static void gs_csp_i2c_rxdata_callback_isr(uint8_t handle, const uint8_t * rx, size_t rx_length, gs_context_switch_t * cswitch)
{
i2c_frame_t * frame = (i2c_frame_t *) (rx - I2C_FRAME_OVERHEAD);
frame->len = rx_length;
#if (__linux__)
csp_i2c_rx(frame, NULL);
#else
csp_i2c_rx(frame, &cswitch->task_woken);
#endif
}
static void * gs_csp_i2c_get_buffer(uint8_t handle)
{
void * buff = csp_buffer_get_isr(I2C_MTU);
if (buff != NULL) {
buff = ((uint8_t *)buff) + I2C_FRAME_OVERHEAD;
}
return buff;
}
/**
CSP send function, required by libcsp
*/
int i2c_send(int handle, i2c_frame_t * frame, uint16_t timeout)
{
int res_tx = gs_i2c_master_transaction(handle, frame->dest, frame->data, frame->len, 0, 0, timeout);
if (res_tx == GS_OK) {
csp_buffer_free(frame);
return E_NO_ERR;
} else {
return E_FAIL;
}
}
/**
CSP init function, required by libcsp
*/
int i2c_init(int handle, int mode, uint8_t addr, uint16_t speed, int queue_len_tx, int queue_len_rx,
i2c_callback_t callback)
{
if (gs_i2c_slave_set_rx(handle, gs_csp_i2c_rxdata_callback_isr) != GS_OK) {
return E_FAIL;
}
if (gs_i2c_slave_set_get_rx_buf(handle, gs_csp_i2c_get_buffer, I2C_MTU) != GS_OK) {
return E_FAIL;
}
if (gs_i2c_slave_start(handle) != GS_OK) {
return E_FAIL;
}
return E_NO_ERR;
}
gs_error_t gs_csp_i2c_init(uint8_t device, uint8_t csp_addr)
{
int dummy_speed = 0; // Speed not used
/* Calls CSP I2C init, which has the I2C interface instance
From here the above "i2c_init" is called */
return gs_csp_error(csp_i2c_init(csp_addr, device, dummy_speed));
}

36
gomspace/libgscsp/src/drivers/kiss/kiss.c Normal file
View File

@ -0,0 +1,36 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/embed/drivers/uart/uart.h>
#include <csp/csp.h>
#include <gs/csp/drivers/kiss/kiss.h>
static csp_iface_t csp_if_kiss;
static uint8_t uart_csp_device;
static void usart_rx_callback(void * user_data, const uint8_t * data, size_t data_size, gs_context_switch_t * cswitch)
{
csp_kiss_rx(&csp_if_kiss, (uint8_t *)data, data_size, cswitch);
}
static void csp_kiss_putc(char c)
{
gs_uart_write(uart_csp_device, -1, c);
}
static void csp_kiss_discard(char c, void *pxTaskWoken)
{
// Do nothing with discarded characters
}
gs_error_t gs_csp_kiss_init(uint8_t device)
{
static csp_kiss_handle_t csp_kiss_driver;
static const char * kiss_name = "KISS";
csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_kiss, CSP_NODE_MAC);
csp_kiss_init(&csp_if_kiss, &csp_kiss_driver, csp_kiss_putc, csp_kiss_discard, kiss_name);
uart_csp_device = device;
return gs_uart_set_rx_callback(device, usart_rx_callback, NULL);
}

54
gomspace/libgscsp/src/error.c Normal file
View File

@ -0,0 +1,54 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/error.h>
gs_error_t gs_csp_error(int csp_error)
{
switch (csp_error) {
case CSP_ERR_NONE: /* No error */
return GS_OK;
case CSP_ERR_NOMEM: /* Not enough memory */
return GS_ERROR_ALLOC;
case CSP_ERR_INVAL: /* Invalid argument */
return GS_ERROR_ARG;
case CSP_ERR_TIMEDOUT: /* Operation timed out */
return GS_ERROR_TIMEOUT;
case CSP_ERR_USED: /* Resource already in use */
return GS_ERROR_IN_USE;
case CSP_ERR_NOTSUP: /* Operation not supported */
return GS_ERROR_NOT_SUPPORTED;
case CSP_ERR_BUSY: /* Device or resource busy */
return GS_ERROR_BUSY;
case CSP_ERR_ALREADY: /* Connection already in progress */
return GS_ERROR_ALREADY_IN_PROGRESS;
case CSP_ERR_RESET: /* Connection reset */
return GS_ERROR_CONNECTION_RESET;
case CSP_ERR_NOBUFS: /* No more buffer space available */
return GS_ERROR_NO_BUFFERS;
case CSP_ERR_TX: /* Transmission failed */
case CSP_ERR_DRIVER: /* Error in driver layer */
return GS_ERROR_IO;
case CSP_ERR_AGAIN:
return GS_ERROR_AGAIN;
case CSP_ERR_HMAC: /* HMAC failed */
case CSP_ERR_XTEA: /* XTEA failed */
case CSP_ERR_CRC32: /* CRC32 failed */
return GS_ERROR_DATA;
default:
break;
}
return csp_error;
}

8
gomspace/libgscsp/src/freertos/cpu.c Normal file
View File

@ -0,0 +1,8 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/embed/drivers/sys/reset.h>
void cpu_reset(void)
{
gs_sys_reset(GS_SYS_RESET_CSP);
}

265
gomspace/libgscsp/src/linux/command_line.c Normal file
View File

@ -0,0 +1,265 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include "../local.h"
#include <gs/csp/drivers/i2c/i2c.h>
#include <csp/interfaces/csp_if_kiss.h>
#include <csp/interfaces/csp_if_can.h>
#include <csp/interfaces/csp_if_i2c.h>
#include <csp/interfaces/csp_if_zmqhub.h>
#include <csp/drivers/usart.h>
#include <csp/drivers/can_socketcan.h>
#include <gs/util/linux/function.h>
#include <gs/util/linux/drivers/i2c/i2c.h>
#include <gs/util/string.h>
#define IF_NAME "if"
#define DEFAULT_CAN_DEVICE "can0"
#define DEFAULT_KISS_IF_NAME "KISS"
#define DEFAULT_KISS_DEVICE "/dev/ttyUSB0"
#define KISS_SPEED "speed"
#define DEFAULT_KISS_SPEED 500000
#define DEFAULT_ZMQ_SERVER "localhost"
#define DEFAULT_I2C_DEVICE "0"
#define CSP_ADDRESS_NOT_SET 255
#define DEFAULT_CSP_ADDRESS 8
static uint8_t csp_address = CSP_ADDRESS_NOT_SET;
static const char * csp_can_device = NULL;
static const char * csp_kiss_device = NULL;
static const char * csp_i2c_device = NULL;
static const char * csp_zmq_server = NULL;
static const char * csp_rtable = NULL;
static int parser(int key, char *arg, struct argp_state *state)
{
switch (key) {
case 'a':
return gs_string_to_uint8(arg, &csp_address);
case 'c':
if (csp_can_device) {
return GS_ERROR_IN_USE;
}
if (arg) {
csp_can_device = arg;
} else {
csp_can_device = DEFAULT_CAN_DEVICE;
}
break;
case 'k':
if (csp_kiss_device) {
return GS_ERROR_IN_USE;
}
if (arg) {
csp_kiss_device = arg;
} else {
csp_kiss_device = DEFAULT_KISS_DEVICE;
}
break;
case 'i':
if (csp_i2c_device) {
return GS_ERROR_IN_USE;
}
if (arg) {
csp_i2c_device = arg;
} else {
csp_i2c_device = DEFAULT_I2C_DEVICE;
}
break;
case 'z':
if (csp_zmq_server) {
return GS_ERROR_IN_USE;
}
if (arg) {
csp_zmq_server = arg;
} else {
csp_zmq_server = DEFAULT_ZMQ_SERVER;
}
break;
case 'R':
csp_rtable = arg;
break;
default:
return ARGP_ERR_UNKNOWN;
}
return 0;
}
static const struct argp_option options[] = {
{
.name = "csp-address",
.key = 'a',
.arg = "ADDR",
.flags = 0,
.doc = "Set address, default: " GS_DEF2STRING(DEFAULT_CSP_ADDRESS)
},
{
.name = "csp-rtable",
.key = 'R',
.arg = "RTABLE",
.flags = 0,
.doc = "Set routing table\nRTABLE=<address>/<mask> <interface> [mac]\nExample: \"0/0 ZMQHUB 24, 24/2 ZMQHUB\""
},
#if (CSP_USE_CAN)
{
.name = "csp-can",
.key = 'c',
.arg = "DEVICE",
.flags = OPTION_ARG_OPTIONAL,
.doc = "Add CAN interface\nDEVICE=" DEFAULT_CAN_DEVICE
},
#endif
#if (CSP_USE_KISS)
{
.name = "csp-kiss",
.key = 'k',
.arg = "DEVICE",
.flags = OPTION_ARG_OPTIONAL,
.doc = "Add KISS over UART interface\nDEVICE=" DEFAULT_KISS_DEVICE "," IF_NAME "=" DEFAULT_KISS_IF_NAME","KISS_SPEED"=" GS_DEF2STRING(DEFAULT_KISS_SPEED)
},
#endif
#if (CSP_USE_I2C)
{
.name = "csp-i2c",
.key = 'i',
.arg = "DEVICE",
.flags = OPTION_ARG_OPTIONAL,
.doc = "Add I2C interface\nDEVICE=0,"GS_I2C_COMMAND_LINE_SPEED"=" GS_DEF2STRING(GS_I2C_DEFAULT_BPS) "," GS_I2C_COMMAND_LINE_ADDRESS "=1," GS_I2C_COMMAND_LINE_DEVICE "=" GS_DEF2STRING(GS_I2C_ALL_DEVICES)
},
#endif
#if (CSP_USE_ZMQHUB)
{
.name = "csp-zmq",
.key = 'z',
.arg = "SERVER",
.flags = OPTION_ARG_OPTIONAL,
.doc = "Add ZMQ interface\nSERVER=" DEFAULT_ZMQ_SERVER
},
#endif
{
.flags = OPTION_DOC,
.name = "Examples:"
#if (CSP_USE_CAN)
"\n CAN: configure address 10 and CAN interface can0:"
"\n $ <application> -a10 -ccan0"
#endif
#if (CSP_USE_KISS)
"\n KISS: configure address 10 and uart on /dev/ttyUSB0 at baudrate 500000:"
"\n $ <application> -a10 -k/dev/ttyUSB0,speed=500000"
#endif
#if (CSP_USE_I2C)
"\n I2C: configure address 10 and I2C Aardvark dongle with id 2238384015, speed 400K:"
"\n $ <application> -a10 -i2238384015,speed=400000"
#endif
#if (CSP_USE_ZMQHUB)
"\n ZMQ: configure address 10 and ZMQ proxy on localhost:"
"\n $ <application> -a10 -zlocalhost"
#endif
},
{0}
};
static const struct argp argp = {.options = options, .parser = parser};
const struct argp_child gs_csp_command_line_options = {.argp = &argp, .header = "CSP"};
gs_error_t gs_csp_command_line_configure_interfaces(void)
{
#if (CSP_USE_KISS)
// KISS - only here, because the embedded init functions are stubbed in libemul
if (csp_kiss_device) {
static char device[50];
static char ifname[50];
uint32_t speed;
int res = gs_string_get_suboption_string(csp_kiss_device, NULL, DEFAULT_KISS_DEVICE, device, sizeof(device));
res |= gs_string_get_suboption_string(csp_kiss_device, IF_NAME, DEFAULT_KISS_IF_NAME, ifname, sizeof(ifname));
res |= gs_string_get_suboption_uint32(csp_kiss_device, KISS_SPEED, DEFAULT_KISS_SPEED, &speed);
if (res == GS_OK) {
static csp_iface_t csp_if_kiss;
static csp_kiss_handle_t csp_kiss_driver;
csp_kiss_init(&csp_if_kiss, &csp_kiss_driver, usart_putc, usart_insert, ifname);
struct usart_conf conf = {.device = device, .baudrate = speed};
usart_init(&conf);
void my_usart_rx(uint8_t * buf, int len, void * pxTaskWoken) {
csp_kiss_rx(&csp_if_kiss, buf, len, pxTaskWoken);
}
usart_set_callback(my_usart_rx);
}
}
#endif
#if (CSP_USE_CAN)
// CAN - only here, because the embedded init functions are stubbed in libemul
if (csp_can_device) {
char device[50];
int res = gs_string_get_suboption_string(csp_can_device, NULL, DEFAULT_CAN_DEVICE, device, sizeof(device));
if (res == GS_OK) {
csp_can_socketcan_init(device, 0, 0);
}
}
#endif
#if (CSP_USE_ZMQHUB)
// ZMQ - currently ZMQ is only supported on Linux, and therefor handled here
if (csp_zmq_server) {
char server[50];
int res = gs_string_get_suboption_string(csp_zmq_server, NULL, DEFAULT_ZMQ_SERVER, server, sizeof(server));
if (res == GS_OK) {
csp_zmqhub_init(csp_get_address(), server);
}
}
#endif
#if (CSP_USE_I2C)
// I2C
if (csp_i2c_device) {
uint8_t device = 0;
gs_string_get_suboption_uint8(csp_i2c_device, GS_I2C_COMMAND_LINE_DEVICE, GS_I2C_ALL_DEVICES, &device);
if (device == GS_I2C_ALL_DEVICES) {
device = 0;
}
char modified_options[300];
snprintf(modified_options, sizeof(modified_options), "%s,%s=%u", csp_i2c_device, GS_I2C_COMMAND_LINE_ADDRESS, csp_get_address());
gs_error_t error = gs_function_invoke("i2c", modified_options);
if (error) {
log_error("Failed to initialize I2C adapter, error: %s", gs_error_string(error));
} else {
error = gs_csp_i2c_init(device, csp_get_address());
if (error) {
log_error("gs_csp_i2c_init(%u, %u) failed, error: %s", device, csp_get_address(), gs_error_string(error));
}
}
}
#endif
return GS_OK;
}
bool gs_csp_command_line_is_address_set(void)
{
return (csp_address != CSP_ADDRESS_NOT_SET);
}
uint8_t gs_csp_command_line_get_address(void)
{
if (gs_csp_command_line_is_address_set()) {
return csp_address;
}
return DEFAULT_CSP_ADDRESS;
}
const char * gs_csp_command_line_get_rtable(void)
{
return csp_rtable;
}

21
gomspace/libgscsp/src/local.h Normal file
View File

@ -0,0 +1,21 @@
#ifndef GS_CSP_SRC_LOCAL_H
#define GS_CSP_SRC_LOCAL_H
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/error.h>
#include <gs/util/log.h>
#if (__linux__)
#define GS_CSP_COMMAND_LINE_SUPPORT 1
#include <gs/csp/linux/command_line.h>
#endif
GS_LOG_GROUP_EXTERN(gs_csp_log);
#define LOG_DEFAULT gs_csp_log
// local command line APIs
bool gs_csp_command_line_is_address_set(void);
uint8_t gs_csp_command_line_get_address(void);
const char * gs_csp_command_line_get_rtable(void);
gs_error_t gs_csp_command_line_configure_interfaces(void);
#endif

64
gomspace/libgscsp/src/log.c Normal file
View File

@ -0,0 +1,64 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/log.h>
#include <gs/util/log/log.h>
#include <csp/csp.h>
#include "local.h"
GS_LOG_GROUP(gs_csp_log, "csp", GS_LOG_CAT_CSP, LOG_DEFAULT_MASK);
static void gs_log_csp_debug_hook(csp_debug_level_t level, const char *format, va_list args)
{
gs_log_level_t mapped_level;
switch (level) {
/* Regular log levels */
default:
case CSP_ERROR:
mapped_level = LOG_ERROR;
break;
case CSP_WARN:
mapped_level = LOG_WARNING;
break;
case CSP_INFO:
mapped_level = LOG_INFO;
break;
/* Extended log levels */
case CSP_BUFFER:
mapped_level = LOG_TRACE;
break;
case CSP_PACKET:
mapped_level = LOG_INFO;
break;
case CSP_PROTOCOL:
mapped_level = LOG_DEBUG;
break;
case CSP_LOCK:
mapped_level = LOG_TRACE;
break;
}
const int do_log = ((LOG_DEFAULT->mask & (1 << mapped_level)) > 0);
if (do_log) {
/* forward to log system */
gs_log_va(mapped_level, LOG_DEFAULT, format, args);
}
}
gs_error_t gs_csp_log_init(void)
{
gs_log_group_register(LOG_DEFAULT);
csp_debug_set_level(CSP_ERROR, true);
csp_debug_set_level(CSP_WARN, true);
csp_debug_set_level(CSP_INFO, true);
csp_debug_set_level(CSP_BUFFER, true);
csp_debug_set_level(CSP_PACKET, true);
csp_debug_set_level(CSP_PROTOCOL, true);
csp_debug_set_level(CSP_LOCK, true);
csp_debug_hook_set(gs_log_csp_debug_hook);
return GS_OK;
}

84
gomspace/libgscsp/src/router.c Normal file
View File

@ -0,0 +1,84 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/csp/router.h>
#include <gs/csp/csp.h>
#include <gs/csp/conn.h>
#include <csp/csp_autoconfig.h>
#include <gs/util/log.h>
#include <gs/util/check.h>
#include <gs/util/time.h>
#include "../lib/libcsp/src/csp_qfifo.h" // internal libcsp header -> FIFO_TIMEOUT, csp_qfifo_wake_up()
#include <../lib/libcsp/src/csp_conn.h> // internal libcsp header
#include "local.h"
typedef struct {
bool run;
gs_thread_t thread;
} gs_csp_router_t;
static gs_csp_router_t gs_csp_router;
static void * gs_csp_router_task(void *param)
{
/* Here there be routing */
while (gs_csp_router.run) {
csp_route_work(FIFO_TIMEOUT);
}
log_info("CSP router task terminating");
gs_thread_exit(0);
}
gs_error_t gs_csp_router_task_stop(void)
{
GS_CHECK_HANDLE(gs_csp_router.run && (gs_csp_router.thread != 0));
// Close connections in state "RDP closing" - instead of waiting for timeout
#ifdef CSP_USE_RDP
{
size_t max_connections;
const csp_conn_t * conn = csp_conn_get_array(&max_connections);
if (conn && max_connections) {
for (unsigned int i = 0; i < max_connections; ++i, ++conn) {
if ((conn->state == CONN_OPEN) && (conn->rdp.state == RDP_CLOSE_WAIT)) {
log_info("Force close RDP %p in state closing", conn);
csp_close((csp_conn_t *) conn);
}
}
}
}
#endif
// wait for RDP connections to close
unsigned int open = gs_csp_conn_get_open();
if (open) {
const unsigned int MAX_TIMEOUT_MS = 30000;
log_info("Waiting up to %u mS for %u connection(s) to timeout/close ...", MAX_TIMEOUT_MS, open);
const uint32_t start_ms = gs_time_rel_ms();
while (gs_csp_conn_get_open() && (gs_time_diff_ms(start_ms, gs_time_rel_ms()) < MAX_TIMEOUT_MS)) {
gs_time_sleep_ms(200);
}
}
log_info("Waiting for CSP router task to stop ...");
gs_csp_router.run = false;
csp_qfifo_wake_up();
gs_error_t error = gs_thread_join(gs_csp_router.thread, NULL);
memset(&gs_csp_router, 0, sizeof(gs_csp_router));
log_info("CSP router task stopped");
return error;
}
gs_error_t gs_csp_router_task_start(size_t stack_size, gs_thread_priority_t priority)
{
if (gs_csp_router.run) {
return GS_ERROR_IN_USE;
}
gs_csp_router.run = true;
gs_error_t error = gs_thread_create("RTE", gs_csp_router_task, NULL, stack_size, priority,
GS_THREAD_CREATE_JOINABLE, &gs_csp_router.thread);
if (error) {
memset(&gs_csp_router, 0, sizeof(gs_csp_router));
}
return error;
}

69
gomspace/libgscsp/src/rtable.c Normal file
View File

@ -0,0 +1,69 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/rtable.h>
#include <gs/util/string.h>
#include "local.h"
// Return interface, if only one configured
static csp_iface_t * get_single_if(unsigned int * return_count)
{
unsigned int count = 0;
csp_iface_t * found = NULL;
for (csp_iface_t * ifc = csp_iflist_get(); ifc; ifc = ifc->next) {
if (strcasecmp(ifc->name, "LOOP") == 0) {
// ignore loopback
} else {
++count;
found = ifc;
}
}
*return_count = count;
return found;
}
gs_error_t gs_csp_rtable_load(const char * rtable, bool set_default_route, bool use_command_line_option)
{
//csp_rtable_clear();
#if GS_CSP_COMMAND_LINE_SUPPORT
if (use_command_line_option && gs_string_empty(rtable)) {
// try rtable from command line (if set)
rtable = gs_csp_command_line_get_rtable();
}
#endif
if (gs_string_empty(rtable) == false) {
if (csp_rtable_check(rtable) > 0) {
csp_rtable_load(rtable);
log_info("%s: loaded routing table [%s]", __FUNCTION__, rtable);
return GS_OK;
}
log_warning("%s: ignoring route table: [%s] due to error(s)", __FUNCTION__, rtable);
}
if (set_default_route) {
unsigned int count = 0;
csp_iface_t * ifc = get_single_if(&count);
if (count == 0) {
log_warning("%s: no interfaces configured", __FUNCTION__);
return GS_ERROR_NOT_FOUND;
}
if (count > 1) {
log_warning("%s: %u interfaces configured - will not set default routes", __FUNCTION__, count);
return GS_ERROR_AMBIGUOUS;
}
// set default route
int res = csp_route_set(CSP_DEFAULT_ROUTE, ifc, CSP_NODE_MAC);
if (res != CSP_ERR_NONE) {
log_warning("%s: failed to set default route on interface: [%s], CSP error: %d", __FUNCTION__, ifc->name, res);
return gs_csp_error(res);
}
}
return GS_OK;
}

213
gomspace/libgscsp/src/service_dispatcher.c Normal file
View File

@ -0,0 +1,213 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/service_dispatcher.h>
#include <gs/util/watchdog/watchdog.h>
#include <gs/util/check.h>
#include <gs/util/log.h>
#include <stdlib.h>
#include "../lib/libcsp/src/csp_conn.h" // internal libcsp header
#include "local.h"
static GS_LOG_GROUP(gs_cspdispatcher_log, "cspdispatcher", GS_LOG_CAT_CSP, LOG_DEFAULT_MASK);
#undef LOG_DEFAULT
#define LOG_DEFAULT gs_cspdispatcher_log
#define WD_TIMEOUT_SECOUNDS 30
#define DEFAULT_TIMEOUT_MS (((WD_TIMEOUT_SECOUNDS * 1000) / 3) * 2)
struct gs_csp_service_dispatcher {
// Configuration
const gs_csp_service_dispatcher_conf_t * conf;
// Run or stop/exit.
bool run;
// Server socket
csp_socket_t * socket;
// Software watchdog
gs_swwd_hdl_t * wd;
// Thread handle.
gs_thread_t thread;
};
static void * service_dispatcher_task(void * parameter)
{
gs_csp_service_dispatcher_t handle = parameter;
unsigned int timeout_ms = (handle->conf->callback) ? 0 : DEFAULT_TIMEOUT_MS;
log_debug("[%s] entering connection loop, timeout: %u mS", handle->conf->name, timeout_ms);
while (handle->run) {
if (handle->wd) {
gs_swwd_touch(handle->wd);
}
/* Wait for incoming connection, or timeout */
csp_conn_t * conn = csp_accept(handle->socket, timeout_ms);
if (conn) {
unsigned int in_port = csp_conn_dport(conn);
log_debug("[%s] new connection %p on port: %u, source: %d:%d, flags: 0x%x",
handle->conf->name, conn, in_port, csp_conn_src(conn), csp_conn_sport(conn), csp_conn_flags(conn));
gs_csp_service_handler_t handler;
if (in_port < handle->conf->handler_array_size) {
handler = handle->conf->handler_array[in_port];
} else {
handler = NULL;
}
if (handler) {
gs_error_t error = (handler)(conn);
log_debug("[%s] connection on port: %u processed by %p, error: %s",
handle->conf->name, in_port, handler, gs_error_string(error));
} else {
log_warning("[%s] no handler on port: %u - closing connection: source: %d:%d, flags: 0x%x",
handle->conf->name, in_port, csp_conn_src(conn), csp_conn_sport(conn), csp_conn_flags(conn));
csp_close(conn);
}
}
if (handle->conf->callback) {
timeout_ms = handle->conf->callback();
if (timeout_ms > DEFAULT_TIMEOUT_MS) {
timeout_ms = DEFAULT_TIMEOUT_MS;
}
}
}
log_debug("[%s] terminating ...", handle->conf->name);
gs_thread_exit(NULL);
}
static gs_error_t service_dispatcher_create(const gs_csp_service_dispatcher_conf_t * conf,
size_t stack_size,
gs_thread_priority_t priority,
gs_csp_service_dispatcher_t * return_handle)
{
gs_csp_service_dispatcher_t handle = calloc(1, sizeof(*handle));
if (handle == 0) {
return GS_ERROR_ALLOC;
}
*return_handle = handle;
handle->conf = conf;
// Create watchdog
if (conf->disable_watchdog == false) {
gs_error_t error = gs_swwd_register(&handle->wd, WD_TIMEOUT_SECOUNDS, NULL, NULL, conf->name);
if (error) {
log_error("[%s] gs_swwd_register(%s, %u) failed, error: %d",
conf->name, conf->name, WD_TIMEOUT_SECOUNDS, error);
handle->wd = NULL;
return error;
}
}
// Open "server" socket
handle->socket = csp_socket(conf->socket_options);
if (handle->socket == NULL) {
log_error("[%s] csp_socket(0) failed",
conf->name);
return GS_ERROR_ALLOC;
}
// Bind to port(s) to socket
for (unsigned int i = 0; i < conf->handler_array_size; ++i) {
if (conf->handler_array[i]) {
int res = csp_bind(handle->socket, i);
if (res) {
log_error("[%s] csp_bind(socket: %p, port: %u) failed, result: %d",
conf->name, handle->socket, i, res);
return GS_ERROR_IN_USE;
}
}
}
// Bind on "any" port?
if (conf->bind_any) {
int res = csp_bind(handle->socket, CSP_ANY);
if (res) {
log_error("[%s] csp_bind(socket: %p, port: %u) failed, result: %d",
conf->name, handle->socket, CSP_ANY, res);
return GS_ERROR_IN_USE;
}
}
// Create listen backlog
{
size_t backlog = conf->listen_backlog ? conf->listen_backlog : 10;
int res = csp_listen(handle->socket, backlog);
if (res) {
log_error("[%s] csp_listen(%p, %zu) failed, result: %d",
conf->name, handle->socket, backlog, res);
return GS_ERROR_UNKNOWN;
}
}
// Launch thread
handle->run = true;
gs_error_t error = gs_thread_create(handle->conf->name, service_dispatcher_task, handle, stack_size, priority,
GS_THREAD_CREATE_JOINABLE, &handle->thread);
if (error) {
handle->thread = 0;
}
return error;
}
gs_error_t gs_csp_service_dispatcher_create(const gs_csp_service_dispatcher_conf_t * conf,
size_t stack_size,
gs_thread_priority_t priority,
gs_csp_service_dispatcher_t * return_handle)
{
GS_CHECK_ARG(conf != NULL);
gs_log_group_register(gs_cspdispatcher_log);
gs_csp_service_dispatcher_t handle;
gs_error_t error = service_dispatcher_create(conf, stack_size, priority, &handle);
if (error) {
//gs_csp_service_dispatcher_destroy(handle);
handle = NULL;
}
if (return_handle) {
*return_handle = handle;
}
return error;
}
gs_error_t gs_csp_service_dispatcher_wake_up(gs_csp_service_dispatcher_t handle)
{
GS_CHECK_HANDLE(handle && handle->socket && handle->socket->socket);
csp_packet_t * packet = NULL;
int res = csp_queue_enqueue(handle->socket->socket, &packet, 0);
return (res == CSP_QUEUE_OK) ? GS_OK : GS_ERROR_FULL;
}
gs_error_t gs_csp_service_dispatcher_destroy(gs_csp_service_dispatcher_t handle)
{
GS_CHECK_HANDLE(handle && handle->conf);
log_debug("[%s] stopping dispatcher ...", handle->conf->name);
handle->run = false;
if (handle->thread) {
gs_csp_service_dispatcher_wake_up(handle);
gs_thread_join(handle->thread, NULL);
}
csp_close(handle->socket);
if (handle->wd) {
gs_swwd_deregister(&handle->wd);
}
memset(handle, 0, sizeof(*handle));
free(handle);
return GS_OK;
}

86
gomspace/libgscsp/src/service_handler.c Normal file
View File

@ -0,0 +1,86 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/csp/service_handler.h>
#include <gs/util/time.h>
#include <gs/util/byteorder.h>
#include <gs/util/drivers/sys/memory.h>
// callback for processing packets on a connection.
typedef void (*csp_service_packet_handler_t)(csp_conn_t * conn, csp_packet_t * packet);
// Process all packets on the connectio and close it when done.
static gs_error_t call_csp_packet_handler(csp_conn_t * conn, csp_service_packet_handler_t handler)
{
csp_packet_t *packet;
while ((packet = csp_read(conn, 0))) {
(handler)(conn, packet);
}
csp_close(conn);
return GS_OK;
}
gs_error_t gs_csp_cmp_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, csp_service_handler);
}
gs_error_t gs_csp_ping_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, csp_service_handler);
}
gs_error_t gs_csp_ps_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, csp_service_handler);
}
static void memfree(csp_conn_t * conn, csp_packet_t * packet)
{
uint32_t mem_free = 0;
gs_mem_ram_type_t ram_type = gs_mem_get_ram_default();
gs_mem_ram_stat_t ram_stat;
if(gs_mem_get_ram_stat(ram_type, &ram_stat) == GS_OK) {
mem_free = ram_stat.available;
}
mem_free = util_hton32(mem_free);
memcpy(packet->data, &mem_free, sizeof(mem_free));
packet->length = sizeof(mem_free);
if (!csp_send(conn, packet, 0)) {
csp_buffer_free(packet);
}
}
gs_error_t gs_csp_mem_free_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, memfree);
}
gs_error_t gs_csp_reboot_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, csp_service_handler);
}
gs_error_t gs_csp_buf_free_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, csp_service_handler);
}
static void uptime(csp_conn_t * conn, csp_packet_t * packet)
{
uint32_t time = gs_time_uptime();
time = util_hton32(time);
memcpy(packet->data, &time, sizeof(time));
packet->length = sizeof(time);
if (!csp_send(conn, packet, 0)) {
csp_buffer_free(packet);
}
}
gs_error_t gs_csp_uptime_service_handler(csp_conn_t * conn)
{
return call_csp_packet_handler(conn, uptime);
}

67
gomspace/libgscsp/src/transaction.c Normal file
View File

@ -0,0 +1,67 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/csp/csp.h>
#include <gs/util/check.h>
gs_error_t gs_csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, const void * tx_buf,
size_t tx_len, void * rx_buf, size_t rx_max_len, size_t * rx_len)
{
GS_CHECK_HANDLE(conn != NULL);
size_t size = (rx_max_len > tx_len) ? rx_max_len : tx_len;
csp_packet_t * packet = csp_buffer_get(size);
if (packet == NULL) {
return GS_ERROR_ALLOC;
}
/* Copy the request */
if (tx_len > 0 && tx_buf != NULL) {
memcpy(packet->data, tx_buf, tx_len);
}
packet->length = tx_len;
if (!csp_send(conn, packet, timeout)) {
csp_buffer_free(packet);
return GS_ERROR_IO;
}
/* If no reply is expected, return now */
if (rx_max_len == 0) {
return GS_OK;
}
packet = csp_read(conn, timeout);
if (packet == NULL) {
return GS_ERROR_IO;
}
gs_error_t return_val;
if (rx_max_len >= packet->length) {
size = packet->length;
return_val = GS_OK;
} else {
csp_log_error("Reply length %u, buffer only %u", packet->length, rx_max_len);
size = rx_max_len;
return_val = GS_ERROR_OVERFLOW;
}
memcpy(rx_buf, packet->data, size);
*rx_len = packet->length;
csp_buffer_free(packet);
return return_val;
}
gs_error_t gs_csp_transaction2(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, const void * tx_buf,
size_t tx_len, void * rx_buf, size_t rx_max_len, size_t * rx_len, uint32_t opts)
{
csp_conn_t * conn = csp_connect(prio, dest, port, 0, opts);
if (conn == NULL) {
return GS_ERROR_HANDLE;
}
gs_error_t res = gs_csp_transaction_persistent(conn, timeout, tx_buf, tx_len, rx_buf, rx_max_len, rx_len);
csp_close(conn);
return res;
}