From 67f37d7713bca2788bbb3ff4cad447de3961c685 Mon Sep 17 00:00:00 2001
From: "Anand H. Krishnan" <anandhk@juniper.net>
Date: Thu, 16 Jun 2016 10:08:44 +0530
Subject: [PATCH] Add priority bits to the vlan tag for subinterfaces

If we are transmitting packets to a vlan sub-interface, along with
the vlan tag, we should also mark the priority bits of the tag based
on the priority that vRouter has determined for the packet.

Change-Id: I7fa27786735e0be618af6baaa145f3e307ede193
Partial-BUG: #1571493
---
 dp-core/vr_datapath.c  | 19 ++++++++++++++++++-
 dp-core/vr_interface.c | 16 ++++++++++------
 include/vr_datapath.h  |  1 +
 include/vr_packet.h    |  1 +
 4 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/dp-core/vr_datapath.c b/dp-core/vr_datapath.c
index 28ccd9f99..0a4683722 100644
--- a/dp-core/vr_datapath.c
+++ b/dp-core/vr_datapath.c
@@ -700,10 +700,27 @@ vr_tag_pkt(struct vr_packet *pkt, unsigned short vlan_id)
     memmove(new_eth, eth, (2 * VR_ETHER_ALEN));
     new_eth->eth_proto = htons(VR_ETH_PROTO_VLAN);
     vlan_tag = (unsigned short *)(new_eth + 1);
-    *vlan_tag = htons(vlan_id);
+    *vlan_tag = htons((pkt->vp_priority << VR_VLAN_PRIORITY_SHIFT) | vlan_id);
     return 0;
 }
 
+void
+vr_vlan_set_priority(struct vr_packet *pkt)
+{
+    struct vr_eth *eth;
+    struct vr_vlan_hdr *vlan;
+
+    eth = (struct vr_eth *)pkt_data(pkt);
+    if (eth->eth_proto == htons(VR_ETH_PROTO_VLAN)) {
+        vlan = (struct vr_vlan_hdr *)(eth + 1);
+        vlan->vlan_tag |= htons((pkt->vp_priority << VR_VLAN_PRIORITY_SHIFT));
+    } else {
+        vr_tag_pkt(pkt, 0);
+    }
+
+    return;
+}
+
 int
 vr_gro_input(struct vr_packet *pkt, struct vr_nexthop *nh)
 {
diff --git a/dp-core/vr_interface.c b/dp-core/vr_interface.c
index 4835cd076..b9bfc946b 100644
--- a/dp-core/vr_interface.c
+++ b/dp-core/vr_interface.c
@@ -757,6 +757,7 @@ vlan_tx(struct vr_interface *vif, struct vr_packet *pkt,
         struct vr_forwarding_md *fmd)
 {
     int ret = 0;
+
     struct vr_interface *pvif;
     struct vr_interface_stats *stats = vif_get_stats(vif, pkt->vp_cpu);
 
@@ -766,15 +767,18 @@ vlan_tx(struct vr_interface *vif, struct vr_packet *pkt,
     }
 
     fmd->fmd_vlan = vif->vif_vlan_id;
-    if (vif_is_vlan(vif) && vif->vif_ovlan_id) {
-        fmd->fmd_vlan = vif->vif_ovlan_id;
-        if (vr_tag_pkt(pkt, vif->vif_ovlan_id)) {
-            goto drop;
+    if (vif_is_vlan(vif)) {
+        if (vif->vif_ovlan_id) {
+            fmd->fmd_vlan = vif->vif_ovlan_id;
+            if (vr_tag_pkt(pkt, vif->vif_ovlan_id)) {
+                goto drop;
+            }
+            vr_pset_data(pkt, pkt->vp_data);
+        } else {
+            vr_vlan_set_priority(pkt);
         }
-        vr_pset_data(pkt, pkt->vp_data);
     }
 
-
     pvif = vif->vif_parent;
     if (!pvif)
         goto drop;
diff --git a/include/vr_datapath.h b/include/vr_datapath.h
index 04f7b9b41..ad3dd4ecc 100644
--- a/include/vr_datapath.h
+++ b/include/vr_datapath.h
@@ -38,6 +38,7 @@ extern uint16_t vr_icmp6_checksum(struct vr_ip6 *, struct vr_icmp *);
 
 int vr_untag_pkt(struct vr_packet *);
 int vr_tag_pkt(struct vr_packet *, unsigned short);
+void vr_vlan_set_priority(struct vr_packet *);
 int vr_pkt_type(struct vr_packet *, unsigned short, struct vr_forwarding_md *);
 
 int vr_trap(struct vr_packet *, unsigned short, unsigned short, void *);
diff --git a/include/vr_packet.h b/include/vr_packet.h
index fd1c014ac..833b9b97e 100644
--- a/include/vr_packet.h
+++ b/include/vr_packet.h
@@ -278,6 +278,7 @@ struct vr_eth {
 
 #define VLAN_ID_INVALID         0xFFFF
 #define VLAN_ID_MAX             0xFFFF
+#define VR_VLAN_PRIORITY_SHIFT  13
 
 struct vr_vlan_hdr {
     unsigned short vlan_tag;