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

@@ -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)
 {

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;

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 *);

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;