From 9fd6bae450d77fe54d11181e180411140c286f6b Mon Sep 17 00:00:00 2001
From: Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
Date: Wed, 8 Jul 2020 16:07:39 +0200
Subject: [PATCH] can: make can_read non-blockin using recv() instead of read()
---
lib/nodes/can.cpp | 103 +++++++++++++++++++++++-----------------------
1 file changed, 51 insertions(+), 52 deletions(-)
diff --git a/lib/nodes/can.cpp b/lib/nodes/can.cpp
index 5155dda43..08eb818a4 100644
--- a/lib/nodes/can.cpp
+++ b/lib/nodes/can.cpp
@@ -294,8 +294,8 @@ int can_resume(struct node *n)
int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
int nbytes;
- unsigned nread;
- unsigned signal_num;
+ unsigned nread = 0;
+ unsigned signal_num = 0;
struct can_frame frame;
struct timeval tv;
bool found_id = false;
@@ -304,55 +304,53 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele
assert(cnt >= 1 && smps[0]->capacity >= 1);
- for (nread=0; nread < cnt; nread++) {
- signal_num = 0;
- found_id = false;
- while (signal_num < (unsigned)vlist_length(&(n->in.signals))) {
- if ((nbytes = read(c->socket, &frame, sizeof(struct can_frame))) == -1) {
- error("CAN read() returned -1. Is the CAN interface up?");
- return 0;
- }
- if ((unsigned)nbytes != sizeof(struct can_frame)) {
- error("CAN read() error. read() returned %d bytes but expected %zu",
- nbytes, sizeof(struct can_frame));
- return 0;
- }
-
- 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]);
-
- if (ioctl(c->socket, SIOCGSTAMP, &tv) != 0) {
- warning("Could not get timestamp from CAN driver on interface \"%s\".", c->interface_name);
- smps[nread]->ts.received = time_now();
- } else {
- TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received);
- }
-
- 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);
- signal_num++;
- found_id = true;
- }
- }
- if (!found_id) {
- error("did not find signal for can id %d\n", frame.can_id);
- return 0;
- }
- }
- /* Set signals, because other VILLASnode parts expect us to */
- smps[nread]->signals = &n->in.signals;
- smps[nread]->length = signal_num;
+ nbytes = recv(c->socket, &frame, sizeof(struct can_frame), MSG_DONTWAIT);
+ if (nbytes == -1 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
+ return 0;
}
- return nread;
+ if (nbytes == -1) {
+ error("CAN read() returned -1. Is the CAN interface up?");
+ return 0;
+ }
+ if ((unsigned)nbytes != sizeof(struct can_frame)) {
+ error("CAN read() error. read() returned %d bytes but expected %zu",
+ nbytes, sizeof(struct can_frame));
+ return 0;
+ }
+
+ 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]);
+
+ if (ioctl(c->socket, SIOCGSTAMP, &tv) != 0) {
+ warning("Could not get timestamp from CAN driver on interface \"%s\".", c->interface_name);
+ smps[nread]->ts.received = time_now();
+ } else {
+ TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received);
+ }
+
+ 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);
+ signal_num++;
+ found_id = true;
+ }
+ }
+ if (!found_id) {
+ error("did not find signal for can id %d\n", frame.can_id);
+ return 0;
+ }
+ /* Set signals, because other VILLASnode parts expect us to */
+ smps[nread]->signals = &n->in.signals;
+ smps[nread]->length = signal_num;
+ return 1;
}
int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
@@ -426,11 +424,12 @@ int can_reverse(struct node *n)
int can_poll_fds(struct node *n, int fds[])
{
- //struct can *c = (struct can *) n->_vd;
+ struct can *c = (struct can *) n->_vd;
+ fds[0] = c->socket;
/* TODO: Add implementation here. */
- return 0; /* The number of file descriptors which have been set in fds */
+ return 1; /* The number of file descriptors which have been set in fds */
}
int can_netem_fds(struct node *n, int fds[])