[vslib]Add MACsec forward and filters to HostInterfaceInfo

vslib/inc/HostInterfaceInfo.h

@@ -5,6 +5,8 @@ extern "C" {
 }
 
 #include "EventQueue.h"
+#include "TrafficFilterPipes.h"
+#include "TrafficForwarder.h"
 
 #include "swss/selectableevent.h"
 
@@ -14,7 +16,8 @@ extern "C" {
 
 namespace saivs
 {
-    class HostInterfaceInfo
+    class HostInterfaceInfo :
+        public TrafficForwarder
     {
         private:
 
@@ -38,6 +41,20 @@ namespace saivs
                     _In_ const uint8_t *buffer,
                     _In_ size_t size) const;
 
+            bool installEth2TapFilter(
+                    _In_ int priority,
+                    _In_ std::shared_ptr<TrafficFilter> filter);
+
+            bool uninstallEth2TapFilter(
+                    _In_ std::shared_ptr<TrafficFilter> filter);
+
+            bool installTap2EthFilter(
+                    _In_ int priority,
+                    _In_ std::shared_ptr<TrafficFilter> filter);
+
+            bool uninstallTap2EthFilter(
+                    _In_ std::shared_ptr<TrafficFilter> filter);
+
         private:
 
             void veth2tap_fun();
@@ -58,13 +75,16 @@ namespace saivs
 
             std::shared_ptr<EventQueue> m_eventQueue;
 
-        private:
-
             int m_tapfd;
 
+        private:
+
             std::shared_ptr<std::thread> m_e2t;
             std::shared_ptr<std::thread> m_t2e;
 
+            TrafficFilterPipes m_e2tFilters;
+            TrafficFilterPipes m_t2eFilters;
+
             swss::SelectableEvent m_e2tEvent;
             swss::SelectableEvent m_t2eEvent;
     };

vslib/inc/MACsecFilter.h

@@ -30,10 +30,10 @@ namespace saivs
             _In_ const void *buffer,
             _In_ size_t length) = 0;
 
-        bool m_macsec_device_enable;
+        bool m_macsecDeviceEnable;
 
         int m_macsecfd;
 
-        const std::string m_macsec_interface_name;
+        const std::string m_macsecInterfaceName;
     };
 }

vslib/inc/MACsecForwarder.h

@@ -1,5 +1,8 @@
 #pragma once
 
+#include "HostInterfaceInfo.h"
+#include "TrafficForwarder.h"
+
 #include "swss/sal.h"
 #include "swss/selectableevent.h"
 
@@ -9,12 +12,13 @@
 
 namespace saivs
 {
-    class MACsecForwarder
+    class MACsecForwarder :
+        public TrafficForwarder
     {
     public:
         MACsecForwarder(
             _In_ const std::string &macsecInterfaceName,
-            _In_ int tapfd);
+            _In_ std::shared_ptr<HostInterfaceInfo> info);
 
         virtual ~MACsecForwarder();
 
@@ -23,7 +27,6 @@ namespace saivs
         void forward();
 
     private:
-        int m_tapfd;
         int m_macsecfd;
 
         const std::string m_macsecInterfaceName;
@@ -33,5 +36,7 @@ namespace saivs
         swss::SelectableEvent m_exitEvent;
 
         std::shared_ptr<std::thread> m_forwardThread;
+
+        std::shared_ptr<HostInterfaceInfo> m_info;
     };
 }

vslib/inc/TrafficForwarder.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "swss/sal.h"
+
+#include <stddef.h>
+#include <sys/socket.h>
+
+namespace saivs
+{
+    class TrafficForwarder
+    {
+    public:
+        virtual ~TrafficForwarder() = default;
+
+    protected:
+        TrafficForwarder() = default;
+
+        void add_vlan_tag(
+            _Inout_ unsigned char *buffer,
+            _Inout_ size_t &length,
+            _Inout_ struct msghdr &msg) const;
+
+        bool send_to(
+            _In_ int fd,
+            _In_ const unsigned char *buffer,
+            _In_ size_t &length) const;
+
+    };
+}

vslib/src/HostInterfaceInfo.cpp

@@ -90,6 +90,40 @@ void HostInterfaceInfo::async_process_packet_for_fdb_event(
     m_eventQueue->enqueue(std::make_shared<Event>(EventType::EVENT_TYPE_PACKET, payload));
 }
 
+bool HostInterfaceInfo::installEth2TapFilter(
+        _In_ int priority,
+        _In_ std::shared_ptr<TrafficFilter> filter)
+{
+    SWSS_LOG_ENTER();
+
+    return m_e2tFilters.installFilter(priority, filter);
+}
+
+bool HostInterfaceInfo::uninstallEth2TapFilter(
+        _In_ std::shared_ptr<TrafficFilter> filter)
+{
+    SWSS_LOG_ENTER();
+
+    return m_e2tFilters.uninstallFilter(filter);
+}
+
+bool HostInterfaceInfo::installTap2EthFilter(
+        _In_ int priority,
+        _In_ std::shared_ptr<TrafficFilter> filter)
+{
+    SWSS_LOG_ENTER();
+
+    return m_t2eFilters.installFilter(priority, filter);
+}
+
+bool HostInterfaceInfo::uninstallTap2EthFilter(
+        _In_ std::shared_ptr<TrafficFilter> filter)
+{
+    SWSS_LOG_ENTER();
+
+    return m_t2eFilters.uninstallFilter(filter);
+}
+
 #define ETH_FRAME_BUFFER_SIZE (0x4000)
 #define CONTROL_MESSAGE_BUFFER_SIZE (0x1000)
 #define IEEE_8021Q_ETHER_TYPE (0x8100)
@@ -161,63 +195,28 @@ void HostInterfaceInfo::veth2tap_fun()
             continue;
         }
 
-        struct cmsghdr *cmsg;
+        // Buffer include the ingress packets
+        // MACsec scenario: EAPOL packets and encrypted packets
+        size_t length = static_cast<size_t>(size);
+        auto ret = m_e2tFilters.execute(buffer, length);
 
-        for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg))
+        if (ret == TrafficFilter::TERMINATE)
         {
-            if (cmsg->cmsg_level != SOL_PACKET || cmsg->cmsg_type != PACKET_AUXDATA)
-                continue;
-
-            struct tpacket_auxdata* aux = (struct tpacket_auxdata*)CMSG_DATA(cmsg);
-
-            if ((aux->tp_status & TP_STATUS_VLAN_VALID) &&
-                    (aux->tp_status & TP_STATUS_VLAN_TPID_VALID))
-            {
-                SWSS_LOG_DEBUG("got vlan tci: 0x%x, vlanid: %d", aux->tp_vlan_tci, aux->tp_vlan_tci & 0xFFF);
-
-                // inject vlan tag into frame
-
-                // for overlapping buffers
-                memmove(buffer + 2 * MAC_ADDRESS_SIZE + VLAN_TAG_SIZE,
-                        buffer + 2 * MAC_ADDRESS_SIZE,
-                        size - (2 * MAC_ADDRESS_SIZE));
-
-                uint16_t tci = htons(aux->tp_vlan_tci);
-                uint16_t tpid = htons(IEEE_8021Q_ETHER_TYPE);
-
-                uint8_t* pvlan =  (uint8_t *)(buffer + 2 * MAC_ADDRESS_SIZE);
-                memcpy(pvlan, &tpid, sizeof(uint16_t));
-                memcpy(pvlan + sizeof(uint16_t), &tci, sizeof(uint16_t));
-
-                size += VLAN_TAG_SIZE;
-
-                break;
-            }
+            continue;
         }
-
-        async_process_packet_for_fdb_event(buffer, size);
-
-        if (write(m_tapfd, buffer, size) < 0)
+        else if (ret == TrafficFilter::ERROR)
         {
-            /*
-             * We filter out EIO because of this patch:
-             * https://github.com/torvalds/linux/commit/1bd4978a88ac2589f3105f599b1d404a312fb7f6
-             */
+            // Error log should be recorded in filter
+            return;
+        }
 
-            if (errno != ENETDOWN && errno != EIO)
-            {
-                SWSS_LOG_ERROR("failed to write to tap device fd %d, errno(%d): %s",
-                        m_tapfd, errno, strerror(errno));
-            }
+        add_vlan_tag(buffer, length, msg);
 
-            if (errno == EBADF)
-            {
-                // bad file descriptor, just end thread
-                SWSS_LOG_NOTICE("ending thread for tap fd %d", m_tapfd);
-                return;
-            }
+        async_process_packet_for_fdb_event(buffer, length);
 
-            continue;
+        if (!send_to(m_tapfd, buffer, length))
+        {
+            break;
         }
     }
 
@@ -268,6 +267,22 @@ void HostInterfaceInfo::tap2veth_fun()
             continue;
         }
 
+        // Buffer include the egress packets
+        // MACsec scenario: EAPOL packets and plaintext packets
+        size_t length = static_cast<size_t>(size);
+        auto ret = m_t2eFilters.execute(buffer, length);
+        size = static_cast<ssize_t>(length);
+
+        if (ret == TrafficFilter::TERMINATE)
+        {
+            continue;
+        }
+        else if (ret == TrafficFilter::ERROR)
+        {
+            // Error log should be recorded in filter
+            return;
+        }
+
         if (write(m_packet_socket, buffer, (int)size) < 0)
         {
             if (errno != ENETDOWN)

vslib/src/MACsecForwarder.cpp

@@ -15,16 +15,25 @@
 using namespace saivs;
 
 #define ETH_FRAME_BUFFER_SIZE (0x4000)
+#define CONTROL_MESSAGE_BUFFER_SIZE (0x1000)
+#define IEEE_8021Q_ETHER_TYPE (0x8100)
+#define MAC_ADDRESS_SIZE (6)
+#define VLAN_TAG_SIZE (4)
 
 MACsecForwarder::MACsecForwarder(
     _In_ const std::string &macsecInterfaceName,
-    _In_ int tapfd):
-    m_tapfd(tapfd),
+    _In_ std::shared_ptr<HostInterfaceInfo> info):
     m_macsecInterfaceName(macsecInterfaceName),
-    m_runThread(true)
+    m_runThread(true),
+    m_info(info)
 {
     SWSS_LOG_ENTER();
 
+    if (m_info == nullptr)
+    {
+        SWSS_LOG_THROW("The HostInterfaceInfo on the MACsec port %s is empty", m_macsecInterfaceName.c_str());
+    }
+
     m_macsecfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
 
     if (m_macsecfd < 0)
@@ -112,6 +121,25 @@ void MACsecForwarder::forward()
 
     while (m_runThread)
     {
+        struct msghdr msg;
+        memset(&msg, 0, sizeof(struct msghdr));
+
+        struct sockaddr_storage srcAddr;
+
+        struct iovec iov[1];
+
+        iov[0].iov_base = buffer; // buffer for message
+        iov[0].iov_len = sizeof(buffer);
+
+        char control[CONTROL_MESSAGE_BUFFER_SIZE]; // buffer for control messages
+
+        msg.msg_name = &srcAddr;
+        msg.msg_namelen = sizeof(srcAddr);
+        msg.msg_iov = iov;
+        msg.msg_iovlen = 1;
+        msg.msg_control = control;
+        msg.msg_controllen = sizeof(control);
+
         swss::Selectable *sel = NULL;
         int result = s.select(&sel);
 
@@ -128,7 +156,7 @@ void MACsecForwarder::forward()
         if (sel == &m_exitEvent) // thread end event
             break;
 
-        ssize_t size = read(m_macsecfd, buffer, sizeof(buffer));
+        ssize_t size = recvmsg(m_macsecfd, &msg, 0);
 
         if (size < 0)
         {
@@ -151,35 +179,26 @@ void MACsecForwarder::forward()
 
             continue;
         }
-
-        if (write(m_tapfd, buffer, static_cast<int>(size)) < 0)
+        else if (size < (ssize_t)sizeof(ethhdr))
         {
-            if (errno != ENETDOWN && errno != EIO)
-            {
-                SWSS_LOG_ERROR(
-                    "failed to write to macsec device %s fd %d, errno(%d): %s",
-                    m_macsecInterfaceName.c_str(),
-                    m_macsecfd,
-                    errno,
-                    strerror(errno));
-            }
-
-            if (errno == EBADF)
-            {
-                // bad file descriptor, just end thread
-                SWSS_LOG_ERROR(
-                    "ending thread for macsec device %s fd %d",
-                    m_macsecInterfaceName.c_str(),
-                    m_macsecfd);
-                return;
-            }
+            SWSS_LOG_ERROR("invalid ethernet frame length: %zu", msg.msg_controllen);
 
             continue;
         }
+
+        size_t length = static_cast<size_t>(size);
+
+        add_vlan_tag(buffer, length, msg);
+
+        m_info->async_process_packet_for_fdb_event(buffer, length);
+
+        if (!send_to(m_info->m_tapfd, buffer, length))
+        {
+            break;
+        }
     }
 
     SWSS_LOG_NOTICE(
         "ending thread proc for %s",
         m_macsecInterfaceName.c_str());
 }
-