diff --git a/include/villas/nodes/can.hpp b/include/villas/nodes/can.hpp index abcf870d5..69e4ede72 100644 --- a/include/villas/nodes/can.hpp +++ b/include/villas/nodes/can.hpp @@ -38,23 +38,23 @@ struct node; union signal_data; struct can_signal { - uint32_t id; - int offset; - int size; + uint32_t id; + int offset; + int size; }; struct can { - /* Settings */ - char *interface_name; - double *sample_rate; - struct can_signal *in; - struct can_signal *out; + /* Settings */ + char *interface_name; + double *sample_rate; + struct can_signal *in; + struct can_signal *out; - /* States */ - int socket; - union signal_data *sample_buf; - size_t sample_buf_num; - struct timespec start_time; + /* States */ + int socket; + union signal_data *sample_buf; + size_t sample_buf_num; + struct timespec start_time; }; /** @see node_type::init */ diff --git a/lib/nodes/can.cpp b/lib/nodes/can.cpp index 64dee02eb..b3c8674af 100644 --- a/lib/nodes/can.cpp +++ b/lib/nodes/can.cpp @@ -67,12 +67,14 @@ int can_destroy(struct node *n) struct can *c = (struct can *) n->_vd; free(c->interface_name); - if (c->socket != 0) { + + if (c->socket != 0) close(c->socket); - } + free(c->sample_buf); free(c->in); free(c->out); + return 0; } @@ -87,11 +89,11 @@ int can_parse_signal(json_t *json, struct vlist *node_signals, struct can_signal json_error_t err; ret = json_unpack_ex(json, &err, 0, "{ s?: s, s?: i, s?: i, s?: i }", - "name", &name, - "can_id", &can_id, - "can_size", &can_size, - "can_offset", &can_offset - ); + "name", &name, + "can_id", &can_id, + "can_size", &can_size, + "can_offset", &can_offset + ); if (ret) { jerror(&err, "Failed to parse signal configuration for can"); @@ -115,11 +117,11 @@ int can_parse_signal(json_t *json, struct vlist *node_signals, struct can_signal can_signals[signal_index].offset = can_offset; ret = 0; goto out; - } else { - error("Signal configuration inconsistency detected: Signal with index %zu (\"%s\") does not match can_signal \"%s\"\n", signal_index, sig->name, name); } - out: - return ret; + else + error("Signal configuration inconsistency detected: Signal with index %zu (\"%s\") does not match can_signal \"%s\"\n", signal_index, sig->name, name); + +out: return ret; } int can_parse(struct node *n, json_t *cfg) @@ -136,13 +138,13 @@ int can_parse(struct node *n, json_t *cfg) c->out = nullptr; ret = json_unpack_ex(cfg, &err, 0, "{ s: s, s: F, s?: { s?: o }, s?: { s?: o } }", - "interface_name", &c->interface_name, - "sample_rate", &c->sample_rate, - "in", - "signals", &json_in_signals, - "out", - "signals", &json_out_signals - ); + "interface_name", &c->interface_name, + "sample_rate", &c->sample_rate, + "in", + "signals", &json_in_signals, + "out", + "signals", &json_out_signals + ); if (ret) { jerror(&err, "Failed to parse configuration of node %s", node_name(n)); goto out; @@ -167,24 +169,27 @@ int can_parse(struct node *n, json_t *cfg) goto out; } } + json_array_foreach(json_out_signals, i, json_signal) { if (can_parse_signal(json_signal, &n->out.signals, c->out, i) != 0) { error("at signal %zu in node %s.",i , node_name(n)); goto out; } } + ret = 0; - out: - if (ret != 0) { + +out: if (ret != 0) { free(c->in); free(c->out); c->in = nullptr; c->out = nullptr; } + return ret; } -char *can_print(struct node *n) +char * can_print(struct node *n) { struct can *c = (struct can *) n->_vd; @@ -206,9 +211,9 @@ int can_check(struct node *n) int can_prepare(struct node *n) { struct can *c = (struct can *) n->_vd; - c->sample_buf = (union signal_data*) calloc( - vlist_length(&n->in.signals), - sizeof(union signal_data)); + + c->sample_buf = (union signal_data*) calloc(vlist_length(&n->in.signals), sizeof(union signal_data)); + return (c->sample_buf != 0 ? 0 : 1); } @@ -221,7 +226,7 @@ int can_start(struct node *n) struct can *c = (struct can *) n->_vd; c->start_time = time_now(); - if((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + if ((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { error("Error while opening CAN socket"); goto out; } @@ -235,13 +240,14 @@ int can_start(struct node *n) addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; - if(bind(c->socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + 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); goto out; } + ret = 0; - out: - return ret; + +out: return ret; } int can_stop(struct node *n) @@ -262,57 +268,67 @@ int can_conv_to_raw(union signal_data* sig, struct signal *from, void* to, int s error("signal size cannot be larger than 8!"); return 1; } + switch(from->type) { - case SignalType::BOOLEAN: - *(uint8_t*)to = sig->b; - return 0; - case SignalType::INTEGER: - switch(size) { - case 1: - *(int8_t*)to = (int8_t)sig->i; + case SignalType::BOOLEAN: + *(uint8_t*)to = sig->b; return 0; - case 2: - *(int16_t*)to = (int16_t)sig->i; - sig->i = (int64_t)*(int16_t*)from; - return 0; - case 3: - *(int16_t*)to = (int16_t)sig->i; - *((int8_t*)to+2) = (int8_t)(sig->i >> 16); - return 0; - case 4: - *(int32_t*)to = (int32_t)sig->i; - return 0; - case 8: - *(int64_t*)to = sig->i; + + case SignalType::INTEGER: + switch(size) { + case 1: + *(int8_t*)to = (int8_t)sig->i; + return 0; + + case 2: + *(int16_t*)to = (int16_t)sig->i; + sig->i = (int64_t)*(int16_t*)from; + return 0; + + case 3: + *(int16_t*)to = (int16_t)sig->i; + *((int8_t*)to+2) = (int8_t)(sig->i >> 16); + return 0; + + case 4: + *(int32_t*)to = (int32_t)sig->i; + return 0; + + case 8: + *(int64_t*)to = sig->i; + return 0; + + default: + goto fail; + } + case SignalType::FLOAT: + switch(size) { + case 4: + assert(sizeof(float) == 4); + *(float*)to = (float)sig->f; + return 0; + + case 8: + *(double*)to = sig->f; + return 0; + + default: + goto fail; + } + case SignalType::COMPLEX: + if (size != 8) + goto fail; + + *(float*)to = sig->z.real(); + *((float*)to+1) = sig->z.imag(); return 0; + default: goto fail; - } - case SignalType::FLOAT: - switch(size) { - case 4: - assert(sizeof(float) == 4); - *(float*)to = (float)sig->f; - return 0; - case 8: - *(double*)to = sig->f; - return 0; - default: - goto fail; - } - case SignalType::COMPLEX: - if (size != 8) { - goto fail; - } - *(float*)to = sig->z.real(); - *((float*)to+1) = sig->z.imag(); - return 0; - default: - goto fail; } - fail: - error("unsupported conversion to %s from raw (%p, %d)", - signal_type_to_str(from->type), to, size); + +fail: error("unsupported conversion to %s from raw (%p, %d)", signal_type_to_str(from->type), to, size); + return 1; } @@ -322,55 +338,64 @@ int can_conv_from_raw(union signal_data* sig, void* from, int size, struct signa error("signal size cannot be larger than 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; + case SignalType::BOOLEAN: + sig->b = (bool)*(uint8_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; + 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: + goto fail; + } + case SignalType::FLOAT: + switch(size) { + case 4: + assert(sizeof(float) == 4); + sig->f = (double)*(float*)from; + return 0; + + case 8: + sig->f = *(double*)from; + return 0; + + default: + goto fail; + } + case SignalType::COMPLEX: + if (size != 8) + goto fail; + + sig->z = std::complex(*(float*)from, *((float*)from+1)); return 0; + default: goto fail; - } - case SignalType::FLOAT: - switch(size) { - case 4: - assert(sizeof(float) == 4); - sig->f = (double)*(float*)from; - return 0; - case 8: - sig->f = *(double*)from; - return 0; - default: - goto fail; - } - case SignalType::COMPLEX: - if (size != 8) { - goto fail; - } - sig->z = std::complex(*(float*)from, *((float*)from+1)); - return 0; - default: - goto fail; } - fail: - error("unsupported conversion from %s to raw (%p, %d)", - signal_type_to_str(to->type), from, size); +fail: + error("unsupported conversion from %s to raw (%p, %d)", signal_type_to_str(to->type), from, size); + return 1; } @@ -392,17 +417,17 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele error("CAN read() returned -1. Is the CAN interface up?"); goto out; } + if ((unsigned)nbytes != sizeof(struct can_frame)) { - error("CAN read() error. read() returned %d bytes but expected %zu", - nbytes, sizeof(struct can_frame)); + error("CAN read() error. read() returned %d bytes but expected %zu", nbytes, sizeof(struct can_frame)); goto out; } debug(0,"received can message: (id:%d, len:%u, data: 0x%x:0x%x)", - frame.can_id, - frame.can_dlc, - ((uint32_t*)&frame.data)[0], - ((uint32_t*)&frame.data)[1]); + frame.can_id, + frame.can_dlc, + ((uint32_t*)&frame.data)[0], + ((uint32_t*)&frame.data)[1]); if (ioctl(c->socket, SIOCGSTAMP, &tv) == 0) { TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received); @@ -417,15 +442,19 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele (struct signal*) vlist_at(&n->in.signals, i)) != 0) { goto out; } + c->sample_buf_num++; found_id = true; } } + if (!found_id) { error("did not find signal for can id %d\n", frame.can_id); return 0; } + debug(0, "received %zu signals\n", c->sample_buf_num); + /* Copy signal data to sample only when all signals have been received */ if (c->sample_buf_num == vlist_length(&n->in.signals)) { smps[nread]->length = c->sample_buf_num; @@ -433,13 +462,15 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele c->sample_buf_num = 0; smps[nread]->flags |= (int) SampleFlags::HAS_DATA; ret = 1; - } else { + } + else { smps[nread]->length = 0; ret = 0; } - out: - /* Set signals, because other VILLASnode parts expect us to */ + + out: /* Set signals, because other VILLASnode parts expect us to */ smps[nread]->signals = &n->in.signals; + return ret; } @@ -454,31 +485,34 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel assert(cnt >= 1 && smps[0]->capacity >= 1); - frame = (struct can_frame*) calloc(sizeof(struct can_frame), - vlist_length(&(n->out.signals))); + frame = (struct can_frame*) calloc(sizeof(struct can_frame), vlist_length(&(n->out.signals))); for (nwrite=0; nwrite < cnt; nwrite++) { for (size_t i=0; i < vlist_length(&(n->out.signals)); i++) { - if (c->out[i].offset != 0) { /* frame is shared */ + if (c->out[i].offset != 0) /* frame is shared */ continue; - } + frame[fsize].can_dlc = c->out[i].size; frame[fsize].can_id = c->out[i].id; + can_conv_to_raw( &smps[nwrite]->data[i], (struct signal*)vlist_at(&(n->out.signals), i), &frame[fsize].data, c->out[i].size); + fsize++; } + for (size_t i=0; i < vlist_length(&(n->out.signals)); i++) { if (c->out[i].offset == 0) { /* frame already stored */ continue; } + for (size_t j=0; j < fsize; j++) { - if (c->out[i].id != frame[j].can_id) { + if (c->out[i].id != frame[j].can_id) continue; - } + frame[j].can_dlc += c->out[i].size; can_conv_to_raw( &smps[nwrite]->data[i], @@ -488,25 +522,27 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel break; } } + for (size_t j=0; j < fsize; j++) { - debug(0,"writing can message: (id:%d, dlc:%u, data:0x%x:0x%x)", frame[j].can_id, - frame[j].can_dlc, - ((uint32_t*)&frame[j].data)[0], - ((uint32_t*)&frame[j].data)[1]); + debug(0,"writing can message: (id:%d, dlc:%u, data:0x%x:0x%x)", + frame[j].can_id, + frame[j].can_dlc, + ((uint32_t*)&frame[j].data)[0], + ((uint32_t*)&frame[j].data)[1] + ); if ((nbytes = write(c->socket, &frame[j], sizeof(struct can_frame))) == -1) { error("CAN write() returned -1. Is the CAN interface up?"); return nwrite; } + if ((unsigned)nbytes != sizeof(struct can_frame)) { error("CAN write() error. write() returned %d bytes but expected %zu", - nbytes, sizeof(struct can_frame)); + nbytes, sizeof(struct can_frame)); return nwrite; } } } - return nwrite; -} return nwrite; } @@ -514,6 +550,7 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel int can_poll_fds(struct node *n, int fds[]) { struct can *c = (struct can *) n->_vd; + fds[0] = c->socket; return 1; /* The number of file descriptors which have been set in fds */