diff --git a/include/villas/nodes/can.hpp b/include/villas/nodes/can.hpp
index 2e0504f47..5b721a9bb 100644
--- a/include/villas/nodes/can.hpp
+++ b/include/villas/nodes/can.hpp
@@ -50,7 +50,7 @@ struct can {
/* States */
int socket;
- union signal_data *data;
+ union signal_data *sample_buf;
struct timespec start_time;
};
diff --git a/lib/nodes/can.cpp b/lib/nodes/can.cpp
index 2c7dd7fd0..d2ff8bc21 100644
--- a/lib/nodes/can.cpp
+++ b/lib/nodes/can.cpp
@@ -21,6 +21,7 @@
* along with this program. If not, see .
*********************************************************************************/
+#include "villas/signal_type.h"
#include
#include
#include
@@ -66,6 +67,9 @@ int can_init(struct node *n)
c->interface_name = nullptr;
c->socket = 0;
+ c->sample_buf = nullptr;
+ c->in = nullptr;
+ c->out = nullptr;
return 0;
}
@@ -78,6 +82,7 @@ int can_destroy(struct node *n)
if (c->socket != 0) {
close(c->socket);
}
+ free(c->sample_buf);
free(c->in);
free(c->out);
return 0;
@@ -105,11 +110,6 @@ int can_parse_signal(json_t *json, struct vlist *node_signals, struct can_signal
goto out;
}
- /*if (!name) {
- error("No signale name specified for signal.");
- goto out;
- }*/
-
if (can_size > 8 || can_size <= 0) {
error("can_size of %d for signal \"%s\" is invalid. You must satisfy 0 < can_size <= 8.", can_size, name);
goto out;
@@ -217,20 +217,16 @@ int can_check(struct node *n)
int can_prepare(struct node *n)
{
- //struct can *c = (struct can *) n->_vd;
-
- /* TODO: Add implementation here. The following is just an can */
- //
- // c->state1 = c->setting1;
- //
- // if (strcmp(c->setting2, "double") == 0)
- // c->state1 *= 2;
-
- return 0;
+ struct can *c = (struct can *) n->_vd;
+ c->sample_buf = (union signal_data*) calloc(
+ vlist_length(&n->in.signals),
+ sizeof(union signal_data));
+ return (c->sample_buf != 0 ? 0 : 1);
}
int can_start(struct node *n)
{
+ int ret = 1;
struct sockaddr_can addr = {0};
struct ifreq ifr;
@@ -239,13 +235,13 @@ int can_start(struct node *n)
if((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
error("Error while opening CAN socket");
- return 1;
+ goto out;
}
strcpy(ifr.ifr_name, c->interface_name);
if (ioctl(c->socket, SIOCGIFINDEX, &ifr) != 0) {
error("Could not find interface with name \"%s\".", c->interface_name);
- return 1;
+ goto out;
}
addr.can_family = AF_CAN;
@@ -253,10 +249,11 @@ int can_start(struct node *n)
if(bind(c->socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
error("Could not bind to interface with name \"%s\" (%d).", c->interface_name, ifr.ifr_ifindex);
- return 1;
+ goto out;
}
-
- return 0;
+ ret = 0;
+ out:
+ return ret;
}
int can_stop(struct node *n)
@@ -290,6 +287,55 @@ int can_resume(struct node *n)
return 0;
}
+int can_conv_from_raw(union signal_data* sig, void* from, int size, struct signal *to)
+{
+ if (size <= 0 || size > 8) {
+ return 1;
+ }
+ switch(to->type) {
+ case SignalType::BOOLEAN:
+ sig->b = (bool)*(uint8_t*)from;
+ return 0;
+ case SignalType::INTEGER:
+ switch(size) {
+ case 1:
+ sig->i = (int64_t)*(int8_t*)from;
+ return 0;
+ case 2:
+ sig->i = (int64_t)*(int16_t*)from;
+ return 0;
+ case 3:
+ sig->i = (int64_t)*(int16_t*)from;
+ sig->i += ((int64_t)*((int8_t*)(from)+2)) << 16;
+ return 0;
+ case 4:
+ sig->i = (int64_t)*(int32_t*)from;
+ return 0;
+ case 8:
+ sig->i = *(uint64_t*)from;
+ return 0;
+ default:
+ error("unsupported conversion");
+ return 1;
+ }
+ case SignalType::FLOAT:
+ switch(size) {
+ case 4:
+ sig->f = (double)*(float*)from;
+ return 0;
+ case 8:
+ sig->f = *(double*)from;
+ return 0;
+ default:
+ error("unsupported conversion");
+ return 1;
+ }
+ default:
+ error("unsupported conversion");
+ return 1;
+ }
+}
+
int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
int nbytes;
@@ -327,12 +373,12 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele
for (size_t i=0; i < vlist_length(&(n->in.signals)); i++) {
if (c->in[i].id == frame.can_id) {
- /* This is a bit ugly. But there is no clean way to
- * clear the union. */
- smps[nread]->data[i].i = 0;
- memcpy(&smps[nread]->data[i],
- ((uint8_t*)&frame.data) + c->in[i].offset,
- c->in[i].size);
+ if (can_conv_from_raw(&c->sample_buf[i],
+ ((uint8_t*)&frame.data) + c->in[i].offset,
+ c->in[i].size,
+ (struct signal*) vlist_at(&n->in.signals, i)) == 0) {
+ return 1;
+ }
signal_num++;
found_id = true;
}