ip: source address selection

Implement a simple source address selection algorithm
for IPv4 and IPv6.
IPv6 does not yet implement RFC6724 but supports link-locals.
ping now chooses correct source address for link-local destination.
Added ping support for link-local multicast (e.g. allnodes).

Type: feature
Signed-off-by: Ole Troan <ot@cisco.com>
Change-Id: I1a3382c1f7d4ace0386c2c19e4e47b045b73a3ed
Signed-off-by: Ole Troan <ot@cisco.com>

7 files changed, 310 insertions, 201 deletions

diff --git a/src/plugins/dns/dns.c b/src/plugins/dns/dns.c
index 0801681b8b3..76ce3dabd30 100644
--- a/src/plugins/dns/dns.c
+++ b/src/plugins/dns/dns.c
@@ -16,9 +16,8 @@
 #include <vnet/vnet.h>
 #include <vnet/udp/udp_local.h>
 #include <vnet/plugin/plugin.h>
-#include <vnet/fib/fib_table.h>
 #include <dns/dns.h>
-
+#include <vnet/ip/ip_sas.h>
 #include <vlibapi/api.h>
 #include <vlibmemory/api.h>
 #include <vpp/app/version.h>
@@ -225,66 +224,16 @@ vnet_dns_send_dns4_request (vlib_main_t * vm, dns_main_t * dm,
   u32 bi;
   vlib_buffer_t *b;
   ip4_header_t *ip;
-  fib_prefix_t prefix;
-  fib_node_index_t fei;
-  u32 sw_if_index, fib_index;
   udp_header_t *udp;
-  ip4_main_t *im4 = &ip4_main;
-  ip_lookup_main_t *lm4 = &im4->lookup_main;
-  ip_interface_address_t *ia = 0;
-  ip4_address_t *src_address;
+  ip4_address_t src_address;
   u8 *dns_request;
   vlib_frame_t *f;
   u32 *to_next;
 
   ASSERT (ep->dns_request);
 
-  /* Find a FIB path to the server */
-  clib_memcpy (&prefix.fp_addr.ip4, server, sizeof (*server));
-  prefix.fp_proto = FIB_PROTOCOL_IP4;
-  prefix.fp_len = 32;
-
-  fib_index = fib_table_find (prefix.fp_proto, 0 /* default VRF for now */ );
-  if (fib_index == (u32) ~ 0)
-    {
-      if (0)
-	clib_warning ("no fib table");
-      return;
-    }
-
-  fei = fib_table_lookup (fib_index, &prefix);
-
-  /* Couldn't find route to destination. Bail out. */
-  if (fei == FIB_NODE_INDEX_INVALID)
-    {
-      if (0)
-	clib_warning ("no route to DNS server");
-      return;
-    }
-
-  sw_if_index = fib_entry_get_resolving_interface (fei);
-
-  if (sw_if_index == ~0)
-    {
-      if (0)
-	clib_warning
-	  ("route to %U exists, fei %d, get_resolving_interface returned"
-	   " ~0", format_ip4_address, &prefix.fp_addr, fei);
-      return;
-    }
-
-  /* *INDENT-OFF* */
-  foreach_ip_interface_address(lm4, ia, sw_if_index, 1 /* honor unnumbered */,
-  ({
-    src_address = ip_interface_address_get_address (lm4, ia);
-    goto found_src_address;
-  }));
-  /* *INDENT-ON* */
-
-  clib_warning ("FIB BUG");
-  return;
-
-found_src_address:
+  if (!ip4_sas (0 /* default VRF for now */, ~0, server, &src_address))
+    return;
 
   /* Go get a buffer */
   if (vlib_buffer_alloc (vm, &bi, 1) != 1)
@@ -311,7 +260,7 @@ found_src_address:
   ip->length = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b));
   ip->ttl = 255;
   ip->protocol = IP_PROTOCOL_UDP;
-  ip->src_address.as_u32 = src_address->as_u32;
+  ip->src_address.as_u32 = src_address.as_u32;
   ip->dst_address.as_u32 = server->as_u32;
   ip->checksum = ip4_header_checksum (ip);
 
@@ -343,14 +292,8 @@ vnet_dns_send_dns6_request (vlib_main_t * vm, dns_main_t * dm,
   u32 bi;
   vlib_buffer_t *b;
   ip6_header_t *ip;
-  fib_prefix_t prefix;
-  fib_node_index_t fei;
-  u32 sw_if_index, fib_index;
   udp_header_t *udp;
-  ip6_main_t *im6 = &ip6_main;
-  ip_lookup_main_t *lm6 = &im6->lookup_main;
-  ip_interface_address_t *ia = 0;
-  ip6_address_t *src_address;
+  ip6_address_t src_address;
   u8 *dns_request;
   vlib_frame_t *f;
   u32 *to_next;
@@ -358,41 +301,8 @@ vnet_dns_send_dns6_request (vlib_main_t * vm, dns_main_t * dm,
 
   ASSERT (ep->dns_request);
 
-  /* Find a FIB path to the server */
-  clib_memcpy (&prefix.fp_addr, server, sizeof (*server));
-  prefix.fp_proto = FIB_PROTOCOL_IP6;
-  prefix.fp_len = 32;
-
-  fib_index = fib_table_find (prefix.fp_proto, 0 /* default VRF for now */ );
-  if (fib_index == (u32) ~ 0)
-    {
-      if (0)
-	clib_warning ("no fib table");
-      return;
-    }
-
-  fei = fib_table_lookup (fib_index, &prefix);
-
-  /* Couldn't find route to destination. Bail out. */
-  if (fei == FIB_NODE_INDEX_INVALID)
-    {
-      clib_warning ("no route to DNS server");
-    }
-
-  sw_if_index = fib_entry_get_resolving_interface (fei);
-
-  /* *INDENT-OFF* */
-  foreach_ip_interface_address(lm6, ia, sw_if_index, 1 /* honor unnumbered */,
-  ({
-    src_address = ip_interface_address_get_address (lm6, ia);
-    goto found_src_address;
-  }));
-  /* *INDENT-ON* */
-
-  clib_warning ("FIB BUG");
-  return;
-
-found_src_address:
+  if (!ip6_sas (0 /* default VRF for now */, ~0, server, &src_address))
+    return;
 
   /* Go get a buffer */
   if (vlib_buffer_alloc (vm, &bi, 1) != 1)
@@ -421,7 +331,7 @@ found_src_address:
 			  - sizeof (ip6_header_t));
   ip->hop_limit = 255;
   ip->protocol = IP_PROTOCOL_UDP;
-  clib_memcpy (&ip->src_address, src_address, sizeof (ip6_address_t));
+  ip6_address_copy (&ip->src_address, &src_address);
   clib_memcpy (&ip->dst_address, server, sizeof (ip6_address_t));
 
   /* UDP header */
@@ -2749,13 +2659,7 @@ vnet_send_dns4_reply (vlib_main_t * vm, dns_main_t * dm,
 		      vlib_buffer_t * b0)
 {
   u32 bi = 0;
-  fib_prefix_t prefix;
-  fib_node_index_t fei;
-  u32 sw_if_index, fib_index;
-  ip4_main_t *im4 = &ip4_main;
-  ip_lookup_main_t *lm4 = &im4->lookup_main;
-  ip_interface_address_t *ia = 0;
-  ip4_address_t *src_address;
+  ip4_address_t src_address;
   ip4_header_t *ip;
   udp_header_t *udp;
   dns_header_t *dh;
@@ -2839,50 +2743,9 @@ vnet_send_dns4_reply (vlib_main_t * vm, dns_main_t * dm,
   vnet_buffer (b0)->sw_if_index[VLIB_RX] = 0;	/* "local0" */
   vnet_buffer (b0)->sw_if_index[VLIB_TX] = 0;	/* default VRF for now */
 
-  /* Find a FIB path to the peer we're trying to answer */
-  clib_memcpy (&prefix.fp_addr.ip4, pr->dst_address, sizeof (ip4_address_t));
-  prefix.fp_proto = FIB_PROTOCOL_IP4;
-  prefix.fp_len = 32;
-
-  fib_index = fib_table_find (prefix.fp_proto, 0 /* default VRF for now */ );
-  if (fib_index == (u32) ~ 0)
-    {
-      clib_warning ("no fib table");
-      return;
-    }
-
-  fei = fib_table_lookup (fib_index, &prefix);
-
-  /* Couldn't find route to destination. Bail out. */
-  if (fei == FIB_NODE_INDEX_INVALID)
-    {
-      clib_warning ("no route to DNS server");
-      return;
-    }
-
-  sw_if_index = fib_entry_get_resolving_interface (fei);
-
-  if (sw_if_index == ~0)
-    {
-      clib_warning (
-	"route to %U exists, fei %d, get_resolving_interface returned"
-	" ~0",
-	format_ip4_address, &prefix.fp_addr, fei);
-      return;
-    }
-
-  /* *INDENT-OFF* */
-  foreach_ip_interface_address(lm4, ia, sw_if_index, 1 /* honor unnumbered */,
-  ({
-    src_address = ip_interface_address_get_address (lm4, ia);
-    goto found_src_address;
-  }));
-  /* *INDENT-ON* */
-
-  clib_warning ("FIB BUG");
-  return;
-
-found_src_address:
+  if (!ip4_sas (0 /* default VRF for now */, ~0,
+		(const ip4_address_t *) &pr->dst_address, &src_address))
+    return;
 
   ip = vlib_buffer_get_current (b0);
   udp = (udp_header_t *) (ip + 1);
@@ -2975,7 +2838,7 @@ found_src_address:
   ip->length = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b0));
   ip->ttl = 255;
   ip->protocol = IP_PROTOCOL_UDP;
-  ip->src_address.as_u32 = src_address->as_u32;
+  ip->src_address.as_u32 = src_address.as_u32;
   clib_memcpy (ip->dst_address.as_u8, pr->dst_address,
 	       sizeof (ip4_address_t));
   ip->checksum = ip4_header_checksum (ip);
diff --git a/src/plugins/ping/ping.c b/src/plugins/ping/ping.c
index d09babd0be2..5973b484045 100644
--- a/src/plugins/ping/ping.c
+++ b/src/plugins/ping/ping.c
@@ -19,8 +19,9 @@
 #include <vlib/unix/unix.h>
 #include <vnet/fib/ip6_fib.h>
 #include <vnet/fib/ip4_fib.h>
-#include <vnet/fib/fib_sas.h>
+#include <vnet/ip/ip_sas.h>
 #include <vnet/ip/ip6_link.h>
+#include <vnet/ip/ip6_ll_table.h>
 #include <vnet/plugin/plugin.h>
 #include <vpp/app/version.h>
 
@@ -682,13 +683,16 @@ ip46_get_resolving_interface (u32 fib_index, ip46_address_t * pa46,
 }
 
 static u32
-ip46_fib_table_get_index_for_sw_if_index (u32 sw_if_index, int is_ip6)
+ip46_fib_table_get_index_for_sw_if_index (u32 sw_if_index, int is_ip6,
+					  ip46_address_t *pa46)
 {
-  u32 fib_table_index = is_ip6 ?
-    ip6_fib_table_get_index_for_sw_if_index (sw_if_index) :
-    ip4_fib_table_get_index_for_sw_if_index (sw_if_index);
-  return fib_table_index;
-
+  if (is_ip6)
+    {
+      if (ip6_address_is_link_local_unicast (&pa46->ip6))
+	return ip6_ll_fib_get (sw_if_index);
+      return ip6_fib_table_get_index_for_sw_if_index (sw_if_index);
+    }
+  return ip4_fib_table_get_index_for_sw_if_index (sw_if_index);
 }
 
 
@@ -735,13 +739,15 @@ ip46_set_src_address (u32 sw_if_index, vlib_buffer_t * b0, int is_ip6)
     {
       ip6_header_t *ip6 = vlib_buffer_get_current (b0);
 
-      res = fib_sas6_get (sw_if_index, &ip6->dst_address, &ip6->src_address);
+      res = ip6_sas_by_sw_if_index (sw_if_index, &ip6->dst_address,
+				    &ip6->src_address);
     }
   else
     {
       ip4_header_t *ip4 = vlib_buffer_get_current (b0);
 
-      res = fib_sas4_get (sw_if_index, &ip4->dst_address, &ip4->src_address);
+      res = ip4_sas_by_sw_if_index (sw_if_index, &ip4->dst_address,
+				    &ip4->src_address);
     }
   return res;
 }
@@ -870,12 +876,10 @@ at_most_a_frame (u32 count)
 }
 
 static int
-ip46_enqueue_packet (vlib_main_t * vm, vlib_buffer_t * b0, u32 burst,
-		     int is_ip6)
+ip46_enqueue_packet (vlib_main_t *vm, vlib_buffer_t *b0, u32 burst,
+		     u32 lookup_node_index)
 {
   vlib_frame_t *f = 0;
-  u32 lookup_node_index =
-    is_ip6 ? ip6_lookup_node.index : ip4_lookup_node.index;
   int n_sent = 0;
 
   u16 n_to_send;
@@ -978,7 +982,7 @@ send_ip46_ping (vlib_main_t * vm,
     }
   else
     fib_index =
-      ip46_fib_table_get_index_for_sw_if_index (sw_if_index, is_ip6);
+      ip46_fib_table_get_index_for_sw_if_index (sw_if_index, is_ip6, pa46);
 
   if (~0 == fib_index)
     ERROR_OUT (SEND_PING_NO_TABLE);
@@ -986,7 +990,6 @@ send_ip46_ping (vlib_main_t * vm,
     ERROR_OUT (SEND_PING_NO_INTERFACE);
 
   vnet_buffer (b0)->sw_if_index[VLIB_RX] = sw_if_index;
-  vnet_buffer (b0)->sw_if_index[VLIB_TX] = fib_index;
 
   int l4_header_offset = ip46_fill_l3_header (pa46, b0, is_ip6);
 
@@ -1002,7 +1005,23 @@ send_ip46_ping (vlib_main_t * vm,
 
   ip46_fix_len_and_csum (vm, l4_header_offset, data_len, b0, is_ip6);
 
-  int n_sent = ip46_enqueue_packet (vm, b0, burst, is_ip6);
+  u32 node_index = ip6_lookup_node.index;
+  if (is_ip6)
+    {
+      if (pa46->ip6.as_u32[0] == clib_host_to_net_u32 (0xff020000))
+	{
+	  node_index = ip6_rewrite_mcast_node.index;
+	  vnet_buffer (b0)->sw_if_index[VLIB_RX] = sw_if_index;
+	  vnet_buffer (b0)->sw_if_index[VLIB_TX] = sw_if_index;
+	  vnet_buffer (b0)->ip.adj_index[VLIB_TX] =
+	    ip6_link_get_mcast_adj (sw_if_index);
+	}
+    }
+  else
+    {
+      node_index = ip4_lookup_node.index;
+    }
+  int n_sent = ip46_enqueue_packet (vm, b0, burst, node_index);
   if (n_sent < burst)
     err = SEND_PING_NO_BUFFERS;
 
diff --git a/src/vnet/CMakeLists.txt b/src/vnet/CMakeLists.txt
index 66a4abc3a41..18e162030b0 100644
--- a/src/vnet/CMakeLists.txt
+++ b/src/vnet/CMakeLists.txt
@@ -73,6 +73,7 @@ list(APPEND VNET_HEADERS
   util/refcount.h
   format_fns.h
   ip/ip_format_fns.h
+  ip/ip_sas.h
   ethernet/ethernet_format_fns.h
 )
 
@@ -413,6 +414,7 @@ list(APPEND VNET_SOURCES
   ip/punt.c
   ip/punt_node.c
   ip/vtep.c
+  ip/ip_sas.c
 )
 
 list(APPEND VNET_MULTIARCH_SOURCES
diff --git a/src/vnet/ip/icmp4.c b/src/vnet/ip/icmp4.c
index 0363092d5d5..5f9ffa3b2b7 100644
--- a/src/vnet/ip/icmp4.c
+++ b/src/vnet/ip/icmp4.c
@@ -40,6 +40,7 @@
 #include <vlib/vlib.h>
 #include <vnet/ip/ip.h>
 #include <vnet/pg/pg.h>
+#include <vnet/ip/ip_sas.h>
 
 static char *icmp_error_strings[] = {
 #define _(f,s) s,
@@ -254,8 +255,6 @@ ip4_icmp_error (vlib_main_t * vm,
   u32 *from, *to_next;
   uword n_left_from, n_left_to_next;
   ip4_icmp_error_next_t next_index;
-  ip4_main_t *im = &ip4_main;
-  ip_lookup_main_t *lm = &im->lookup_main;
 
   from = vlib_frame_vector_args (frame);
   n_left_from = frame->n_vectors;
@@ -286,7 +285,7 @@ ip4_icmp_error (vlib_main_t * vm,
 	  vlib_buffer_t *p0, *org_p0;
 	  ip4_header_t *ip0, *out_ip0;
 	  icmp46_header_t *icmp0;
-	  u32 sw_if_index0, if_add_index0;
+	  u32 sw_if_index0;
 	  ip_csum_t sum;
 
 	  org_p0 = vlib_get_buffer (vm, org_pi0);
@@ -323,25 +322,14 @@ ip4_icmp_error (vlib_main_t * vm,
 	  out_ip0->ttl = 0xff;
 	  out_ip0->protocol = IP_PROTOCOL_ICMP;
 	  out_ip0->dst_address = ip0->src_address;
-	  if_add_index0 = ~0;
-	  if (PREDICT_TRUE (vec_len (lm->if_address_pool_index_by_sw_if_index)
-			    > sw_if_index0))
-	    if_add_index0 =
-	      lm->if_address_pool_index_by_sw_if_index[sw_if_index0];
-	  if (PREDICT_TRUE (if_add_index0 != ~0))
-	    {
-	      ip_interface_address_t *if_add =
-		pool_elt_at_index (lm->if_address_pool, if_add_index0);
-	      ip4_address_t *if_ip =
-		ip_interface_address_get_address (lm, if_add);
-	      out_ip0->src_address = *if_ip;
-	    }
-	  else
-	    {
-	      /* interface has no IP4 address - should not happen */
+	  /* Prefer a source address from "offending interface" */
+	  if (!ip4_sas_by_sw_if_index (sw_if_index0, &out_ip0->dst_address,
+				       &out_ip0->src_address))
+	    { /* interface has no IP6 address - should not happen */
 	      next0 = IP4_ICMP_ERROR_NEXT_DROP;
 	      error0 = ICMP4_ERROR_DROP;
 	    }
+
 	  out_ip0->checksum = ip4_header_checksum (out_ip0);
 
 	  /* Fill icmp header fields */
diff --git a/src/vnet/ip/icmp6.c b/src/vnet/ip/icmp6.c
index 4bba430fadc..b6ed3ea0ec9 100644
--- a/src/vnet/ip/icmp6.c
+++ b/src/vnet/ip/icmp6.c
@@ -40,6 +40,7 @@
 #include <vlib/vlib.h>
 #include <vnet/ip/ip.h>
 #include <vnet/pg/pg.h>
+#include <vnet/ip/ip_sas.h>
 
 static u8 *
 format_ip6_icmp_type_and_code (u8 * s, va_list * args)
@@ -475,8 +476,6 @@ ip6_icmp_error (vlib_main_t * vm,
   u32 *from, *to_next;
   uword n_left_from, n_left_to_next;
   ip6_icmp_error_next_t next_index;
-  ip6_main_t *im = &ip6_main;
-  ip_lookup_main_t *lm = &im->lookup_main;
 
   from = vlib_frame_vector_args (frame);
   n_left_from = frame->n_vectors;
@@ -507,7 +506,7 @@ ip6_icmp_error (vlib_main_t * vm,
 	  vlib_buffer_t *p0, *org_p0;
 	  ip6_header_t *ip0, *out_ip0;
 	  icmp46_header_t *icmp0;
-	  u32 sw_if_index0, if_add_index0;
+	  u32 sw_if_index0;
 	  int bogus_length;
 
 	  org_p0 = vlib_get_buffer (vm, org_pi0);
@@ -547,18 +546,10 @@ ip6_icmp_error (vlib_main_t * vm,
 	  out_ip0->protocol = IP_PROTOCOL_ICMP6;
 	  out_ip0->hop_limit = 0xff;
 	  out_ip0->dst_address = ip0->src_address;
-	  if_add_index0 =
-	    lm->if_address_pool_index_by_sw_if_index[sw_if_index0];
-	  if (PREDICT_TRUE (if_add_index0 != ~0))
-	    {
-	      ip_interface_address_t *if_add =
-		pool_elt_at_index (lm->if_address_pool, if_add_index0);
-	      ip6_address_t *if_ip =
-		ip_interface_address_get_address (lm, if_add);
-	      out_ip0->src_address = *if_ip;
-	    }
-	  else			/* interface has no IP6 address - should not happen */
-	    {
+	  /* Prefer a source address from "offending interface" */
+	  if (!ip6_sas_by_sw_if_index (sw_if_index0, &out_ip0->dst_address,
+				       &out_ip0->src_address))
+	    { /* interface has no IP6 address - should not happen */
 	      next0 = IP6_ICMP_ERROR_NEXT_DROP;
 	      error0 = ICMP6_ERROR_DROP;
 	    }
diff --git a/src/vnet/ip/ip_sas.c b/src/vnet/ip/ip_sas.c
new file mode 100644
index 00000000000..7d3632d95ed
--- /dev/null
+++ b/src/vnet/ip/ip_sas.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright (c) 2021 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ip_sas.h"
+#include <vppinfra/types.h>
+#include <vnet/ip/ip_interface.h>
+#include <vnet/fib/fib_table.h>
+#include <vnet/ip/ip6_link.h>
+#include <vppinfra/byte_order.h>
+
+/*
+ * This file implement source address selection for VPP applications
+ * (e.g. ping, DNS, ICMP)
+ * It does not yet implement full fledged RFC6724 SAS.
+ * SAS assumes every IP enabled interface has an address. The algorithm will
+ * not go and hunt for a suitable IP address on other interfaces than the
+ * output interface or the specified preferred sw_if_index.
+ * That means that an interface with just an IPv6 link-local address must also
+ * be configured with an unnumbered configuration pointing to a numbered
+ * interface.
+ */
+
+static int
+ip6_sas_commonlen (const ip6_address_t *a1, const ip6_address_t *a2)
+{
+  u64 fa = clib_net_to_host_u64 (a1->as_u64[0]) ^
+	   clib_net_to_host_u64 (a2->as_u64[0]);
+  if (fa == 0)
+    {
+      u64 la = clib_net_to_host_u64 (a1->as_u64[1]) ^
+	       clib_net_to_host_u64 (a2->as_u64[1]);
+      if (la == 0)
+	return 128;
+      return 64 + __builtin_clzll (la);
+    }
+  else
+    {
+      return __builtin_clzll (fa);
+    }
+}
+
+static int
+ip4_sas_commonlen (const ip4_address_t *a1, const ip4_address_t *a2)
+{
+  u64 a =
+    clib_net_to_host_u32 (a1->as_u32) ^ clib_net_to_host_u32 (a2->as_u32);
+  if (a == 0)
+    return 32;
+  return __builtin_clz (a);
+}
+
+/*
+ * walk all addresses on an interface:
+ *  - prefer a source matching the scope of the destination address.
+ *  - last resort pick the source address with the longest
+ *    common prefix with destination
+ * NOTE: This should at some point implement RFC6724.
+ */
+bool
+ip6_sas_by_sw_if_index (u32 sw_if_index, const ip6_address_t *dst,
+			ip6_address_t *src)
+{
+  ip_interface_address_t *ia = 0;
+  ip_lookup_main_t *lm6 = &ip6_main.lookup_main;
+  ip6_address_t *tmp, *bestsrc = 0;
+  int bestlen = 0, l;
+
+  if (ip6_address_is_link_local_unicast (dst) ||
+      dst->as_u32[0] == clib_host_to_net_u32 (0xff020000))
+    {
+      ip6_address_copy (src, ip6_get_link_local_address (sw_if_index));
+      return true;
+    }
+
+  foreach_ip_interface_address (
+    lm6, ia, sw_if_index, 1, ({
+      if (ia->flags & IP_INTERFACE_ADDRESS_FLAG_STALE)
+	continue;
+      tmp = ip_interface_address_get_address (lm6, ia);
+      l = ip6_sas_commonlen (tmp, dst);
+      if (l > bestlen || bestsrc == 0)
+	{
+	  bestsrc = tmp;
+	  bestlen = l;
+	}
+    }));
+  if (bestsrc)
+    {
+      ip6_address_copy (src, bestsrc);
+      return true;
+    }
+  return false;
+}
+
+/*
+ * walk all addresses on an interface and pick the source address with the
+ * longest common prefix with destination.
+ */
+bool
+ip4_sas_by_sw_if_index (u32 sw_if_index, const ip4_address_t *dst,
+			ip4_address_t *src)
+{
+  ip_interface_address_t *ia = 0;
+  ip_lookup_main_t *lm4 = &ip4_main.lookup_main;
+  ip4_address_t *tmp, *bestsrc = 0;
+  int bestlen = 0, l;
+
+  foreach_ip_interface_address (
+    lm4, ia, sw_if_index, 1, ({
+      if (ia->flags & IP_INTERFACE_ADDRESS_FLAG_STALE)
+	continue;
+      tmp = ip_interface_address_get_address (lm4, ia);
+      l = ip4_sas_commonlen (tmp, dst);
+      if (l > bestlen || bestsrc == 0)
+	{
+	  bestsrc = tmp;
+	  bestlen = l;
+	}
+    }));
+  if (bestsrc)
+    {
+      src->as_u32 = bestsrc->as_u32;
+      return true;
+    }
+  return false;
+}
+
+/*
+ * table_id must be set. Default = 0.
+ * sw_if_index is the interface to pick SA from otherwise ~0 will pick from
+ * outbound interface.
+ *
+ * NOTE: What to do if multiple output interfaces?
+ *
+ */
+bool
+ip6_sas (u32 table_id, u32 sw_if_index, const ip6_address_t *dst,
+	 ip6_address_t *src)
+{
+  fib_prefix_t prefix;
+  u32 if_index = sw_if_index;
+
+  /* If sw_if_index is not specified use the output interface. */
+  if (sw_if_index == ~0)
+    {
+      clib_memcpy (&prefix.fp_addr.ip6, dst, sizeof (*dst));
+      prefix.fp_proto = FIB_PROTOCOL_IP6;
+      prefix.fp_len = 128;
+
+      u32 fib_index = fib_table_find (prefix.fp_proto, table_id);
+      if (fib_index == (u32) ~0)
+	return false;
+
+      fib_node_index_t fei = fib_table_lookup (fib_index, &prefix);
+      if (fei == FIB_NODE_INDEX_INVALID)
+	return false;
+
+      u32 output_sw_if_index = fib_entry_get_resolving_interface (fei);
+      if (output_sw_if_index == ~0)
+	return false;
+      if_index = output_sw_if_index;
+    }
+  return ip6_sas_by_sw_if_index (if_index, dst, src);
+}
+
+/*
+ * table_id must be set. Default = 0.
+ * sw_if_index is the interface to pick SA from otherwise ~0 will pick from
+ * outbound interface.
+ *
+ * NOTE: What to do if multiple output interfaces?
+ *
+ */
+bool
+ip4_sas (u32 table_id, u32 sw_if_index, const ip4_address_t *dst,
+	 ip4_address_t *src)
+{
+  fib_prefix_t prefix;
+  u32 if_index = sw_if_index;
+
+  /* If sw_if_index is not specified use the output interface. */
+  if (sw_if_index == ~0)
+    {
+      clib_memcpy (&prefix.fp_addr.ip4, dst, sizeof (*dst));
+      prefix.fp_proto = FIB_PROTOCOL_IP4;
+      prefix.fp_len = 32;
+
+      u32 fib_index = fib_table_find (prefix.fp_proto, table_id);
+      if (fib_index == (u32) ~0)
+	return false;
+
+      fib_node_index_t fei = fib_table_lookup (fib_index, &prefix);
+      if (fei == FIB_NODE_INDEX_INVALID)
+	return false;
+
+      u32 output_sw_if_index = fib_entry_get_resolving_interface (fei);
+      if (output_sw_if_index == ~0)
+	return false;
+      if_index = output_sw_if_index;
+    }
+  return ip4_sas_by_sw_if_index (if_index, dst, src);
+}
diff --git a/src/vnet/ip/ip_sas.h b/src/vnet/ip/ip_sas.h
new file mode 100644
index 00000000000..b1e9e732ed9
--- /dev/null
+++ b/src/vnet/ip/ip_sas.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2021 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef included_ip_sas_h
+#define included_ip_sas_h
+
+#include <stdbool.h>
+#include <vnet/ip/ip6_packet.h>
+#include <vnet/ip/ip4_packet.h>
+
+bool ip6_sas_by_sw_if_index (u32 sw_if_index, const ip6_address_t *dst,
+			     ip6_address_t *src);
+bool ip4_sas_by_sw_if_index (u32 sw_if_index, const ip4_address_t *dst,
+			     ip4_address_t *src);
+bool ip6_sas (u32 table_id, u32 sw_if_index, const ip6_address_t *dst,
+	      ip6_address_t *src);
+bool ip4_sas (u32 table_id, u32 sw_if_index, const ip4_address_t *dst,
+	      ip4_address_t *src);
+
+#endif