1 files changed, 149 insertions, 32 deletions

diff --git a/src/vnet/tcp/tcp_pg.c b/src/vnet/tcp/tcp_pg.c
index 07bdb113fd0..9b98e3d8ee4 100644
--- a/src/vnet/tcp/tcp_pg.c
+++ b/src/vnet/tcp/tcp_pg.c
@@ -51,6 +51,13 @@
   _ (ECE)					\
   _ (CWR)
 
+#define foreach_tcp_options                                                   \
+  _ (mss, TCP_OPTION_MSS, TCP_OPTION_LEN_MSS, 1)                              \
+  _ (timestamp, TCP_OPTION_TIMESTAMP, TCP_OPTION_LEN_TIMESTAMP, 2)            \
+  _ (winscale, TCP_OPTION_WINDOW_SCALE, TCP_OPTION_LEN_WINDOW_SCALE, 1)       \
+  _ (sackperm, TCP_OPTION_SACK_PERMITTED, TCP_OPTION_LEN_SACK_PERMITTED, 0)   \
+  _ (sack, TCP_OPTION_SACK_BLOCK, TCP_OPTION_LEN_SACK_BLOCK, 0)
+
 static void
 tcp_pg_edit_function (pg_main_t * pg,
 		      pg_stream_t * s,
@@ -150,82 +157,192 @@ uword
 unformat_pg_tcp_header (unformat_input_t * input, va_list * args)
 {
   pg_stream_t *s = va_arg (*args, pg_stream_t *);
-  pg_tcp_header_t *p;
-  u32 group_index;
+  pg_tcp_header_t *pth;
+  u32 header_group_index, opt_group_index = ~0, noop_len, opts_len = 0;
 
-  p = pg_create_edit_group (s, sizeof (p[0]), sizeof (tcp_header_t),
-			    &group_index);
-  pg_tcp_header_init (p);
+  pth = pg_create_edit_group (s, sizeof (pth[0]), sizeof (tcp_header_t),
+			      &header_group_index);
+  pg_tcp_header_init (pth);
 
   /* Defaults. */
-  pg_edit_set_fixed (&p->seq_number, 0);
-  pg_edit_set_fixed (&p->ack_number, 0);
-
-  pg_edit_set_fixed (&p->data_offset_and_reserved,
-		     sizeof (tcp_header_t) / sizeof (u32));
+  pg_edit_set_fixed (&pth->seq_number, 0);
+  pg_edit_set_fixed (&pth->ack_number, 0);
 
-  pg_edit_set_fixed (&p->window, 4096);
-  pg_edit_set_fixed (&p->urgent_pointer, 0);
+  pg_edit_set_fixed (&pth->window, 4096);
+  pg_edit_set_fixed (&pth->urgent_pointer, 0);
 
-#define _(f) pg_edit_set_fixed (&p->f##_flag, 0);
+#define _(f) pg_edit_set_fixed (&pth->f##_flag, 0);
   foreach_tcp_flag
 #undef _
-    p->checksum.type = PG_EDIT_UNSPECIFIED;
+    pth->checksum.type = PG_EDIT_UNSPECIFIED;
 
-  if (!unformat (input, "TCP: %U -> %U",
-		 unformat_pg_edit,
-		 unformat_tcp_udp_port, &p->src,
-		 unformat_pg_edit, unformat_tcp_udp_port, &p->dst))
+  if (!unformat (input, "TCP: %U -> %U", unformat_pg_edit,
+		 unformat_tcp_udp_port, &pth->src, unformat_pg_edit,
+		 unformat_tcp_udp_port, &pth->dst))
     goto error;
 
   /* Parse options. */
   while (1)
     {
-      if (unformat (input, "window %U",
-		    unformat_pg_edit, unformat_pg_number, &p->window))
+      if (unformat (input, "window %U", unformat_pg_edit, unformat_pg_number,
+		    &pth->window))
 	;
 
-      else if (unformat (input, "checksum %U",
-			 unformat_pg_edit, unformat_pg_number, &p->checksum))
+      else if (unformat (input, "checksum %U", unformat_pg_edit,
+			 unformat_pg_number, &pth->checksum))
 	;
 
       else if (unformat (input, "seqnum %U", unformat_pg_edit,
-			 unformat_pg_number, &p->seq_number))
+			 unformat_pg_number, &pth->seq_number))
 	;
       else if (unformat (input, "acknum %U", unformat_pg_edit,
-			 unformat_pg_number, &p->ack_number))
+			 unformat_pg_number, &pth->ack_number))
 	;
       /* Flags. */
-#define _(f) else if (unformat (input, #f)) pg_edit_set_fixed (&p->f##_flag, 1);
+#define _(f)                                                                  \
+  else if (unformat (input, #f)) pg_edit_set_fixed (&pth->f##_flag, 1);
       foreach_tcp_flag
 #undef _
-	/* Can't parse input: try next protocol level. */
+	/* Can't parse input: try TCP options and next protocol level. */
+	else break;
+    }
+
+  while (unformat (input, "opt"))
+    {
+      int i;
+      pg_edit_t *opt_header, *opt_values;
+      u8 type, opt_len, n_values;
+
+      /* first allocate a new edit group for options */
+      if (opt_group_index == ~0)
+	(void) pg_create_edit_group (s, 0, 0, &opt_group_index);
+
+      if (false)
+	{
+	}
+#define _(n, t, l, k)                                                         \
+  else if (unformat (input, #n))                                              \
+  {                                                                           \
+    type = (t);                                                               \
+    opt_len = (l);                                                            \
+    n_values = (k);                                                           \
+  }
+      foreach_tcp_options
+#undef _
 	else
+      {
+	/* unknown TCP option */
 	break;
+      }
+
+#define pg_tcp_option_init(e, o, b)                                           \
+  do                                                                          \
+    {                                                                         \
+      *(o) += (b);                                                            \
+      (e)->lsb_bit_offset = *(o) > 0 ? (*(o) -1) * BITS (u8) : 0;             \
+      (e)->n_bits = (b) *BITS (u8);                                           \
+    }                                                                         \
+  while (0);
+
+      /* if we don't know how many values to read, just ask */
+      if (n_values == 0 &&
+	  unformat (input, "nvalues %D", sizeof (n_values), &n_values))
+	{
+	  switch (type)
+	    {
+	    case TCP_OPTION_SACK_BLOCK:
+	      /* each sack block is composed of 2 32-bits values */
+	      n_values *= 2;
+	      /*
+		opt_len contains the length of a single sack block,
+		it needs to be updated to contains the final number of bytes
+		for the sack options
+	      */
+	      opt_len = 2 + 2 * opt_len;
+	      break;
+	    default:
+	      /* unknown variable options */
+	      continue;
+	    }
+	}
+
+      opt_header = pg_add_edits (s, sizeof (pg_edit_t) * (2 + n_values),
+				 opt_len, opt_group_index);
+      pg_tcp_option_init (opt_header, &opts_len, 1);
+      pg_tcp_option_init (opt_header + 1, &opts_len, 1);
+      pg_edit_set_fixed (opt_header, type);
+      pg_edit_set_fixed (opt_header + 1, opt_len);
+      opt_values = opt_header + 2;
+
+      switch (type)
+	{
+	case TCP_OPTION_MSS:
+	  pg_tcp_option_init (opt_values, &opts_len, 2);
+	  break;
+	case TCP_OPTION_WINDOW_SCALE:
+	  pg_tcp_option_init (opt_values, &opts_len, 1);
+	  break;
+	case TCP_OPTION_TIMESTAMP:
+	case TCP_OPTION_SACK_BLOCK:
+	  for (i = 0; i < n_values; ++i)
+	    pg_tcp_option_init (opt_values + i, &opts_len, 4);
+	  break;
+	default:
+	  break;
+	}
+
+      for (i = 0; i < n_values; ++i)
+	{
+	  if (!unformat (input, "%U", unformat_pg_edit, unformat_pg_number,
+			 opt_values + i))
+	    goto error;
+	}
     }
 
+  /* add TCP NO-OP options to fill options up to a 4-bytes boundary */
+  noop_len = (TCP_OPTS_ALIGN - opts_len % TCP_OPTS_ALIGN) % TCP_OPTS_ALIGN;
+  if (noop_len > 0)
+    {
+      pg_edit_t *noop_edit;
+      u8 *noops = 0;
+
+      vec_validate (noops, noop_len - 1);
+      clib_memset (noops, 1, noop_len);
+
+      noop_edit =
+	pg_add_edits (s, sizeof (noop_edit[0]), noop_len, opt_group_index);
+      pg_tcp_option_init (noop_edit, &opts_len, noop_len);
+      noop_edit->type = PG_EDIT_FIXED;
+      noop_edit->values[PG_EDIT_LO] = noops;
+    }
+#undef pg_tcp_option_init
+
+  /* set the data offset according to options */
+  pg_edit_set_fixed (&pth->data_offset_and_reserved,
+		     (sizeof (tcp_header_t) + opts_len) / sizeof (u32));
+
   {
     ip_main_t *im = &ip_main;
     u16 dst_port;
     tcp_udp_port_info_t *pi;
 
     pi = 0;
-    if (p->dst.type == PG_EDIT_FIXED)
+    if (pth->dst.type == PG_EDIT_FIXED)
       {
-	dst_port = pg_edit_get_value (&p->dst, PG_EDIT_LO);
+	dst_port = pg_edit_get_value (&pth->dst, PG_EDIT_LO);
 	pi = ip_get_tcp_udp_port_info (im, dst_port);
       }
 
-    if (pi && pi->unformat_pg_edit
-	&& unformat_user (input, pi->unformat_pg_edit, s))
+    if (pi && pi->unformat_pg_edit &&
+	unformat_user (input, pi->unformat_pg_edit, s))
       ;
 
     else if (!unformat_user (input, unformat_pg_payload, s))
       goto error;
 
-    if (p->checksum.type == PG_EDIT_UNSPECIFIED)
+    if (pth->checksum.type == PG_EDIT_UNSPECIFIED)
       {
-	pg_edit_group_t *g = pg_stream_get_group (s, group_index);
+	pg_edit_group_t *g = pg_stream_get_group (s, header_group_index);
 	g->edit_function = tcp_pg_edit_function;
 	g->edit_function_opaque = 0;
       }