sr: SRv6 Path Tracing Midpoint behaviour

Type: feature

Signed-off-by: Julian Klaiber <julian@klaiber.me>
Change-Id: I866a2d2e06013380309c98a54078c1b3f6ad76fc

3 files changed, 195 insertions, 0 deletions

diff --git a/src/vnet/srv6/sr_pt.c b/src/vnet/srv6/sr_pt.c
index 5d907eaf639..6299faa84ab 100644
--- a/src/vnet/srv6/sr_pt.c
+++ b/src/vnet/srv6/sr_pt.c
@@ -69,6 +69,8 @@ sr_pt_add_iface (u32 iface, u16 id, u8 ingress_load, u8 egress_load,
   if (tts_template > SR_PT_TTS_TEMPLATE_MAX)
     return SR_PT_ERR_TTS_TEMPLATE_INVALID;
 
+  vnet_feature_enable_disable ("ip6-output", "pt", iface, 1, 0, 0);
+
   /* Create a new sr_pt_iface */
   pool_get_zero (sr_pt->sr_pt_iface, ls);
   ls->iface = iface;
@@ -101,6 +103,7 @@ sr_pt_del_iface (u32 iface)
     {
       /* Retrieve sr_pt_iface */
       ls = pool_elt_at_index (sr_pt->sr_pt_iface, p[0]);
+      vnet_feature_enable_disable ("ip6-output", "pt", iface, 0, 0, 0);
       /* Delete sr_pt_iface */
       pool_put (sr_pt->sr_pt_iface, ls);
       mhash_unset (&sr_pt->sr_pt_iface_index_hash, &iface, NULL);
diff --git a/src/vnet/srv6/sr_pt.h b/src/vnet/srv6/sr_pt.h
index 87fdb68a36e..cd70cd09ad2 100644
--- a/src/vnet/srv6/sr_pt.h
+++ b/src/vnet/srv6/sr_pt.h
@@ -11,6 +11,8 @@
 #ifndef included_vnet_sr_pt_h
 #define included_vnet_sr_pt_h
 
+#define IP6_HBH_PT_TYPE 50
+
 /*SR PT error codes*/
 #define SR_PT_ERR_NOENT		       -1 /* No such entry*/
 #define SR_PT_ERR_EXIST		       -2 /* Entry exists */
@@ -37,6 +39,11 @@
 #define SR_PT_TTS_SHIFT_TEMPLATE_2 16
 #define SR_PT_TTS_SHIFT_TEMPLATE_3 20
 
+/*PT node behaviors*/
+#define PT_BEHAVIOR_SRC 0
+#define PT_BEHAVIOR_MID 1
+#define PT_BEHAVIOR_SNK 2
+
 typedef struct
 {
   u32 iface;	   /**< Interface */
@@ -46,6 +53,17 @@ typedef struct
   u8 tts_template; /**< Interface TTS Template */
 } sr_pt_iface_t;
 
+typedef struct
+{
+  u16 oif_oil;
+  u8 tts;
+} __clib_packed sr_pt_cmd_t;
+
+typedef struct
+{
+  sr_pt_cmd_t cmd_stack[12];
+} __clib_packed ip6_hop_by_hop_option_pt_t;
+
 /**
  * @brief SR Path Tracing main datastructure
  */
@@ -60,6 +78,7 @@ typedef struct
 } sr_pt_main_t;
 
 extern sr_pt_main_t sr_pt_main;
+extern vlib_node_registration_t sr_pt_node;
 extern int sr_pt_add_iface (u32 iface, u16 id, u8 ingress_load, u8 egress_load,
 			    u8 tts_template);
 extern int sr_pt_del_iface (u32 iface);
diff --git a/src/vnet/srv6/sr_pt_node.c b/src/vnet/srv6/sr_pt_node.c
new file mode 100644
index 00000000000..392101973d4
--- /dev/null
+++ b/src/vnet/srv6/sr_pt_node.c
@@ -0,0 +1,173 @@
+/* SPDX-License-Identifier: Apache-2.0
+ * Copyright(c) 2022 Cisco Systems, Inc.
+ */
+
+#include <vnet/fib/ip6_fib.h>
+#include <vnet/dpo/load_balance.h>
+#include <vnet/l2/feat_bitmap.h>
+#include <vnet/fib/fib_table.h>
+#include <vnet/srv6/sr.h>
+#include <vnet/srv6/sr_pt.h>
+
+/**
+ * @brief PT node trace
+ */
+typedef struct
+{
+  u32 iface;
+  u16 id;
+  u8 load;
+  timestamp_64_t t64;
+  u8 tts_template;
+  u8 tts;
+  u8 behavior;
+} pt_trace_t;
+
+static u8 *
+format_pt_trace (u8 *s, va_list *args)
+{
+  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
+  CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
+  pt_trace_t *t = va_arg (*args, pt_trace_t *);
+  switch (t->behavior)
+    {
+    case PT_BEHAVIOR_MID:
+      s = format (
+	s,
+	"Behavior Midpoint, outgoing interface %U, outgoing interface id %u, "
+	"outgoing interface load %u, t64_sec %u, t64_nsec %u, tts_template "
+	"%u, tts %u",
+	format_vnet_sw_if_index_name, vnet_get_main (), t->iface, t->id,
+	t->load, htobe32 (t->t64.sec), htobe32 (t->t64.nsec), t->tts_template,
+	t->tts);
+      break;
+    default:
+      break;
+    }
+  return s;
+}
+
+static_always_inline void
+pt_midpoint_processing (vlib_main_t *vm, vlib_node_runtime_t *node,
+			vlib_buffer_t *b0, ip6_header_t *ip0,
+			sr_pt_iface_t *ls, timestamp_64_t t64)
+{
+  ip6_hop_by_hop_header_t *hbh;
+  ip6_hop_by_hop_option_t *hbh_opt;
+  ip6_hop_by_hop_option_pt_t *hbh_opt_pt;
+
+  if (ip0->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
+    {
+      hbh = (void *) (ip0 + 1);
+      hbh_opt = (void *) (hbh + 1);
+      if (hbh_opt->type == IP6_HBH_PT_TYPE)
+	{
+	  hbh_opt_pt = (void *) (hbh_opt + 1);
+	  clib_memcpy_fast (&hbh_opt_pt->cmd_stack[1],
+			    &hbh_opt_pt->cmd_stack[0], 33);
+	  hbh_opt_pt->cmd_stack[0].oif_oil = htobe16 (ls->id << 4);
+	  hbh_opt_pt->cmd_stack[0].oif_oil |= ls->egress_load;
+	  switch (ls->tts_template)
+	    {
+	    case SR_PT_TTS_TEMPLATE_0:
+	      hbh_opt_pt->cmd_stack[0].tts =
+		t64.nsec >> SR_PT_TTS_SHIFT_TEMPLATE_0;
+	      break;
+	    case SR_PT_TTS_TEMPLATE_1:
+	      hbh_opt_pt->cmd_stack[0].tts =
+		t64.nsec >> SR_PT_TTS_SHIFT_TEMPLATE_1;
+	      break;
+	    case SR_PT_TTS_TEMPLATE_2:
+	      hbh_opt_pt->cmd_stack[0].tts =
+		t64.nsec >> SR_PT_TTS_SHIFT_TEMPLATE_2;
+	      break;
+	    case SR_PT_TTS_TEMPLATE_3:
+	      hbh_opt_pt->cmd_stack[0].tts =
+		t64.nsec >> SR_PT_TTS_SHIFT_TEMPLATE_0;
+	      break;
+	    default:
+	      break;
+	    }
+	}
+    }
+  return;
+}
+
+VLIB_NODE_FN (sr_pt_node)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *from_frame)
+{
+  u32 n_left_from, next_index, *from, *to_next;
+  from = vlib_frame_vector_args (from_frame);
+  n_left_from = from_frame->n_vectors;
+  next_index = node->cached_next_index;
+  u8 pt_behavior = ~(u8) 0;
+  sr_pt_iface_t *ls = 0;
+  while (n_left_from > 0)
+    {
+      u32 n_left_to_next;
+      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
+
+      // Single loop for potentially the last three packets
+      while (n_left_from > 0 && n_left_to_next > 0)
+	{
+	  u32 bi0;
+	  u32 iface;
+	  vlib_buffer_t *b0;
+	  u32 next0 = 0;
+	  ethernet_header_t *en0;
+	  ip6_header_t *ip0 = 0;
+	  bi0 = from[0];
+	  to_next[0] = bi0;
+	  from += 1;
+	  to_next += 1;
+	  n_left_from -= 1;
+	  n_left_to_next -= 1;
+	  timestamp_64_t t64;
+
+	  b0 = vlib_get_buffer (vm, bi0);
+	  iface = vnet_buffer (b0)->sw_if_index[VLIB_TX];
+	  ls = sr_pt_find_iface (iface);
+	  if (ls)
+	    {
+	      en0 = vlib_buffer_get_current (b0);
+	      ip0 = (void *) (en0 + 1);
+	      unix_time_now_nsec_fraction (&t64.sec, &t64.nsec);
+	      pt_midpoint_processing (vm, node, b0, ip0, ls, t64);
+	      pt_behavior = PT_BEHAVIOR_MID;
+	    }
+	  if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
+	    {
+	      pt_trace_t *tr = vlib_add_trace (vm, node, b0, sizeof (*tr));
+	      tr->iface = iface;
+	      tr->id = ls->id;
+	      tr->load = ls->egress_load;
+	      tr->tts_template = ls->tts_template;
+	      tr->t64.sec = t64.sec;
+	      tr->t64.nsec = t64.nsec;
+	      tr->tts = t64.nsec >> 20;
+	      tr->behavior = pt_behavior;
+	    }
+	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
+					   n_left_to_next, bi0, next0);
+	}
+      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
+    }
+
+  return from_frame->n_vectors;
+}
+
+VLIB_REGISTER_NODE (sr_pt_node) = {
+  .name = "pt",
+  .vector_size = sizeof (u32),
+  .format_trace = format_pt_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+  .n_errors = 0,
+  .n_next_nodes = 1,
+  .next_nodes = { [0] = "interface-output" },
+};
+
+VNET_FEATURE_INIT (sr_pt_node, static) = {
+  .arc_name = "ip6-output",
+  .node_name = "pt",
+  .runs_after = VNET_FEATURES ("ip6-lookup"),
+};
\ No newline at end of file