Support QOS_SOURCE_IP recording from L2 input node.

Some scenarios not involving ip[4,6]-input paths might benefit from IP
header QOS fields recorded and applied.
An example: L2 (overlay) traffic being encapsulated by VPP in VXLAN
and transmitted on another (underlay) interface might want the QOS
information carried over in the outer IP header.

Change-Id: I4d9462c47ae6ba97680edb1e53340b17cfd7845b
Signed-off-by: Igor Mikhailov (imichail) <imichail@cisco.com>
diff --git a/src/vnet/l2/l2_input.h b/src/vnet/l2/l2_input.h
index 5d67f25..23bb9b6 100644
--- a/src/vnet/l2/l2_input.h
+++ b/src/vnet/l2/l2_input.h
@@ -117,6 +117,7 @@
  _(GBP_NULL_CLASSIFY, "gbp-null-classify")      \
  _(GBP_SRC_CLASSIFY,  "gbp-src-classify")       \
  _(VTR,           "l2-input-vtr")               \
+ _(L2_IP_QOS_RECORD, "l2-ip-qos-record")        \
  _(VPATH,         "vpath-input-l2")             \
  _(ACL,           "l2-input-acl")               \
  _(POLICER_CLAS,  "l2-policer-classify")	\
diff --git a/src/vnet/qos/qos_record.c b/src/vnet/qos/qos_record.c
index 047bd25..c69b4f1 100644
--- a/src/vnet/qos/qos_record.c
+++ b/src/vnet/qos/qos_record.c
@@ -23,6 +23,7 @@
  * Per-interface, per-protocol vector of feature on/off configurations
  */
 static u8 *qos_record_configs[QOS_N_SOURCES];
+static u32 l2_qos_input_next[QOS_N_SOURCES][32];
 
 static void
 qos_record_feature_config (u32 sw_if_index,
@@ -39,6 +40,8 @@
 				   sw_if_index, enable, NULL, 0);
       vnet_feature_enable_disable ("ip4-multicast", "ip4-qos-record",
 				   sw_if_index, enable, NULL, 0);
+      l2input_intf_bitmap_enable (sw_if_index, L2INPUT_FEAT_L2_IP_QOS_RECORD,
+				  enable);
       break;
     case QOS_SOURCE_MPLS:
     case QOS_SOURCE_VLAN:
@@ -116,7 +119,7 @@
 static inline uword
 qos_record_inline (vlib_main_t * vm,
 		   vlib_node_runtime_t * node,
-		   vlib_frame_t * frame, int is_ip6)
+		   vlib_frame_t * frame, int is_ip6, int is_l2)
 {
   u32 n_left_from, *from, *to_next, next_index;
 
@@ -137,6 +140,7 @@
 	  vlib_buffer_t *b0;
 	  u32 sw_if_index0, next0, bi0;
 	  qos_bits_t qos0;
+	  u8 l2_len;
 
 	  next0 = 0;
 	  bi0 = from[0];
@@ -147,6 +151,26 @@
 	  n_left_to_next -= 1;
 
 	  b0 = vlib_get_buffer (vm, bi0);
+
+	  if (is_l2)
+	    {
+	      l2_len = vnet_buffer (b0)->l2.l2_len;
+	      u8 *l3h;
+	      u16 ethertype;
+
+	      vlib_buffer_advance (b0, l2_len);
+
+	      l3h = vlib_buffer_get_current (b0);
+	      ethertype = clib_net_to_host_u16 (*(u16 *) (l3h - 2));
+
+	      if (ethertype == ETHERNET_TYPE_IP4)
+		is_ip6 = 0;
+	      else if (ethertype == ETHERNET_TYPE_IP6)
+		is_ip6 = 1;
+	      else
+		goto non_ip;
+	    }
+
 	  if (is_ip6)
 	    {
 	      ip6_0 = vlib_buffer_get_current (b0);
@@ -162,8 +186,6 @@
 	  b0->flags |= VNET_BUFFER_F_QOS_DATA_VALID;
 	  sw_if_index0 = vnet_buffer (b0)->sw_if_index[VLIB_RX];
 
-	  vnet_feature_next (sw_if_index0, &next0, b0);
-
 	  if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE) &&
 			     (b0->flags & VLIB_BUFFER_IS_TRACED)))
 	    {
@@ -172,6 +194,17 @@
 	      t->bits = qos0;
 	    }
 
+	non_ip:
+	  if (is_l2)
+	    {
+	      vlib_buffer_advance (b0, -l2_len);
+	      next0 = vnet_l2_feature_next (b0,
+					    l2_qos_input_next[QOS_SOURCE_IP],
+					    L2INPUT_FEAT_L2_IP_QOS_RECORD);
+	    }
+	  else
+	    vnet_feature_next (sw_if_index0, &next0, b0);
+
 	  /* verify speculative enqueue, maybe switch current next frame */
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
 					   to_next, n_left_to_next,
@@ -201,14 +234,21 @@
 ip4_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
 		vlib_frame_t * frame)
 {
-  return (qos_record_inline (vm, node, frame, 0));
+  return (qos_record_inline (vm, node, frame, 0, 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));
+  return (qos_record_inline (vm, node, frame, 1, 0));
+}
+
+static inline uword
+l2_ip_qos_record (vlib_main_t * vm, vlib_node_runtime_t * node,
+		  vlib_frame_t * frame)
+{
+  return (qos_record_inline (vm, node, frame, 0, 1));
 }
 
 /* *INDENT-OFF* */
@@ -255,8 +295,39 @@
     .arc_name = "ip6-unicast",
     .node_name = "ip6-qos-record",
 };
+
+VLIB_REGISTER_NODE (l2_ip_qos_record_node, static) = {
+  .function = l2_ip_qos_record,
+  .name = "l2-ip-qos-record",
+  .vector_size = sizeof (u32),
+  .format_trace = format_qos_record_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = 0,
+  .n_next_nodes = 1,
+
+  /* Consider adding error "no IP after L2, no recording" */
+  .next_nodes = {
+    [0] = "error-drop",
+  },
+};
+
+VLIB_NODE_FUNCTION_MULTIARCH (l2_ip_qos_record_node, l2_ip_qos_record);
 /* *INDENT-ON* */
 
+clib_error_t *
+l2_ip_qos_init (vlib_main_t * vm)
+{
+  /* Initialize the feature next-node indexes */
+  feat_bitmap_init_next_nodes (vm,
+			       l2_ip_qos_record_node.index,
+			       L2INPUT_N_FEAT,
+			       l2input_get_feat_names (),
+			       l2_qos_input_next[QOS_SOURCE_IP]);
+  return 0;
+}
+
+VLIB_INIT_FUNCTION (l2_ip_qos_init);
 
 static clib_error_t *
 qos_record_cli (vlib_main_t * vm,