4 files changed, 103 insertions, 27 deletions

diff --git a/src/vnet/ipsec/ah.h b/src/vnet/ipsec/ah.h
index 37fc29a1ced..f74ad9b01c1 100644
--- a/src/vnet/ipsec/ah.h
+++ b/src/vnet/ipsec/ah.h
@@ -49,6 +49,15 @@ typedef CLIB_PACKED (struct {
 }) ip6_and_ah_header_t;
 /* *INDENT-ON* */
 
+always_inline u8
+ah_calc_icv_padding_len (u8 icv_size, int is_ipv6)
+{
+  ASSERT (0 == is_ipv6 || 1 == is_ipv6);
+  const u8 req_multiple = 4 + 4 * is_ipv6;	// 4 for ipv4, 8 for ipv6
+  const u8 total_size = sizeof (ah_header_t) + icv_size;
+  return (req_multiple - total_size % req_multiple) % req_multiple;
+}
+
 #endif /* __AH_H__ */
 
 /*
diff --git a/src/vnet/ipsec/ah_decrypt.c b/src/vnet/ipsec/ah_decrypt.c
index c487d82e34a..abe2e6f5f80 100644
--- a/src/vnet/ipsec/ah_decrypt.c
+++ b/src/vnet/ipsec/ah_decrypt.c
@@ -112,6 +112,10 @@ ah_decrypt_node_fn (vlib_main_t * vm,
 	  u8 ip_hdr_size = 0;
 	  u8 tos = 0;
 	  u8 ttl = 0;
+	  u32 ip_version_traffic_class_and_flow_label = 0;
+	  u8 hop_limit = 0;
+	  u8 nexthdr = 0;
+	  u8 icv_padding_len = 0;
 
 
 	  i_bi0 = from[0];
@@ -125,12 +129,29 @@ ah_decrypt_node_fn (vlib_main_t * vm,
 	  to_next[0] = i_bi0;
 	  to_next += 1;
 	  ih4 = vlib_buffer_get_current (i_b0);
-	  ip_hdr_size = ip4_header_bytes (ih4);
-	  ah0 = (ah_header_t *) ((u8 *) ih4 + ip_hdr_size);
-
+	  ih6 = vlib_buffer_get_current (i_b0);
 	  sa_index0 = vnet_buffer (i_b0)->ipsec.sad_index;
 	  sa0 = pool_elt_at_index (im->sad, sa_index0);
 
+	  if ((ih4->ip_version_and_header_length & 0xF0) == 0x40)
+	    {
+	      ip_hdr_size = ip4_header_bytes (ih4);
+	      ah0 = (ah_header_t *) ((u8 *) ih4 + ip_hdr_size);
+	    }
+	  else if ((ih4->ip_version_and_header_length & 0xF0) == 0x60)
+	    {
+	      ip6_ext_header_t *prev = NULL;
+	      ip6_ext_header_find_t (ih6, prev, ah0, IP_PROTOCOL_IPSEC_AH);
+	      ip_hdr_size = sizeof (ip6_header_t);
+	      ASSERT ((u8 *) ah0 - (u8 *) ih6 == ip_hdr_size);
+	    }
+	  else
+	    {
+	      vlib_node_increment_counter (vm, ah_decrypt_node.index,
+					   AH_DECRYPT_ERROR_NOT_IP, 1);
+	      goto trace;
+	    }
+
 	  seq = clib_host_to_net_u32 (ah0->seq_no);
 	  /* anti-replay check */
 	  //TODO UT remaining
@@ -164,9 +185,7 @@ ah_decrypt_node_fn (vlib_main_t * vm,
 	      u8 digest[64];
 	      memset (sig, 0, sizeof (sig));
 	      memset (digest, 0, sizeof (digest));
-	      u8 *icv =
-		vlib_buffer_get_current (i_b0) + ip_hdr_size +
-		sizeof (ah_header_t);
+	      u8 *icv = ah0->auth_data;
 	      memcpy (digest, icv, icv_size);
 	      memset (icv, 0, icv_size);
 
@@ -178,7 +197,20 @@ ah_decrypt_node_fn (vlib_main_t * vm,
 		  ih4->ttl = 0;
 		  ih4->checksum = 0;
 		  ih4->flags_and_fragment_offset = 0;
-		}		//TODO else part for IPv6
+		  icv_padding_len =
+		    ah_calc_icv_padding_len (icv_size, 0 /* is_ipv6 */ );
+		}
+	      else
+		{
+		  ip_version_traffic_class_and_flow_label =
+		    ih6->ip_version_traffic_class_and_flow_label;
+		  hop_limit = ih6->hop_limit;
+		  ih6->ip_version_traffic_class_and_flow_label = 0x60;
+		  ih6->hop_limit = 0;
+		  nexthdr = ah0->nexthdr;
+		  icv_padding_len =
+		    ah_calc_icv_padding_len (icv_size, 1 /* is_ipv6 */ );
+		}
 	      hmac_calc (sa0->integ_alg, sa0->integ_key, sa0->integ_key_len,
 			 (u8 *) ih4, i_b0->current_length, sig, sa0->use_esn,
 			 sa0->seq_hi);
@@ -204,9 +236,9 @@ ah_decrypt_node_fn (vlib_main_t * vm,
 
 	    }
 
-
 	  vlib_buffer_advance (i_b0,
-			       ip_hdr_size + sizeof (ah_header_t) + icv_size);
+			       ip_hdr_size + sizeof (ah_header_t) + icv_size +
+			       icv_padding_len);
 	  i_b0->flags |= VLIB_BUFFER_TOTAL_LENGTH_VALID;
 
 	  /* transport mode */
@@ -251,15 +283,15 @@ ah_decrypt_node_fn (vlib_main_t * vm,
 	    {
 	      if (PREDICT_FALSE (transport_ip6))
 		{
-		  ih6 =
-		    (ip6_header_t *) (i_b0->data +
-				      sizeof (ethernet_header_t));
 		  vlib_buffer_advance (i_b0, -sizeof (ip6_header_t));
 		  oh6 = vlib_buffer_get_current (i_b0);
 		  memmove (oh6, ih6, sizeof (ip6_header_t));
 
 		  next0 = AH_DECRYPT_NEXT_IP6_INPUT;
-		  oh6->protocol = ah0->nexthdr;
+		  oh6->protocol = nexthdr;
+		  oh6->hop_limit = hop_limit;
+		  oh6->ip_version_traffic_class_and_flow_label =
+		    ip_version_traffic_class_and_flow_label;
 		  oh6->payload_length =
 		    clib_host_to_net_u16 (vlib_buffer_length_in_chain
 					  (vm, i_b0) - sizeof (ip6_header_t));
diff --git a/src/vnet/ipsec/ah_encrypt.c b/src/vnet/ipsec/ah_encrypt.c
index 898c0f27547..911dd333882 100644
--- a/src/vnet/ipsec/ah_encrypt.c
+++ b/src/vnet/ipsec/ah_encrypt.c
@@ -110,6 +110,8 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 	  u8 transport_mode = 0;
 	  u8 tos = 0;
 	  u8 ttl = 0;
+	  u8 hop_limit = 0;
+	  u32 ip_version_traffic_class_and_flow_label = 0;
 
 	  i_bi0 = from[0];
 	  from += 1;
@@ -159,6 +161,8 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 
 	  icv_size =
 	    em->ipsec_proto_main_integ_algs[sa0->integ_alg].trunc_size;
+	  const u8 padding_len = ah_calc_icv_padding_len (icv_size, is_ipv6);
+	  adv -= padding_len;
 	  /*transport mode save the eth header before it is overwritten */
 	  if (PREDICT_FALSE (!sa0->is_tunnel))
 	    {
@@ -172,18 +176,18 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 
 	  vlib_buffer_advance (i_b0, adv - icv_size);
 
-	  /* is ipv6 */
 	  if (PREDICT_FALSE (is_ipv6))
-	    {
+	    {			/* is ipv6 */
 	      ih6_0 = (ip6_and_ah_header_t *) ih0;
 	      ip_hdr_size = sizeof (ip6_header_t);
 	      oh6_0 = vlib_buffer_get_current (i_b0);
 
+	      hop_limit = ih6_0->ip6.hop_limit;
+	      ip_version_traffic_class_and_flow_label =
+		ih6_0->ip6.ip_version_traffic_class_and_flow_label;
 	      if (PREDICT_TRUE (sa0->is_tunnel))
 		{
 		  next_hdr_type = IP_PROTOCOL_IPV6;
-		  oh6_0->ip6.ip_version_traffic_class_and_flow_label =
-		    ih6_0->ip6.ip_version_traffic_class_and_flow_label;
 		}
 	      else
 		{
@@ -192,12 +196,17 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 		}
 
 	      oh6_0->ip6.protocol = IP_PROTOCOL_IPSEC_AH;
-	      oh6_0->ip6.hop_limit = 254;
+	      oh6_0->ip6.hop_limit = 0;
+	      oh6_0->ip6.ip_version_traffic_class_and_flow_label = 0x60;
+	      oh6_0->ah.reserved = 0;
+	      oh6_0->ah.nexthdr = next_hdr_type;
 	      oh6_0->ah.spi = clib_net_to_host_u32 (sa0->spi);
 	      oh6_0->ah.seq_no = clib_net_to_host_u32 (sa0->seq);
 	      oh6_0->ip6.payload_length =
 		clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, i_b0) -
 				      sizeof (ip6_header_t));
+	      oh6_0->ah.hdrlen =
+		(sizeof (ah_header_t) + icv_size + padding_len) / 4 - 2;
 	    }
 	  else
 	    {
@@ -227,11 +236,11 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 	      oh0->ah.seq_no = clib_net_to_host_u32 (sa0->seq);
 	      oh0->ip4.checksum = 0;
 	      oh0->ah.nexthdr = next_hdr_type;
-	      oh0->ah.hdrlen = 4;
+	      oh0->ah.hdrlen =
+		(sizeof (ah_header_t) + icv_size + padding_len) / 4 - 2;
 	    }
 
 
-
 	  if (PREDICT_TRUE
 	      (!is_ipv6 && sa0->is_tunnel && !sa0->is_tunnel_ip6))
 	    {
@@ -264,7 +273,8 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 	  u8 sig[64];
 	  memset (sig, 0, sizeof (sig));
 	  u8 *digest =
-	    vlib_buffer_get_current (i_b0) + ip_hdr_size + icv_size;
+	    vlib_buffer_get_current (i_b0) + ip_hdr_size +
+	    sizeof (ah_header_t);
 	  memset (digest, 0, icv_size);
 
 	  unsigned size = hmac_calc (sa0->integ_alg, sa0->integ_key,
@@ -276,6 +286,9 @@ ah_encrypt_node_fn (vlib_main_t * vm,
 	  memcpy (digest, sig, size);
 	  if (PREDICT_FALSE (is_ipv6))
 	    {
+	      oh6_0->ip6.hop_limit = hop_limit;
+	      oh6_0->ip6.ip_version_traffic_class_and_flow_label =
+		ip_version_traffic_class_and_flow_label;
 	    }
 	  else
 	    {
diff --git a/src/vnet/ipsec/ipsec_input.c b/src/vnet/ipsec/ipsec_input.c
index b7bb07bf051..ebfb909cbcf 100644
--- a/src/vnet/ipsec/ipsec_input.c
+++ b/src/vnet/ipsec/ipsec_input.c
@@ -348,13 +348,13 @@ VLIB_REGISTER_NODE (ipsec_input_ip4_node,static) = {
 };
 /* *INDENT-ON* */
 
-VLIB_NODE_FUNCTION_MULTIARCH (ipsec_input_ip4_node, ipsec_input_ip4_node_fn)
-     static vlib_node_registration_t ipsec_input_ip6_node;
+VLIB_NODE_FUNCTION_MULTIARCH (ipsec_input_ip4_node, ipsec_input_ip4_node_fn);
+static vlib_node_registration_t ipsec_input_ip6_node;
 
-     static uword
-       ipsec_input_ip6_node_fn (vlib_main_t * vm,
-				vlib_node_runtime_t * node,
-				vlib_frame_t * from_frame)
+static uword
+ipsec_input_ip6_node_fn (vlib_main_t * vm,
+			 vlib_node_runtime_t * node,
+			 vlib_frame_t * from_frame)
 {
   u32 n_left_from, *from, next_index, *to_next;
   ipsec_main_t *im = &ipsec_main;
@@ -379,6 +379,7 @@ VLIB_NODE_FUNCTION_MULTIARCH (ipsec_input_ip4_node, ipsec_input_ip4_node_fn)
 	  ip4_ipsec_config_t *c0;
 	  ipsec_spd_t *spd0;
 	  ipsec_policy_t *p0 = 0;
+	  ah_header_t *ah0;
 	  u32 header_size = sizeof (ip0[0]);
 
 	  bi0 = to_next[0] = from[0];
@@ -396,6 +397,7 @@ VLIB_NODE_FUNCTION_MULTIARCH (ipsec_input_ip4_node, ipsec_input_ip4_node_fn)
 
 	  ip0 = vlib_buffer_get_current (b0);
 	  esp0 = (esp_header_t *) ((u8 *) ip0 + header_size);
+	  ah0 = (ah_header_t *) ((u8 *) ip0 + header_size);
 
 	  if (PREDICT_TRUE (ip0->protocol == IP_PROTOCOL_IPSEC_ESP))
 	    {
@@ -426,6 +428,26 @@ VLIB_NODE_FUNCTION_MULTIARCH (ipsec_input_ip4_node, ipsec_input_ip4_node_fn)
 		  goto trace0;
 		}
 	    }
+	  else if (ip0->protocol == IP_PROTOCOL_IPSEC_AH)
+	    {
+	      p0 = ipsec_input_ip6_protect_policy_match (spd0,
+							 &ip0->src_address,
+							 &ip0->dst_address,
+							 clib_net_to_host_u32
+							 (ah0->spi));
+
+	      if (PREDICT_TRUE (p0 != 0))
+		{
+		  p0->counter.packets++;
+		  p0->counter.bytes +=
+		    clib_net_to_host_u16 (ip0->payload_length);
+		  p0->counter.bytes += header_size;
+		  vnet_buffer (b0)->ipsec.sad_index = p0->sa_index;
+		  vnet_buffer (b0)->ipsec.flags = 0;
+		  next0 = im->ah_decrypt_next_index;
+		  goto trace0;
+		}
+	    }
 
 	trace0:
 	  if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))