Add MACsec forward and filters to HostInterfaceInfo

Signed-off-by: Ze Gan <ganze718@gmail.com>

vslib/inc/HostInterfaceInfo.h

@@ -5,6 +5,7 @@ extern "C" {
 }
 
 #include "EventQueue.h"
+#include "TrafficFilterPipes.h"
 
 #include "swss/selectableevent.h"
 
@@ -38,6 +39,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 +73,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,7 @@
 #pragma once
 
+#include "HostInterfaceInfo.h"
+
 #include "swss/sal.h"
 #include "swss/selectableevent.h"
 
@@ -14,7 +16,8 @@ namespace saivs
     public:
         MACsecForwarder(
             _In_ const std::string &macsecInterfaceName,
-            _In_ int tapfd);
+            _In_ int tapfd,
+            _In_ std::shared_ptr<HostInterfaceInfo> info);
 
         virtual ~MACsecForwarder();
 
@@ -33,5 +36,7 @@ namespace saivs
         swss::SelectableEvent m_exitEvent;
 
         std::shared_ptr<std::thread> m_forwardThread;
+
+        std::shared_ptr<HostInterfaceInfo> m_info;
     };
 }

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,6 +195,20 @@ void HostInterfaceInfo::veth2tap_fun()
             continue;
         }
 
+        size_t length = static_cast<size_t>(size);
+        auto ret = m_e2tFilters.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;
+        }
+
         struct cmsghdr *cmsg;
 
         for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg))
@@ -268,6 +316,20 @@ void HostInterfaceInfo::tap2veth_fun()
             continue;
         }
 
+        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,13 +15,19 @@
 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):
+    _In_ int tapfd,
+    _In_ std::shared_ptr<HostInterfaceInfo> info):
     m_tapfd(tapfd),
     m_macsecInterfaceName(macsecInterfaceName),
-    m_runThread(true)
+    m_runThread(true),
+    m_info(info)
 {
     SWSS_LOG_ENTER();
 
@@ -112,6 +118,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 +153,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,6 +176,55 @@ void MACsecForwarder::forward()
 
             continue;
         }
+        else if (size < (ssize_t)sizeof(ethhdr))
+        {
+            SWSS_LOG_ERROR("invalid ethernet frame length: %zu", msg.msg_controllen);
+
+            continue;
+        }
+
+        struct cmsghdr *cmsg;
+
+        for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg))
+        {
+            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;
+            }
+        }
+
+        if (m_info == nullptr)
+        {
+            SWSS_LOG_ERROR("The HostInterfaceInfo on the MACsec port %s is empty", m_macsecInterfaceName.c_str());
+
+            break;
+        }
+
+        m_info->async_process_packet_for_fdb_event(buffer, size);
 
         if (write(m_tapfd, buffer, static_cast<int>(size)) < 0)
         {

vslib/src/MACsecManager.cpp

@@ -579,7 +579,7 @@ bool MACsecManager::add_macsec_forwarder(
 
     auto &manager = itr->second;
 
-    manager.m_forwarder = std::make_shared<MACsecForwarder>(macsecInterface, manager.m_info->m_tapfd);
+    manager.m_forwarder = std::make_shared<MACsecForwarder>(macsecInterface, manager.m_info->m_tapfd, manager.m_info);
     return true;
 }
 

vslib/src/Makefile.am

@@ -53,7 +53,11 @@ libSaiVS_a_SOURCES = \
 					  SwitchMLNX2700.cpp \
 					  CorePortIndexMap.cpp \
 					  CorePortIndexMapContainer.cpp \
-					  CorePortIndexMapFileParser.cpp
+					  CorePortIndexMapFileParser.cpp \
+					  MACsecFilter.cpp \
+					  MACsecEgressFilter.cpp \
+					  MACsecIngressFilter.cpp \
+					  TrafficFilterPipes.cpp
 
 libsaivs_la_SOURCES = \
 					  sai_vs_fdb.cpp \