1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-09 00:00:00 +01:00

can: align code to VILLASnode coding-style

This commit is contained in:
Steffen Vogel 2020-07-08 13:33:58 +02:00
parent 06fc6ed6ed
commit 33eef47f97
2 changed files with 188 additions and 151 deletions

View file

@ -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 */

View file

@ -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>(*(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>(*(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 */