diff --git a/vslib/inc/HostInterfaceInfo.h b/vslib/inc/HostInterfaceInfo.h index dc97e1dad..c9e29cfe8 100644 --- a/vslib/inc/HostInterfaceInfo.h +++ b/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 filter); + + bool uninstallEth2TapFilter( + _In_ std::shared_ptr filter); + + bool installTap2EthFilter( + _In_ int priority, + _In_ std::shared_ptr filter); + + bool uninstallTap2EthFilter( + _In_ std::shared_ptr filter); + private: void veth2tap_fun(); @@ -58,13 +73,16 @@ namespace saivs std::shared_ptr m_eventQueue; - private: - int m_tapfd; + private: + std::shared_ptr m_e2t; std::shared_ptr m_t2e; + TrafficFilterPipes m_e2tFilters; + TrafficFilterPipes m_t2eFilters; + swss::SelectableEvent m_e2tEvent; swss::SelectableEvent m_t2eEvent; }; diff --git a/vslib/inc/MACsecFilter.h b/vslib/inc/MACsecFilter.h index 3cd06f5c6..6e272bb21 100644 --- a/vslib/inc/MACsecFilter.h +++ b/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; }; } diff --git a/vslib/inc/MACsecForwarder.h b/vslib/inc/MACsecForwarder.h index 3d0a5a7f2..a62132bb1 100644 --- a/vslib/inc/MACsecForwarder.h +++ b/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 info); virtual ~MACsecForwarder(); @@ -33,5 +36,7 @@ namespace saivs swss::SelectableEvent m_exitEvent; std::shared_ptr m_forwardThread; + + std::shared_ptr m_info; }; } diff --git a/vslib/src/HostInterfaceInfo.cpp b/vslib/src/HostInterfaceInfo.cpp index 02cb88c76..8c43f436b 100644 --- a/vslib/src/HostInterfaceInfo.cpp +++ b/vslib/src/HostInterfaceInfo.cpp @@ -90,6 +90,40 @@ void HostInterfaceInfo::async_process_packet_for_fdb_event( m_eventQueue->enqueue(std::make_shared(EventType::EVENT_TYPE_PACKET, payload)); } +bool HostInterfaceInfo::installEth2TapFilter( + _In_ int priority, + _In_ std::shared_ptr filter) +{ + SWSS_LOG_ENTER(); + + return m_e2tFilters.installFilter(priority, filter); +} + +bool HostInterfaceInfo::uninstallEth2TapFilter( + _In_ std::shared_ptr filter) +{ + SWSS_LOG_ENTER(); + + return m_e2tFilters.uninstallFilter(filter); +} + +bool HostInterfaceInfo::installTap2EthFilter( + _In_ int priority, + _In_ std::shared_ptr filter) +{ + SWSS_LOG_ENTER(); + + return m_t2eFilters.installFilter(priority, filter); +} + +bool HostInterfaceInfo::uninstallTap2EthFilter( + _In_ std::shared_ptr 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); + auto ret = m_e2tFilters.execute(buffer, length); + size = static_cast(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); + auto ret = m_t2eFilters.execute(buffer, length); + size = static_cast(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) diff --git a/vslib/src/MACsecForwarder.cpp b/vslib/src/MACsecForwarder.cpp index 72fcb500e..9fb02abb6 100644 --- a/vslib/src/MACsecForwarder.cpp +++ b/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 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(size)) < 0) { diff --git a/vslib/src/MACsecManager.cpp b/vslib/src/MACsecManager.cpp index f9c5a47dc..56ceb7001 100644 --- a/vslib/src/MACsecManager.cpp +++ b/vslib/src/MACsecManager.cpp @@ -579,7 +579,7 @@ bool MACsecManager::add_macsec_forwarder( auto &manager = itr->second; - manager.m_forwarder = std::make_shared(macsecInterface, manager.m_info->m_tapfd); + manager.m_forwarder = std::make_shared(macsecInterface, manager.m_info->m_tapfd, manager.m_info); return true; } diff --git a/vslib/src/Makefile.am b/vslib/src/Makefile.am index 81f2e40fb..621f93952 100644 --- a/vslib/src/Makefile.am +++ b/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 \