From ab35dfafc6048bbc4b629e1b39c3b138b83cfb5e Mon Sep 17 00:00:00 2001
From: Kamil Cudnik <kcudnik@gmail.com>
Date: Mon, 11 Feb 2019 12:39:55 +0100
Subject: [PATCH] Add support for vlan tagged frames in virtual switch (#417)

---
 vslib/src/sai_vs_hostintf.cpp | 74 ++++++++++++++++++++++++++++++++++-
 1 file changed, 73 insertions(+), 1 deletion(-)

diff --git a/vslib/src/sai_vs_hostintf.cpp b/vslib/src/sai_vs_hostintf.cpp
index 940877931f22..94338ad9a140 100644
--- a/vslib/src/sai_vs_hostintf.cpp
+++ b/vslib/src/sai_vs_hostintf.cpp
@@ -794,6 +794,11 @@ int promisc(const char *dev)
     return err;
 }
 
+#define ETH_JUMBO_FRAME_SIZE (9000)
+#define IEEE_8021Q_ETHER_TYPE (0x8100)
+#define MAC_ADDRESS_SIZE (6)
+#define VLAN_TAG_SIZE (4)
+
 void veth2tap_fun(std::shared_ptr<hostif_info_t> info)
 {
     SWSS_LOG_ENTER();
@@ -804,7 +809,26 @@ void veth2tap_fun(std::shared_ptr<hostif_info_t> info)
     {
         // TODO convert to non blocking using select
 
-        ssize_t size = read(info->packet_socket, buffer, sizeof(buffer));
+        struct msghdr  msg;
+        memset(&msg, 0, sizeof(struct msghdr));
+
+        struct sockaddr_storage src_addr;
+
+        struct iovec iov[1];
+
+        iov[0].iov_base = buffer;       // buffer for message
+        iov[0].iov_len = sizeof(buffer);
+
+        char control[0x1000];   // buffer for control messages
+
+        msg.msg_name = &src_addr;
+        msg.msg_namelen = sizeof(src_addr);
+        msg.msg_iov = iov;
+        msg.msg_iovlen = 1;
+        msg.msg_control = control;
+        msg.msg_controllen = sizeof(control);
+
+        ssize_t size = recvmsg(info->packet_socket, &msg, 0);
 
         if (size < 0)
         {
@@ -814,6 +838,47 @@ void veth2tap_fun(std::shared_ptr<hostif_info_t> info)
             continue;
         }
 
+        if (size < (ssize_t)sizeof(ethhdr) || size > ETH_JUMBO_FRAME_SIZE)
+        {
+            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);
+
+                uint16_t* pvlan = (uint16_t*)(buffer + 2 * MAC_ADDRESS_SIZE);
+
+                pvlan[0] = tpid;
+                pvlan[1] = tci;
+
+                size += VLAN_TAG_SIZE;
+
+                break;
+            }
+        }
+
         process_packet_for_fdb_event(buffer, size, info);
 
         if (write(info->tapfd, buffer, size) < 0)
@@ -934,6 +999,13 @@ bool hostif_create_tap_veth_forwarding(
         return false;
     }
 
+    int val = 1;
+    if (setsockopt(packet_socket, SOL_PACKET, PACKET_AUXDATA, &val, sizeof(val)) < 0)
+    {
+        SWSS_LOG_ERROR("setsockopt() set PACKET_AUXDATA failed: %s", strerror(errno));
+        return false;
+    }
+
     // bind to device
 
     struct sockaddr_ll sock_address;