diff --git a/include/villas/nodes/gtwif.h b/include/villas/nodes/gtwif.h index 91c8555bb..19676b4a7 100644 --- a/include/villas/nodes/gtwif.h +++ b/include/villas/nodes/gtwif.h @@ -29,13 +29,28 @@ #pragma once -#include +#include #include "node.h" #include "list.h" +#include "rscad.h" struct gtwif { + struct sockaddr_in remote; /**< The IP / port address of the RTDS racks. */ + + struct gtwif_direction { + uint32_t *addresses; + int count; + } in, out; + struct rscad_inf inffile; /**< The RSCAD case information file. */ + + int sd; /**< The socket descriptor. */ + + double rate; /**< The polling rate. */ + double timeout; /**< The recv() timeout. */ + + struct sample *last; /**< The last sample which has been sent by this node. */ }; /** @} */ \ No newline at end of file diff --git a/lib/nodes/gtwif.c b/lib/nodes/gtwif.c index 462450df7..e174f3cba 100644 --- a/lib/nodes/gtwif.c +++ b/lib/nodes/gtwif.c @@ -20,23 +20,198 @@ * along with this program. If not, see . *********************************************************************************/ -#include +#include +#include +#include #include "plugin.h" +#include "timing.h" #include "nodes/gtwif.h" -#include "utils.h" -#include "msg.h" -int gtwif_reverse(struct node *n) +#define GTWIF_CMD_MODIFY 0x004D +#define GTWIF_CMD_READLIST2 0x0143 + +#define GTWIF_MAX_RATE 10.0 + +static int gtwif_cmd_readlist2(int sd, uint32_t cnt, uint32_t addrs[], uint32_t vals[]) { - struct gtwif *g __attribute__((unused)) = n->_vd; + ssize_t sent, recvd; + + uint32_t buf[cnt + 4]; + + int retries = 3; + + buf[0] = htons(GTWIF_CMD_READLIST2); + buf[1] = 0; + buf[2] = 0; + buf[3] = cnt; + + for (int i = 0; i < cnt; i++) + buf[i+3] = addrs[i]; + +retry: sent = send(sd, buf, sizeof(buf), 0); + if (sent < 0) + return -1; + + recvd = recv(sd, buf, sizeof(buf), 0); + if (recvd < 0) { + if (errno == ETIMEDOUT && retries > 0) { + retries--; + goto retry; + } + else + return -1; + } + + if (buf[0] != htons(GTWIF_CMD_READLIST2)) + return -1; + + /* We update the number of values with the returned number of values */ + cnt = buf[1]; + + for (int i = 0; i < cnt; i++) + vals[i] = buf[i+3]; + + return cnt; +} + +static int gtwif_cmd_modify(int sd, uint32_t addr, uint32_t val) +{ + ssize_t sent, recvd; + uint32_t buf[4]; + int retries = 3; + + buf[0] = htons(GTWIF_CMD_MODIFY); + buf[1] = htonl(addr); + buf[2] = htonl(1); + buf[3] = htonl(val); + +retry: sent = send(sd, buf, sizeof(buf), 0); + if (sent < 0) + return -1; + + recvd = recv(sd, buf, sizeof(buf), 0); + if (recvd < 0) { + if (errno == ETIMEDOUT && retries > 0) { + retries--; + goto retry; + } + else + return -1; + } + + return 0; +} + +/** Sends multiple CMD_MODIFY commands to the GTWIF card. + * + * We we only modify values which have been changed. + * This is checked by comparing \p vals[] with \p old_vals[] + */ +static int gtwif_cmd_modify_many(int sd, uint32_t cnt, uint32_t addrs[], uint32_t vals[], uint32_t old_vals[]) +{ + int ret, mod = 0; + + for (int i = 0; i < cnt; i++) { + if (old_vals && vals[i] == old_vals[i]) + continue; + + ret = gtwif_cmd_modify(sd, addrs[i], vals[i]); + if (ret) + continue; + + mod++; + } + + return mod; +} + +static int gtwif_parse_direction(struct gtwif_direction *d, config_setting_t *cfg) +{ + if (config_setting_type(cfg) != CONFIG_TYPE_ARRAY) + cerror(cfg, "GTWIF node 'in' / 'out' must be any array of addresses or strings"); + + d->count = config_setting_length(cfg); + d->addresses = alloc(d->count * sizeof(d->addresses[0])); + + for (int i = 0; i < d->count; i++) { + config_setting_t *cfg_elm = config_setting_get_elem(cfg, i); + + switch (config_setting_type(cfg_elm)) { +#if 0 + case CONFIG_TYPE_STRING: { + const char *elm = config_setting_get_string(cfg, i); + + + + break; + } +#endif + case CONFIG_TYPE_INT: + d->addresses[i] = config_setting_get_int(cfg); + break; + + default: + cerror(cfg_elm, "Invalid type"); + } + } return 0; } int gtwif_parse(struct node *n, config_setting_t *cfg) { - struct gtwif *g __attribute__((unused)) = n->_vd; + struct gtwif *g = n->_vd; + + int ret; + const char *remote; + config_setting_t *cfg_in, *cfg_out, *cfg_remote, *cfg_rate; + + cfg_in = config_setting_get_member(cfg, "in"); + if (cfg_in) { + ret = gtwif_parse_direction(&g->in, cfg_in); + if (ret) + cerror(cfg_in, "Failed to parse inputs of GTWIF node: %s", node_name(n)); + + } + + cfg_out = config_setting_get_member(cfg, "out"); + if (cfg_out) { + ret = gtwif_parse_direction(&g->out, cfg_out); + if (ret) + cerror(cfg_out, "Failed to parse outputs of GTWIF node: %s", node_name(n)); + + } + + if (!config_setting_lookup_float(cfg, "timeout", &g->timeout)) + g->timeout = 1; + + cfg_rate = config_setting_get_member(cfg, "rate"); + if (cfg_rate) { + g->rate = config_setting_get_float(cfg_rate); + if (g->rate == 0) + cerror(cfg_rate, "The GTWIF node %s has an invalid rate", node_name(n)); + if (g->rate > GTWIF_MAX_RATE) + cerror(cfg_rate, "The GTWIF node %s 'rate' setting (%f) exceeds the allowed maximum (%f)", node_name(n), g->rate, GTWIF_MAX_RATE); + } + else + g->rate = 1; + + cfg_remote = config_setting_get_member(cfg, "remote"); + if (cfg_remote) { + remote = config_setting_get_string(cfg_remote); + if (!remote) + cerror(cfg_remote, "The 'remote' setting must be a string!"); + + ret = inet_aton(remote, &g->remote.sin_addr); + if (!ret) + cerror(cfg_remote, "The setting 'remote' = %s is not a valid IP address!", remote); + + g->remote.sin_family = AF_INET; + g->remote.sin_port = htons(2); + } + else + cerror(cfg, "GTWIF node %s is missing setting 'rack'", node_name(n)); return 0; } @@ -51,36 +226,80 @@ char * gtwif_print(struct node *n) int gtwif_start(struct node *n) { - struct gtwif *g __attribute__((unused)) = n->_vd; + struct gtwif *g = n->_vd; + int ret; + + g->sd = socket(PF_INET, SOCK_DGRAM, 0); + if (g->sd < 0) + return -1; + + ret = connect(g->sd, &g->remote, sizeof(g->remote)); + if (ret) + return ret; + + struct timeval tv = { + .tv_sec = (int) g->timeout, + .tv_usec = fmod(g->timeout, 1.0) * 1e6 + }; + + setsockopt(g->sd, SOL_SOCKET, SO_RCVTIMEO, (const char *) &tv, sizeof(tv)); + + g->last = NULL; return 0; } int gtwif_stop(struct node *n) { - struct gtwif *g __attribute__((unused)) = n->_vd; - - return 0; -} - -int gtwif_deinit() -{ + int ret; + struct gtwif *g = n->_vd; + + ret = close(g->sd); + if (ret) + return ret; + + sample_put(g->last); return 0; } int gtwif_read(struct node *n, struct sample *smps[], unsigned cnt) { - struct gtwif *g __attribute__((unused)) = n->_vd; + int ret; + struct gtwif *g = n->_vd; + struct sample *smp = smps[0]; + + uint32_t count = MIN(g->in.count, smp->capacity); + uint32_t *vals = (uint32_t *) &smp->data; - return 0; + ret = gtwif_cmd_readlist2(g->sd, count, g->in.addresses, vals); + if (ret < 0) + return ret; + + return 1; } int gtwif_write(struct node *n, struct sample *smps[], unsigned cnt) { - struct gtwif *g __attribute__((unused)) = n->_vd; + int ret; + struct gtwif *g = n->_vd; + + struct sample *smp = smps[0]; + + uint32_t count = MIN(g->out.count, smp->length); + uint32_t *vals = (uint32_t *) &smp->data; + uint32_t *old_vals = (uint32_t *) (g->last ? &g->last->data : NULL); + + ret = gtwif_cmd_modify_many(g->sd, count, g->out.addresses, vals, old_vals); + if (ret < 0) + return ret; + + sample_get(smp); + sample_put(g->last); + + g->last = smp; - return 0; + return 1; } static struct plugin p = { @@ -88,14 +307,12 @@ static struct plugin p = { .description = "GTWIF - RSCAD protocol for RTDS", .type = PLUGIN_TYPE_NODE, .node = { - .vectorize = 0, + .vectorize = 1, .size = sizeof(struct gtwif), - .reverse = gtwif_reverse, .parse = gtwif_parse, .print = gtwif_print, .start = gtwif_start, .stop = gtwif_stop, - .deinit = gtwif_deinit, .read = gtwif_read, .write = gtwif_write, .instances = LIST_INIT()