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[])