zebra: add ZEBRA_IPV4_NEXTHOP_LOOKUP_MRIB

This adds a new zapi call "ZEBRA_IPV4_NEXTHOP_LOOKUP_MRIB" performing a
Multicast RPF lookup for a given source.  Details of the lookup
behaviour are left to the zebra side of things.

Note: this is non-reactive, as in, only delivers a snapshot of the state
at a particular point in time.  There's no push notification of changes
happening to the RIB.

This combines the following 3 original patches:
- zebra: add zsend_ipv4_nexthop_lookup_mrib()
- zserv: Query mrib (SAFI_MULTICAST).
- zebra: Cleanups to zebra_rib.

Cc: Everton Marques <everton.marques@gmail.com>
Cc: Balaji G <balajig81@gmail.com>
Signed-off-by: David Lamparter <equinox@opensourcerouting.org>
diff --git a/lib/zebra.h b/lib/zebra.h
index b289a19..a4e0214 100644
--- a/lib/zebra.h
+++ b/lib/zebra.h
@@ -425,7 +425,8 @@
 #define ZEBRA_ROUTER_ID_DELETE            21
 #define ZEBRA_ROUTER_ID_UPDATE            22
 #define ZEBRA_HELLO                       23
-#define ZEBRA_MESSAGE_MAX                 24
+#define ZEBRA_IPV4_NEXTHOP_LOOKUP_MRIB    24
+#define ZEBRA_MESSAGE_MAX                 25
 
 /* Marker value used in new Zserv, in the byte location corresponding
  * the command value in the old zserv header. To allow old and new
diff --git a/zebra/zserv.c b/zebra/zserv.c
index ca17c2c..89eb266 100644
--- a/zebra/zserv.c
+++ b/zebra/zserv.c
@@ -599,6 +599,89 @@
   return zebra_server_send_message(client);
 }
 
+/*
+  Modified version of zsend_ipv4_nexthop_lookup():
+  Query unicast rib if nexthop is not found on mrib.
+  Returns both route metric and protocol distance.
+*/
+static int
+zsend_ipv4_nexthop_lookup_mrib (struct zserv *client, struct in_addr addr)
+{
+  struct stream *s;
+  struct rib *rib;
+  unsigned long nump;
+  u_char num;
+  struct nexthop *nexthop;
+
+  /* Lookup nexthop. */
+  rib = rib_match_ipv4_safi (addr, SAFI_MULTICAST);
+
+  if (IS_ZEBRA_DEBUG_PACKET && IS_ZEBRA_DEBUG_RECV)
+    zlog_debug("%s: %s mrib entry found.", __func__, rib ? "Matching" : "No matching");
+
+  if (!rib) {
+    /* Retry lookup with unicast rib */
+    rib = rib_match_ipv4_safi (addr, SAFI_UNICAST);
+    if (IS_ZEBRA_DEBUG_PACKET && IS_ZEBRA_DEBUG_RECV)
+      zlog_debug("%s: %s rib entry found.", __func__, rib ? "Matching" : "No matching");
+  }
+
+  /* Get output stream. */
+  s = client->obuf;
+  stream_reset (s);
+
+  /* Fill in result. */
+  zserv_create_header (s, ZEBRA_IPV4_NEXTHOP_LOOKUP_MRIB);
+  stream_put_in_addr (s, &addr);
+
+  if (rib)
+    {
+      stream_putc (s, rib->distance);
+      stream_putl (s, rib->metric);
+      num = 0;
+      nump = stream_get_endp(s); /* remember position for nexthop_num */
+      stream_putc (s, 0);        /* reserve room for nexthop_num */
+      /* Only non-recursive routes are elegible to resolve the nexthop we
+       * are looking up. Therefore, we will just iterate over the top
+       * chain of nexthops. */
+      for (nexthop = rib->nexthop; nexthop; nexthop = nexthop->next)
+	if (CHECK_FLAG (nexthop->flags, NEXTHOP_FLAG_FIB))
+	  {
+	    stream_putc (s, nexthop->type);
+	    switch (nexthop->type)
+	      {
+	      case ZEBRA_NEXTHOP_IPV4:
+		stream_put_in_addr (s, &nexthop->gate.ipv4);
+		break;
+	      case ZEBRA_NEXTHOP_IPV4_IFINDEX:
+		stream_put_in_addr (s, &nexthop->gate.ipv4);
+		stream_putl (s, nexthop->ifindex);
+		break;
+	      case ZEBRA_NEXTHOP_IFINDEX:
+	      case ZEBRA_NEXTHOP_IFNAME:
+		stream_putl (s, nexthop->ifindex);
+		break;
+	      default:
+		/* do nothing */
+		break;
+	      }
+	    num++;
+	  }
+    
+      stream_putc_at (s, nump, num); /* store nexthop_num */
+    }
+  else
+    {
+      stream_putc (s, 0); /* distance */
+      stream_putl (s, 0); /* metric */
+      stream_putc (s, 0); /* nexthop_num */
+    }
+
+  stream_putw_at (s, 0, stream_get_endp (s));
+  
+  return zebra_server_send_message(client);
+}
+
 static int
 zsend_ipv4_import_lookup (struct zserv *client, struct prefix_ipv4 *p)
 {
@@ -920,6 +1003,16 @@
   return zsend_ipv4_nexthop_lookup (client, addr);
 }
 
+/* MRIB Nexthop lookup for IPv4. */
+static int
+zread_ipv4_nexthop_lookup_mrib (struct zserv *client, u_short length)
+{
+  struct in_addr addr;
+
+  addr.s_addr = stream_get_ipv4 (client->ibuf);
+  return zsend_ipv4_nexthop_lookup_mrib (client, addr);
+}
+
 /* Nexthop lookup for IPv4. */
 static int
 zread_ipv4_import_lookup (struct zserv *client, u_short length)
@@ -1352,6 +1445,9 @@
     case ZEBRA_IPV4_NEXTHOP_LOOKUP:
       zread_ipv4_nexthop_lookup (client, length);
       break;
+    case ZEBRA_IPV4_NEXTHOP_LOOKUP_MRIB:
+      zread_ipv4_nexthop_lookup_mrib (client, length);
+      break;
 #ifdef HAVE_IPV6
     case ZEBRA_IPV6_NEXTHOP_LOOKUP:
       zread_ipv6_nexthop_lookup (client, length);