diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index 745412da6..c64376594 100644 --- 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 index c8c6d2ba4..668b650f5 100644 --- 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 index 5e1710c3f..1709ed67f 100644 --- 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; }