From a019b145398dd9eaf0340b2a7c2cdd5aba8cf58b Mon Sep 17 00:00:00 2001
From: Jean-Roland <jean.roland.gosse@gmail.com>
Date: Wed, 29 Nov 2023 10:09:01 +0100
Subject: [PATCH] feat: add rx whitelist in config

---
 include/zenoh-pico/transport/raweth/config.h | 10 +++++++++-
 src/system/unix/link/raweth.c                | 20 ++++++++++++++++----
 src/transport/raweth/config.c                |  7 +++++++
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h
index 7921f8389..a465c1e57 100644
--- a/include/zenoh-pico/transport/raweth/config.h
+++ b/include/zenoh-pico/transport/raweth/config.h
@@ -31,17 +31,25 @@ typedef struct {
     _Bool _has_vlan;
 } _zp_raweth_cfg_entry;
 
+typedef struct {
+    uint8_t _mac[_ZP_MAC_ADDR_LENGTH];
+} _zp_raweth_cfg_whitelist_val;
+
 // Ethertype to use in frame
 extern const uint16_t _ZP_RAWETH_CFG_ETHTYPE;
 
 // Source mac address
 extern const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH];
 
-// Sort the keyexpr alphabetically to use binary search (if size ~100+), otherwise use simple linear search
+// Main config array
 extern const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[];
 
+// Mac address rx whitelist array
+extern const _zp_raweth_cfg_whitelist_val _ZP_RAWETH_CFG_WHITELIST[];
+
 // Array size
 extern const size_t _ZP_RAWETH_CFG_SIZE;
+extern const size_t _ZP_RAWETH_CFG_WHITELIST_SIZE;
 
 #endif  // Z_FEATURE_RAWETH_TRANSPORT == 1
 #endif  // ZENOH_PICO_RAWETH_CONFIG_H
diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c
index 1774f301a..e19178bea 100644
--- a/src/system/unix/link/raweth.c
+++ b/src/system/unix/link/raweth.c
@@ -32,6 +32,7 @@
 #include "zenoh-pico/collections/string.h"
 #include "zenoh-pico/config.h"
 #include "zenoh-pico/system/platform/unix.h"
+#include "zenoh-pico/transport/raweth/config.h"
 #include "zenoh-pico/utils/logging.h"
 #include "zenoh-pico/utils/pointers.h"
 
@@ -94,14 +95,25 @@ 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);
-    if (bytesRead < 0) {
+    if ((bytesRead < 0) || (bytesRead < sizeof(_zp_eth_header_t))) {
+        return SIZE_MAX;
+    }
+    // Address filtering
+    _zp_eth_header_t *header = (_zp_eth_header_t *)buff;
+    _Bool is_valid = false;
+    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;
+        }
+    }
+    // Ignore packet from unknown sources
+    if (!is_valid) {
         return SIZE_MAX;
     }
-    // Soft Filtering ?
-
     // Copy sender mac if needed
-    if ((addr != NULL) && (bytesRead > 2 * ETH_ALEN)) {
+    if (addr != NULL) {
         *addr = _z_bytes_make(sizeof(ETH_ALEN));
         (void)memcpy((uint8_t *)addr->start, (buff + ETH_ALEN), sizeof(ETH_ALEN));
     }
diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c
index fb5186462..1132865dd 100644
--- a/src/transport/raweth/config.c
+++ b/src/transport/raweth/config.c
@@ -29,7 +29,14 @@ const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = {
     {{0, {0}, "another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true},  // entry2
 };
 
+// Should be generated
+const _zp_raweth_cfg_whitelist_val _ZP_RAWETH_CFG_WHITELIST[] = {
+    {{0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}},
+    {{0x00, 0x11, 0x22, 0x33, 0x44, 0x55}},
+};
+
 // Don't modify
 const size_t _ZP_RAWETH_CFG_SIZE = _ZP_ARRAY_SIZE(_ZP_RAWETH_CFG_ARRAY);
+const size_t _ZP_RAWETH_CFG_WHITELIST_SIZE = _ZP_ARRAY_SIZE(_ZP_RAWETH_CFG_WHITELIST);
 
 #endif  // Z_FEATURE_RAWETH_TRANSPORT == 1