fix: add valid path to read task

eclipse-zenoh · Dec 1, 2023 · 1fb4344 · 1fb4344
1 parent 24b1d1f
commit 1fb4344
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 9 deletions.
diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c
@@ -91,7 +91,7 @@ size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t
 
 size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr) {
     // Read from socket
-    size_t bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL);
+    ssize_t bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL);
     if ((bytesRead < 0) || (bytesRead < sizeof(_zp_eth_header_t))) {
         return SIZE_MAX;
     }
@@ -101,6 +101,7 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf
     for (size_t i = 0; i < _ZP_RAWETH_CFG_WHITELIST_SIZE; i++) {
         if (memcmp(&header->smac, _ZP_RAWETH_CFG_WHITELIST[i]._mac, _ZP_MAC_ADDR_LENGTH) == 0) {  // Test byte ordering
             is_valid = true;
+            break;
         }
     }
     // Ignore packet from unknown sources

diff --git a/src/transport/raweth/read.c b/src/transport/raweth/read.c
@@ -68,12 +68,20 @@ void *_zp_raweth_read_task(void *ztm_arg) {
     while (ztm->_read_task_running == true) {
         // Read message from link
         int8_t ret = _z_raweth_recv_t_msg(ztm, &t_msg, &addr);
-        if (ret == _Z_ERR_TRANSPORT_RX_FAILED) {
-            continue;
-        } else {
-            _Z_ERROR("Connection closed due to malformed message\n");
-            ztm->_read_task_running = false;
-            continue;
+        switch (ret) {
+            case _Z_RES_OK:
+                // Process message
+                break;
+            case _Z_ERR_TRANSPORT_RX_FAILED:
+                // Drop message
+                continue;
+                break;
+            default:
+                // Drop message & stop task
+                _Z_ERROR("Connection closed due to malformed message\n");
+                ztm->_read_task_running = false;
+                continue;
+                break;
         }
         // Process message
         if (_z_multicast_handle_transport_message(ztm, &t_msg, &addr) != _Z_RES_OK) {

diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c
@@ -27,6 +27,10 @@
 
 #if Z_FEATURE_RAWETH_TRANSPORT == 1
 
+void print_buf(_z_zbuf_t *buf) {
+    printf("Buff info: %ld, %ld, %ld\n", buf->_ios._r_pos, buf->_ios._w_pos, buf->_ios._capacity);
+}
+
 static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z_bytes_t *addr) {
     uint8_t *buff = _z_zbuf_get_wptr(zbf);
     size_t rb = _z_receive_raweth(&link->_socket._raweth._sock, buff, _z_zbuf_space_left(zbf), addr);
@@ -47,9 +51,9 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z
     // Update buffer but skip eth header
     _z_zbuf_set_wpos(zbf, _z_zbuf_get_wpos(zbf) + rb);
     if (has_vlan) {
-        _z_zbuf_set_rpos(zbf, _z_zbuf_get_wpos(zbf) - (rb - sizeof(_zp_eth_vlan_header_t)));
+        _z_zbuf_set_rpos(zbf, _z_zbuf_get_rpos(zbf) + sizeof(_zp_eth_vlan_header_t));
     } else {
-        _z_zbuf_set_rpos(zbf, _z_zbuf_get_wpos(zbf) - (rb - sizeof(_zp_eth_header_t)));
+        _z_zbuf_set_rpos(zbf, _z_zbuf_get_rpos(zbf) + sizeof(_zp_eth_header_t));
     }
     return rb;
 }