.1p based QOS marking for L2 packets

For packets that are not IPv4 or IPv6, qos remarking will be based on
802.1p information present in the packet. For supporting this feature,
we do the following:

. save the priority information in the packet before untagging the
  packet, if it comes in the vRouter vlan interface
. use this saved information to lookup in the qos configuration table
  to determine the forwarding class, as is the case with DSCP based
  lookups
. save and restore 802.1p information in the forwarding metadata, if
  and when the packet is queued somewhere

Also, fix the incremental checksum code that comes into play when tos
bits in the IPv4 header is changed. There is a problem with the current
code where the diff is shifted by 8 bits to reflect the position of TOS
bits in the IPv4 header. We should do this shift to original values
rather than the diff.

Change-Id: Iade0adae4163a3dd5e4ad9ae41cb91c6bcbb8c0d
Closes-Bug: #1603340
Closes-Bug: #1603864

dp-core/vr_bridge.c

@@ -478,15 +478,20 @@ vr_bridge_input(struct vrouter *router, struct vr_packet *pkt,
             vr_pfree(pkt, VP_DROP_PULL);
             return 0;
         }
-    }
 
-    if (fmd->fmd_dscp < 0) {
-        if (pkt->vp_type == VP_TYPE_IP) {
-            fmd->fmd_dscp =
-                vr_inet_get_tos((struct vr_ip *)pkt_network_header(pkt));
-        } else if (pkt->vp_type == VP_TYPE_IP6) {
-            fmd->fmd_dscp =
-                vr_inet6_get_tos((struct vr_ip6 *)pkt_network_header(pkt));
+        if (fmd->fmd_dscp < 0) {
+            if (pkt->vp_type == VP_TYPE_IP) {
+                fmd->fmd_dscp =
+                    vr_inet_get_tos((struct vr_ip *)pkt_network_header(pkt));
+            } else if (pkt->vp_type == VP_TYPE_IP6) {
+                fmd->fmd_dscp =
+                    vr_inet6_get_tos((struct vr_ip6 *)pkt_network_header(pkt));
+            }
+        }
+
+    } else {
+        if (fmd->fmd_dotonep < 0) {
+            fmd->fmd_dotonep = vr_vlan_get_tos(pkt_data(pkt));
         }
     }
 

dp-core/vr_datapath.c

@@ -561,6 +561,10 @@ vr_virtual_input(unsigned short vrf, struct vr_interface *vif,
     vr_init_forwarding_md(&fmd);
     fmd.fmd_vlan = vlan_id;
     fmd.fmd_dvrf = vrf;
+    if (pkt->vp_priority != VP_PRIORITY_INVALID) {
+        fmd.fmd_dotonep = pkt->vp_priority;
+        pkt->vp_priority = VP_PRIORITY_INVALID;
+    }
 
     if (vr_pkt_type(pkt, 0, &fmd) < 0) {
         vif_drop_pkt(vif, pkt, 1);
@@ -686,6 +690,7 @@ vr_untag_pkt(struct vr_packet *pkt)
 int
 vr_tag_pkt(struct vr_packet *pkt, unsigned short vlan_id)
 {
+    uint8_t priority = 0;
     struct vr_eth *new_eth, *eth;
     unsigned short *vlan_tag;
 
@@ -700,7 +705,10 @@ 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((pkt->vp_priority << VR_VLAN_PRIORITY_SHIFT) | vlan_id);
+    if (pkt->vp_priority != VP_PRIORITY_INVALID)
+        priority = pkt->vp_priority;
+
+    *vlan_tag = htons((priority << VR_VLAN_PRIORITY_SHIFT) | vlan_id);
     return 0;
 }
 
@@ -713,9 +721,10 @@ vr_vlan_set_priority(struct vr_packet *pkt)
     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);
+        if (pkt->vp_priority != VP_PRIORITY_INVALID) {
+            vlan->vlan_tag |=
+                htons((pkt->vp_priority << VR_VLAN_PRIORITY_SHIFT));
+        }
     }
 
     return;

dp-core/vr_flow.c

@@ -602,6 +602,7 @@ vr_flow_fill_pnode(struct vr_packet_node *pnode, struct vr_packet *pkt,
     }
 
     pnode->pl_dscp = fmd->fmd_dscp;
+    pnode->pl_dotonep = fmd->fmd_dotonep;
     pnode->pl_vrf = fmd->fmd_dvrf;
     pnode->pl_vlan = fmd->fmd_vlan;
 
@@ -1405,6 +1406,7 @@ vr_flow_flush_pnode(struct vrouter *router, struct vr_packet_node *pnode,
         return -EINVAL;
 
     fmd->fmd_dscp = pnode->pl_dscp;
+    fmd->fmd_dotonep = pnode->pl_dotonep;
     pnode->pl_packet = NULL;
     /*
      * this is only a security check and not a catch all check. one note

dp-core/vr_interface.c

@@ -733,13 +733,18 @@ static int
 vlan_rx(struct vr_interface *vif, struct vr_packet *pkt,
         unsigned short vlan_id __attribute__((unused)))
 {
+    int8_t tos;
     struct vr_interface_stats *stats = vif_get_stats(vif, pkt->vp_cpu);
 
     pkt->vp_if = vif;
 
     stats->vis_ibytes += pkt_len(pkt);
     stats->vis_ipackets++;
 
+    tos = vr_vlan_get_tos(pkt_data(pkt));
+    if (tos >= 0)
+        pkt->vp_priority = tos;
+
     if (vr_untag_pkt(pkt)) {
         stats->vis_ierrors++;
         vr_pfree(pkt, VP_DROP_PULL);

dp-core/vr_qos.c

@@ -742,7 +742,7 @@ struct vr_forwarding_class_qos *
 vr_qos_get_forwarding_class(struct vrouter *router, struct vr_packet *pkt,
         struct vr_forwarding_md *fmd)
 {
-    uint8_t tos;
+    int8_t tos;
     int16_t qos_id = -1;
     unsigned int fc_id;
 
@@ -764,7 +764,14 @@ vr_qos_get_forwarding_class(struct vrouter *router, struct vr_packet *pkt,
         if (!fc_p)
             return NULL;
 
-        tos = fmd->fmd_dscp;
+        if (pkt->vp_type == VP_TYPE_IP || pkt->vp_type == VP_TYPE_IP6) {
+            tos = fmd->fmd_dscp;
+        } else {
+            tos = fmd->fmd_dotonep;
+            if (tos >= 0)
+                tos += VR_DSCP_QOS_ENTRIES + VR_MPLS_QOS_ENTRIES;
+        }
+
         if (tos >= 0) {
             fc_id = fc_p[tos].vfc_id;
             fc_p = vr_fc_map_get_fc(router, fc_id);

include/vr_flow.h

@@ -234,7 +234,8 @@ struct vr_packet_node {
     uint32_t pl_label;
     uint32_t pl_vif_idx;
     uint16_t pl_flags;
-    int16_t pl_dscp;
+    int8_t pl_dscp;
+    int8_t pl_dotonep;
     uint32_t pl_vrf;
     int32_t pl_vlan;
 };

include/vr_packet.h

@@ -261,6 +261,8 @@ struct vr_packet {
 };
 
 
+#define VP_PRIORITY_INVALID 0xF
+
 extern void pkt_reset(struct vr_packet *);
 extern struct vr_packet *pkt_copy(struct vr_packet *, unsigned short,
         unsigned short);
@@ -288,6 +290,7 @@ struct vr_vlan_hdr {
     unsigned short vlan_proto;
 } __attribute__((packed));
 
+
 #define VR_ARP_HW_LEN           6
 #define VR_ARP_OP_REQUEST       1
 #define VR_ARP_OP_REPLY         2
@@ -299,6 +302,19 @@ struct vr_vlan_hdr {
 
 #define VR_DIAG_CSUM         0xffff
 
+static inline int8_t
+vr_vlan_get_tos(uint8_t *eth_data)
+{
+    struct vr_eth *eth = (struct vr_eth *)eth_data;
+    struct vr_vlan_hdr *vlan;
+
+    if (ntohs(eth->eth_proto) != VR_ETH_PROTO_VLAN)
+        return -1;
+
+    vlan = (struct vr_vlan_hdr *)(eth + 1);
+    return (ntohs(vlan->vlan_tag) >> VR_VLAN_PRIORITY_SHIFT);
+}
+
 #ifdef arp_op
 #undef arp_op
 #endif
@@ -421,8 +437,7 @@ vr_inet_set_tos(struct vr_ip *iph, uint8_t tos)
     if (iph->ip_tos == tos)
         return;
 
-    vr_incremental_diff(iph->ip_tos, tos, &diff);
-    diff <<= 8;
+    vr_incremental_diff(iph->ip_tos << 8, tos << 8, &diff);
     iph->ip_tos = tos;
     vr_ip_incremental_csum(iph, diff);
 
@@ -891,6 +906,7 @@ struct vr_forwarding_md {
     uint8_t fmd_src;
     uint8_t fmd_flags;
     int8_t fmd_dscp;
+    int8_t fmd_dotonep;
     int8_t fmd_queue;
 };
 
@@ -909,6 +925,7 @@ vr_init_forwarding_md(struct vr_forwarding_md *fmd)
     fmd->fmd_src = 0;
     fmd->fmd_flags = 0;
     fmd->fmd_dscp = -1;
+    fmd->fmd_dotonep = -1;
     return;
 }
 

linux/vr_host_interface.c

@@ -793,8 +793,9 @@ linux_if_tx(struct vr_interface *vif, struct vr_packet *pkt)
     }
 
     skb_reset_mac_header(skb);
-    skb->queue_mapping = pkt->vp_queue;
-    skb->priority = pkt->vp_priority;
+    /* skb->queue_mapping = pkt->vp_queue; */
+    if (pkt->vp_priority != VP_PRIORITY_INVALID)
+        skb->priority = pkt->vp_priority;
 
     /*
      * Set the network header and trasport header of skb only if the type is
@@ -923,7 +924,7 @@ linux_get_packet(struct sk_buff *skb, struct vr_interface *vif)
     pkt->vp_ttl = 64;
     pkt->vp_type = VP_TYPE_NULL;
     pkt->vp_queue = 0;
-    pkt->vp_priority = 0;
+    pkt->vp_priority = VP_PRIORITY_INVALID;
 
     return pkt;