2 files changed, 210 insertions, 58 deletions

diff --git a/src/vnet/tcp/tcp_format.c b/src/vnet/tcp/tcp_format.c
index a3245f2046a..751042ce1cd 100644
--- a/src/vnet/tcp/tcp_format.c
+++ b/src/vnet/tcp/tcp_format.c
@@ -52,12 +52,68 @@ format_tcp_flags (u8 * s, va_list * args)
     return s;
 }
 
+u8 *
+format_tcp_options (u8 *s, va_list *args)
+{
+  tcp_options_t *opts = va_arg (*args, tcp_options_t *);
+  u32 indent, n_opts = 0;
+  int i;
+
+  if (!opts->flags)
+    return s;
+
+  indent = format_get_indent (s);
+  indent += 2;
+
+  s = format (s, "options:\n%U", format_white_space, indent);
+
+  if (tcp_opts_mss (opts))
+    {
+      s = format (s, "mss %d", opts->mss);
+      n_opts++;
+    }
+  if (tcp_opts_wscale (opts))
+    {
+      s = format (s, "%swindow scale %d", n_opts > 0 ? ", " : "",
+		  format_white_space, indent, opts->wscale);
+      n_opts++;
+    }
+  if (tcp_opts_tstamp (opts))
+    {
+      s = format (s, "%stimestamp %d, echo/reflected timestamp",
+		  n_opts > 0 ? ", " : "", format_white_space, indent,
+		  opts->tsval, opts->tsecr);
+      n_opts++;
+    }
+  if (tcp_opts_sack_permitted (opts))
+    {
+      s = format (s, "%ssack permitted", n_opts > 0 ? ", " : "",
+		  format_white_space, indent);
+      n_opts++;
+    }
+  if (tcp_opts_sack (opts))
+    {
+      s = format (s, "%ssacks:", n_opts > 0 ? ", " : "", format_white_space,
+		  indent);
+      for (i = 0; i < opts->n_sack_blocks; ++i)
+	{
+	  s = format (s, "\n%Ublock %d: start %d, end %d", format_white_space,
+		      indent + 2, i + 1, opts->sacks[i].start,
+		      opts->sacks[i].end);
+	}
+      n_opts++;
+    }
+
+  return s;
+}
+
 /* Format TCP header. */
 u8 *
 format_tcp_header (u8 * s, va_list * args)
 {
   tcp_header_t *tcp = va_arg (*args, tcp_header_t *);
   u32 max_header_bytes = va_arg (*args, u32);
+  tcp_options_t opts = { .flags = 0 };
   u32 header_bytes;
   u32 indent;
 
@@ -83,32 +139,11 @@ format_tcp_header (u8 * s, va_list * args)
 	      clib_net_to_host_u16 (tcp->window),
 	      clib_net_to_host_u16 (tcp->checksum));
 
-
-#if 0
-  /* Format TCP options. */
-  {
-    u8 *o;
-    u8 *option_start = (void *) (tcp + 1);
-    u8 *option_end = (void *) tcp + header_bytes;
-
-    for (o = option_start; o < option_end;)
-      {
-	u32 length = o[1];
-	switch (o[0])
-	  {
-	  case TCP_OPTION_END:
-	    length = 1;
-	    o = option_end;
-	    break;
-
-	  case TCP_OPTION_NOOP:
-	    length = 1;
-	    break;
-
-	  }
-      }
-  }
-#endif
+  if (tcp_options_parse (tcp, &opts, tcp_is_syn (tcp)) < 0)
+    s = format (s, "\n%Uoptions: parsing failed", format_white_space, indent);
+  else
+    s = format (s, "\n%U%U", format_white_space, indent, format_tcp_options,
+		&opts);
 
   /* Recurse into next protocol layer. */
   if (max_header_bytes != 0 && header_bytes < max_header_bytes)
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;
       }