[vslib]Add MACsec Filters

vslib/inc/MACsecEgressFilter.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "MACsecFilter.h"
+
+namespace saivs
+{
+    class MACsecEgressFilter : public MACsecFilter
+    {
+    public:
+        MACsecEgressFilter(_In_ const std::string &macsecInterfaceName);
+
+        virtual ~MACsecEgressFilter() = default;
+
+    protected:
+        virtual FilterStatus forward(
+            _In_ const void *buffer,
+            _In_ ssize_t length) override;
+    };
+}

vslib/inc/MACsecFilter.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "TrafficFilter.h"
+
+#include <string>
+
+namespace saivs
+{
+    class MACsecFilter : public TrafficFilter
+    {
+    public:
+        MACsecFilter(
+            _In_ const std::string &macsec_interface_name);
+
+        virtual ~MACsecFilter() = default;
+
+        virtual FilterStatus execute(
+            _Inout_ void *buffer,
+            _Inout_ ssize_t &length) override;
+
+        void enable_macsec_device(bool enable);
+
+        void set_macsec_fd(int macsecfd);
+
+    protected:
+        virtual FilterStatus forward(
+            _In_ const void *buffer,
+            _In_ ssize_t length) = 0;
+
+        bool m_macsec_device_enable;
+
+        int m_macsecfd;
+
+        const std::string m_macsec_interface_name;
+    };
+}

vslib/inc/MACsecIngressFilter.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "MACsecFilter.h"
+
+namespace saivs
+{
+    class MACsecIngressFilter : public MACsecFilter
+    {
+    public:
+        MACsecIngressFilter(_In_ const std::string &macsec_interface_name);
+
+        virtual ~MACsecIngressFilter() = default;
+
+    protected:
+        virtual FilterStatus forward(
+            _In_ const void *buffer,
+            _In_ ssize_t length) override;
+    };
+}

vslib/inc/TrafficFilter.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "swss/sal.h"
+
+#include <sys/types.h>
+
+namespace saivs
+{
+    enum FilterPriority
+    {
+        MACSEC_FILTER,
+    };
+
+    class TrafficFilter
+    {
+    public:
+        enum FilterStatus
+        {
+            CONTINUE,
+            TERMINATE,
+            ERROR,
+        };
+
+        TrafficFilter() = default;
+
+        virtual ~TrafficFilter() = default;
+
+        virtual FilterStatus execute(
+            _Inout_ void *buffer,
+            _Inout_ ssize_t &length) = 0;
+    };
+}

vslib/inc/TrafficFilterPipes.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "TrafficFilter.h"
+
+#include <memory>
+#include <map>
+#include <mutex>
+
+namespace saivs
+{
+    class TrafficFilterPipes
+    {
+    public:
+        TrafficFilterPipes() = default;
+
+        virtual ~TrafficFilterPipes() = default;
+
+        bool installFilter(
+            _In_ int priority,
+            _In_ std::shared_ptr<TrafficFilter> filter);
+
+        bool uninstallFilter(
+            _In_ std::shared_ptr<TrafficFilter> filter);
+
+        TrafficFilter::FilterStatus execute(
+            _Inout_ void *buffer,
+            _Inout_ ssize_t &length);
+
+    private:
+        typedef std::map<int, std::shared_ptr<TrafficFilter> > FilterPriorityQueue;
+
+        std::mutex m_mutex;
+        FilterPriorityQueue m_filters;
+    };
+}

vslib/src/MACsecEgressFilter.cpp

@@ -0,0 +1,48 @@
+#include "MACsecEgressFilter.h"
+
+#include <swss/logger.h>
+
+#include <unistd.h>
+#include <string.h>
+
+using namespace saivs;
+
+MACsecEgressFilter::MACsecEgressFilter(_In_ const std::string &macsecInterfaceName):
+    MACsecFilter(macsecInterfaceName)
+{
+    SWSS_LOG_ENTER();
+
+    // empty intentionally
+}
+
+TrafficFilter::FilterStatus MACsecEgressFilter::forward(
+    _In_ const void *buffer,
+    _In_ ssize_t length)
+{
+    SWSS_LOG_ENTER();
+
+    if (write(m_macsecfd, buffer, length) < 0)
+    {
+        if (errno != ENETDOWN && errno != EIO)
+        {
+            SWSS_LOG_ERROR(
+                "failed to write to macsec device %s fd %d, errno(%d): %s",
+                m_macsec_interface_name.c_str(),
+                m_macsecfd,
+                errno,
+                strerror(errno));
+        }
+
+        if (errno == EBADF)
+        {
+            SWSS_LOG_ERROR(
+                "ending thread for macsec device %s fd %d",
+                m_macsec_interface_name.c_str(),
+                m_macsecfd);
+
+            return TrafficFilter::ERROR;
+        }
+    }
+
+    return TrafficFilter::TERMINATE;
+}

vslib/src/MACsecFilter.cpp

@@ -0,0 +1,58 @@
+#include "MACsecFilter.h"
+
+#include "swss/logger.h"
+#include "swss/select.h"
+
+#include <sys/socket.h>
+#include <linux/if_packet.h>
+#include <linux/if_ether.h>
+#include <arpa/inet.h>
+#include <net/if.h>
+
+using namespace saivs;
+
+#define EAPOL_ETHER_TYPE (0x888e)
+
+MACsecFilter::MACsecFilter(
+    _In_ const std::string &macsec_interface_name):
+    m_macsec_device_enable(false),
+    m_macsecfd(0),
+    m_macsec_interface_name(macsec_interface_name)
+{
+    SWSS_LOG_ENTER();
+
+    // empty intentionally
+}
+
+void MACsecFilter::enable_macsec_device(bool enable)
+{
+    m_macsec_device_enable = enable;
+}
+
+void MACsecFilter::set_macsec_fd(int macsecfd)
+{
+    m_macsecfd = macsecfd;
+}
+
+TrafficFilter::FilterStatus MACsecFilter::execute(
+    _Inout_ void *buffer,
+    _Inout_ ssize_t &length)
+{
+    SWSS_LOG_ENTER();
+
+    auto mac_hdr = static_cast<const ethhdr *>(buffer);
+
+    if (ntohs(mac_hdr->h_proto) == EAPOL_ETHER_TYPE)
+    {
+        // EAPOL traffic will never be delivered to MACsec device
+        return TrafficFilter::CONTINUE;
+    }
+
+    if (m_macsec_device_enable)
+    {
+        return forward(buffer, length);
+    }
+
+    // Drop all non-EAPOL packets if macsec device haven't been enable.
+    return TrafficFilter::TERMINATE;
+}

vslib/src/MACsecIngressFilter.cpp

@@ -0,0 +1,30 @@
+#include "MACsecIngressFilter.h"
+
+#include "swss/logger.h"
+
+#include <unistd.h>
+#include <string.h>
+
+using namespace saivs;
+
+MACsecIngressFilter::MACsecIngressFilter(
+    _In_ const std::string &macsec_interface_name) :
+    MACsecFilter(macsec_interface_name)
+{
+    SWSS_LOG_ENTER();
+
+    // empty intentionally
+}
+
+TrafficFilter::FilterStatus MACsecIngressFilter::forward(
+    _In_ const void *buffer,
+    _In_ ssize_t length)
+{
+    SWSS_LOG_ENTER();
+
+    // MACsec interface will automatically forward ingress MACsec traffic
+    // by Linux Kernel.
+    // So this filter just need to drop all ingress MACsec traffic directly
+
+    return TrafficFilter::TERMINATE;
+}

vslib/src/TrafficFilterPipes.cpp

@@ -0,0 +1,76 @@
+#include "TrafficFilterPipes.h"
+
+#include "swss/logger.h"
+
+using namespace saivs;
+
+bool TrafficFilterPipes::installFilter(
+    _In_ int priority,
+    _In_ std::shared_ptr<TrafficFilter> filter)
+{
+    SWSS_LOG_ENTER();
+
+    std::unique_lock<std::mutex> guard(m_mutex);
+
+    return m_filters.emplace(priority, filter).second;
+}
+
+bool TrafficFilterPipes::uninstallFilter(
+    _In_ std::shared_ptr<TrafficFilter> filter)
+{
+    SWSS_LOG_ENTER();
+
+    std::unique_lock<std::mutex> guard(m_mutex);
+
+    for (auto itr = m_filters.begin();
+        itr != m_filters.end();
+        itr ++)
+    {
+        if (itr->second == filter)
+        {
+            m_filters.erase(itr);
+
+            return true;
+        }
+    }
+
+    return false;
+}
+
+TrafficFilter::FilterStatus TrafficFilterPipes::execute(
+    _Inout_ void *buffer,
+    _Inout_ ssize_t &length)
+{
+    SWSS_LOG_ENTER();
+
+    std::unique_lock<std::mutex> guard(m_mutex);
+    TrafficFilter::FilterStatus ret = TrafficFilter::CONTINUE;
+
+    for (auto itr = m_filters.begin(); itr != m_filters.end();)
+    {
+        auto filter = itr->second;
+
+        if (filter)
+        {
+            ret = filter->execute(
+                buffer,
+                length);
+
+            if (ret == TrafficFilter::CONTINUE)
+            {
+                itr ++;
+            }
+            else
+            {
+                break;
+            }
+        }
+        else
+        {
+            itr = m_filters.erase(itr);
+        }
+
+    }
+
+    return ret;
+}