QoS: marking and recording for MPLS and VLAN

Change-Id: Icec79aa9039d5d7835d311fde0b7c1a0c76c9eb1
Signed-off-by: Neale Ranns <nranns@cisco.com>
(cherry picked from commit 0809f6c0300f85cf5cf5d49df1aa8f1e2d080f6c)

3 files changed, 267 insertions, 18 deletions

diff --git a/src/vnet/ethernet/packet.h b/src/vnet/ethernet/packet.h
index 1a0506dc4d6..d70960b0f7b 100644
--- a/src/vnet/ethernet/packet.h
+++ b/src/vnet/ethernet/packet.h
@@ -146,6 +146,14 @@ ethernet_vlan_header_set_priority_net_order (ethernet_vlan_header_t * h,
   bytes[0] |= (prio & 0x7) << 5;
 }
 
+always_inline u8
+ethernet_vlan_header_get_priority_net_order (ethernet_vlan_header_t * h)
+{
+  u8 *bytes = (u8 *) (&h->priority_cfi_and_id);
+
+  return (bytes[0] >> 5);
+}
+
 /* VLAN with ethertype first and vlan id second */
 typedef struct
 {
diff --git a/src/vnet/qos/qos_mark.c b/src/vnet/qos/qos_mark.c
index 5dc87e8ef81..0f569b766a6 100644
--- a/src/vnet/qos/qos_mark.c
+++ b/src/vnet/qos/qos_mark.c
@@ -36,7 +36,15 @@ qos_mark_ip_enable_disable (u32 sw_if_index, u8 enable)
 void
 qos_mark_vlan_enable_disable (u32 sw_if_index, u8 enable)
 {
-  vnet_feature_enable_disable ("interface-output", "vlan-qos-mark",
+  /*
+   * one cannot run a feature on a sub-interface, so we need
+   * to enable a feature on all the L3 output paths
+   */
+  vnet_feature_enable_disable ("ip6-output", "vlan-ip6-qos-mark",
+			       sw_if_index, enable, NULL, 0);
+  vnet_feature_enable_disable ("ip4-output", "vlan-ip4-qos-mark",
+			       sw_if_index, enable, NULL, 0);
+  vnet_feature_enable_disable ("mpls-output", "vlan-mpls-qos-mark",
 			       sw_if_index, enable, NULL, 0);
 }
 
@@ -239,8 +247,22 @@ mpls_qos_mark (vlib_main_t * vm, vlib_node_runtime_t * node,
 }
 
 static inline uword
-vlan_qos_mark (vlib_main_t * vm, vlib_node_runtime_t * node,
-	       vlib_frame_t * frame)
+vlan_mpls_qos_mark (vlib_main_t * vm, vlib_node_runtime_t * node,
+		    vlib_frame_t * frame)
+{
+  return (qos_mark_inline (vm, node, frame, QOS_SOURCE_VLAN, 0));
+}
+
+static inline uword
+vlan_ip4_qos_mark (vlib_main_t * vm, vlib_node_runtime_t * node,
+		   vlib_frame_t * frame)
+{
+  return (qos_mark_inline (vm, node, frame, QOS_SOURCE_VLAN, 0));
+}
+
+static inline uword
+vlan_ip6_qos_mark (vlib_main_t * vm, vlib_node_runtime_t * node,
+		   vlib_frame_t * frame)
 {
   return (qos_mark_inline (vm, node, frame, QOS_SOURCE_VLAN, 0));
 }
@@ -311,9 +333,10 @@ VNET_FEATURE_INIT (mpls_qos_mark_node, static) = {
     .arc_name = "mpls-output",
     .node_name = "mpls-qos-mark",
 };
-VLIB_REGISTER_NODE (vlan_qos_mark_node) = {
-  .function = vlan_qos_mark,
-  .name = "vlan-qos-mark",
+
+VLIB_REGISTER_NODE (vlan_ip4_qos_mark_node) = {
+  .function = vlan_ip4_qos_mark,
+  .name = "vlan-ip4-qos-mark",
   .vector_size = sizeof (u32),
   .format_trace = format_qos_mark_trace,
   .type = VLIB_NODE_TYPE_INTERNAL,
@@ -326,11 +349,58 @@ VLIB_REGISTER_NODE (vlan_qos_mark_node) = {
   },
 };
 
-VLIB_NODE_FUNCTION_MULTIARCH (vlan_qos_mark_node, vlan_qos_mark);
+VLIB_NODE_FUNCTION_MULTIARCH (vlan_ip4_qos_mark_node, vlan_ip4_qos_mark);
+
+VNET_FEATURE_INIT (vlan_ip4_qos_mark_node, static) = {
+    .arc_name = "ip4-output",
+    .node_name = "vlan-ip4-qos-mark",
+    .runs_after = VNET_FEATURES ("ip4-qos-mark"),
+};
+
+VLIB_REGISTER_NODE (vlan_ip6_qos_mark_node) = {
+  .function = vlan_ip6_qos_mark,
+  .name = "vlan-ip6-qos-mark",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_mark_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
 
-VNET_FEATURE_INIT (vlan_qos_mark_node, static) = {
-    .arc_name = "interface-output",
-    .node_name = "vlan-qos-mark",
+  .next_nodes = {
+    [0] = "error-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (vlan_ip6_qos_mark_node, vlan_ip6_qos_mark);
+
+VNET_FEATURE_INIT (vlan_ip6_qos_mark_node, static) = {
+    .arc_name = "ip6-output",
+    .node_name = "vlan-ip6-qos-mark",
+    .runs_after = VNET_FEATURES ("ip6-qos-mark"),
+};
+
+VLIB_REGISTER_NODE (vlan_mpls_qos_mark_node) = {
+  .function = vlan_mpls_qos_mark,
+  .name = "vlan-mpls-qos-mark",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_mark_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
+
+  .next_nodes = {
+    [0] = "error-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (vlan_mpls_qos_mark_node, vlan_mpls_qos_mark);
+
+VNET_FEATURE_INIT (vlan_mpls_qos_mark_node, static) = {
+    .arc_name = "mpls-output",
+    .node_name = "vlan-mpls-qos-mark",
+    .runs_after = VNET_FEATURES ("mpls-qos-mark"),
 };
 /* *INDENT-ON* */
 
diff --git a/src/vnet/qos/qos_record.c b/src/vnet/qos/qos_record.c
index 7181e3bf599..92620458f08 100644
--- a/src/vnet/qos/qos_record.c
+++ b/src/vnet/qos/qos_record.c
@@ -44,9 +44,23 @@ qos_record_feature_config (u32 sw_if_index,
 				  enable);
       break;
     case QOS_SOURCE_MPLS:
+      vnet_feature_enable_disable ("mpls-input", "mpls-qos-record",
+				   sw_if_index, enable, NULL, 0);
+      break;
     case QOS_SOURCE_VLAN:
+      vnet_feature_enable_disable ("ip6-unicast", "vlan-ip6-qos-record",
+				   sw_if_index, enable, NULL, 0);
+      vnet_feature_enable_disable ("ip6-multicast", "vlan-ip6-qos-record",
+				   sw_if_index, enable, NULL, 0);
+      vnet_feature_enable_disable ("ip4-unicast", "vlan-ip4-qos-record",
+				   sw_if_index, enable, NULL, 0);
+      vnet_feature_enable_disable ("ip4-multicast", "vlan-ip4-qos-record",
+				   sw_if_index, enable, NULL, 0);
+      vnet_feature_enable_disable ("mpls-input", "vlan-mpls-qos-record",
+				   sw_if_index, enable, NULL, 0);
+      break;
     case QOS_SOURCE_EXT:
-      // not implemented yet
+      /* not a valid option for recording */
       break;
     }
 }
@@ -119,7 +133,7 @@ typedef struct qos_record_trace_t_
 static inline uword
 qos_record_inline (vlib_main_t * vm,
 		   vlib_node_runtime_t * node,
-		   vlib_frame_t * frame, int is_ip6, int is_l2)
+		   vlib_frame_t * frame, dpo_proto_t dproto, int is_l2)
 {
   u32 n_left_from, *from, *to_next, next_index;
 
@@ -164,23 +178,42 @@ qos_record_inline (vlib_main_t * vm,
 	      ethertype = clib_net_to_host_u16 (*(u16 *) (l3h - 2));
 
 	      if (ethertype == ETHERNET_TYPE_IP4)
-		is_ip6 = 0;
+		dproto = DPO_PROTO_IP4;
 	      else if (ethertype == ETHERNET_TYPE_IP6)
-		is_ip6 = 1;
+		dproto = DPO_PROTO_IP6;
+	      else if (ethertype == ETHERNET_TYPE_MPLS)
+		dproto = DPO_PROTO_MPLS;
 	      else
 		goto non_ip;
 	    }
 
-	  if (is_ip6)
+	  if (DPO_PROTO_IP6 == dproto)
 	    {
 	      ip6_0 = vlib_buffer_get_current (b0);
 	      qos0 = ip6_traffic_class_network_order (ip6_0);
 	    }
-	  else
+	  else if (DPO_PROTO_IP4 == dproto)
 	    {
 	      ip4_0 = vlib_buffer_get_current (b0);
 	      qos0 = ip4_0->tos;
 	    }
+	  else if (DPO_PROTO_ETHERNET == dproto)
+	    {
+	      ethernet_vlan_header_t *vlan0;
+
+	      vlan0 = (vlib_buffer_get_current (b0) -
+		       sizeof (ethernet_vlan_header_t));
+
+	      qos0 = ethernet_vlan_header_get_priority_net_order (vlan0);
+	    }
+	  else if (DPO_PROTO_MPLS)
+	    {
+	      mpls_unicast_header_t *mh;
+
+	      mh = vlib_buffer_get_current (b0);
+	      qos0 = vnet_mpls_uc_get_exp (mh->label_exp_s_ttl);
+	    }
+
 	  vnet_buffer2 (b0)->qos.bits = qos0;
 	  vnet_buffer2 (b0)->qos.source = QOS_SOURCE_IP;
 	  b0->flags |= VNET_BUFFER_F_QOS_DATA_VALID;
@@ -234,14 +267,42 @@ static inline uword
 ip4_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
 		vlib_frame_t * frame)
 {
-  return (qos_record_inline (vm, node, frame, 0, 0));
+  return (qos_record_inline (vm, node, frame, DPO_PROTO_IP4, 0));
 }
 
 static inline uword
 ip6_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
 		vlib_frame_t * frame)
 {
-  return (qos_record_inline (vm, node, frame, 1, 0));
+  return (qos_record_inline (vm, node, frame, DPO_PROTO_IP6, 0));
+}
+
+static inline uword
+mpls_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
+		 vlib_frame_t * frame)
+{
+  return (qos_record_inline (vm, node, frame, DPO_PROTO_MPLS, 0));
+}
+
+static inline uword
+vlan_ip4_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
+		     vlib_frame_t * frame)
+{
+  return (qos_record_inline (vm, node, frame, DPO_PROTO_ETHERNET, 0));
+}
+
+static inline uword
+vlan_ip6_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
+		     vlib_frame_t * frame)
+{
+  return (qos_record_inline (vm, node, frame, DPO_PROTO_ETHERNET, 0));
+}
+
+static inline uword
+vlan_mpls_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
+		      vlib_frame_t * frame)
+{
+  return (qos_record_inline (vm, node, frame, DPO_PROTO_ETHERNET, 0));
 }
 
 static inline uword
@@ -273,6 +334,10 @@ VNET_FEATURE_INIT (ip4_qos_record_node, static) = {
     .arc_name = "ip4-unicast",
     .node_name = "ip4-qos-record",
 };
+VNET_FEATURE_INIT (ip4m_qos_record_node, static) = {
+    .arc_name = "ip4-multicast",
+    .node_name = "ip4-qos-record",
+};
 
 VLIB_REGISTER_NODE (ip6_qos_record_node) = {
   .function = ip6_qos_record,
@@ -295,6 +360,111 @@ VNET_FEATURE_INIT (ip6_qos_record_node, static) = {
     .arc_name = "ip6-unicast",
     .node_name = "ip6-qos-record",
 };
+VNET_FEATURE_INIT (ip6m_qos_record_node, static) = {
+    .arc_name = "ip6-multicast",
+    .node_name = "ip6-qos-record",
+};
+
+VLIB_REGISTER_NODE (mpls_qos_record_node) = {
+  .function = mpls_qos_record,
+  .name = "mpls-qos-record",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_record_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
+
+  .next_nodes = {
+    [0] = "mpls-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (mpls_qos_record_node, mpls_qos_record);
+
+VNET_FEATURE_INIT (mpls_qos_record_node, static) = {
+    .arc_name = "mpls-input",
+    .node_name = "mpls-qos-record",
+};
+
+VLIB_REGISTER_NODE (vlan_mpls_qos_record_node) = {
+  .function = vlan_mpls_qos_record,
+  .name = "vlan-mpls-qos-record",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_record_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
+
+  .next_nodes = {
+    [0] = "mpls-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (vlan_mpls_qos_record_node, vlan_mpls_qos_record);
+
+VNET_FEATURE_INIT (vlan_mpls_qos_record_node, static) = {
+    .arc_name = "mpls-input",
+    .node_name = "vlan-mpls-qos-record",
+    .runs_before = VNET_FEATURES ("mpls-qos-mark"),
+};
+
+VLIB_REGISTER_NODE (vlan_ip4_qos_record_node) = {
+  .function = vlan_ip4_qos_record,
+  .name = "vlan-ip4-qos-record",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_record_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
+
+  .next_nodes = {
+    [0] = "ip4-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (vlan_ip4_qos_record_node, vlan_ip4_qos_record);
+
+VNET_FEATURE_INIT (vlan_ip4_qos_record_node, static) = {
+    .arc_name = "ip4-unicast",
+    .node_name = "vlan-ip4-qos-record",
+    .runs_before = VNET_FEATURES ("ip4-qos-mark"),
+};
+VNET_FEATURE_INIT (vlan_ip4m_qos_record_node, static) = {
+    .arc_name = "ip4-multicast",
+    .node_name = "vlan-ip4-qos-record",
+    .runs_before = VNET_FEATURES ("ip4-qos-mark"),
+};
+
+VLIB_REGISTER_NODE (vlan_ip6_qos_record_node) = {
+  .function = vlan_ip6_qos_record,
+  .name = "vlan-ip6-qos-record",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_record_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
+
+  .next_nodes = {
+    [0] = "ip6-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (vlan_ip6_qos_record_node, vlan_ip6_qos_record);
+
+VNET_FEATURE_INIT (vlan_ip6_qos_record_node, static) = {
+    .arc_name = "ip6-unicast",
+    .node_name = "vlan-ip6-qos-record",
+    .runs_before = VNET_FEATURES ("ip6-qos-mark"),
+};
+VNET_FEATURE_INIT (vlan_ip6m_qos_record_node, static) = {
+    .arc_name = "ip6-multicast",
+    .node_name = "vlan-ip6-qos-record",
+    .runs_before = VNET_FEATURES ("ip6-qos-mark"),
+};
 
 VLIB_REGISTER_NODE (l2_ip_qos_record_node, static) = {
   .function = l2_ip_qos_record,
@@ -313,6 +483,7 @@ VLIB_REGISTER_NODE (l2_ip_qos_record_node, static) = {
 };
 
 VLIB_NODE_FUNCTION_MULTIARCH (l2_ip_qos_record_node, l2_ip_qos_record);
+
 /* *INDENT-ON* */
 
 clib_error_t *