pimd: multiple rp commands

Allow the user to specify multiple rp commands.

'ip pim rp A.B.C.D'   -> translates to 'ip pim rp A.G.C.D 224.0.0.0/24'
ip pim rp A.B.C.D A.B.C.D/M
  First is the rp, second is the group with mask.

Groups and masks cannot be over each other except 224.0.0.0/24 which
is the fallback if used.

Ticket: CM-7860
Signed-off-by: Donald Sharp <sharpd@cumulusnetworks.com>

9 files changed, 358 insertions, 53 deletions

diff --git a/pimd/pim_cmd.c b/pimd/pim_cmd.c
index 9c32ab1c32..e804b47ebd 100644
--- a/pimd/pim_cmd.c
+++ b/pimd/pim_cmd.c
@@ -2425,6 +2425,33 @@ DEFUN (show_ip_ssmpingd,
   return CMD_SUCCESS;
 }
 
+static int
+pim_rp_cmd_worker (struct vty *vty, const char *rp, const char *group)
+{
+  int result;
+  result = pim_rp_new (rp, group);
+
+  if (result == -1)
+    {
+      vty_out (vty, "%% Bad RP/group address specified: %s", rp);
+      return CMD_WARNING;
+    }
+
+  if (result == -2)
+    {
+      vty_out (vty, "%% No Path to RP address specified: %s", rp);
+            return CMD_WARNING;
+    }
+
+  if (result == -3)
+    {
+      vty_out (vty, "%% Group range specified cannot overlap");
+      return CMD_ERR_NO_MATCH;
+    }
+
+  return CMD_SUCCESS;
+}
+
 DEFUN (ip_pim_rp,
        ip_pim_rp_cmd,
        "ip pim rp A.B.C.D [A.B.C.D/M]",
@@ -2434,19 +2461,23 @@ DEFUN (ip_pim_rp,
        "ip address of RP\n")
 {
   int idx_ipv4 = 3;
-  int result;
+  return pim_rp_cmd_worker (vty, argv[idx_ipv4]->arg, argv[idx_ipv4 + 1]->arg);
+}
 
-  result = pim_rp_new (argv[idx_ipv4]->arg, argv[idx_ipv4 + 1]->arg);
+static int
+pim_no_rp_cmd_worker (struct vty *vty, const char *rp, const char *group)
+{
+  int result = pim_rp_del (rp, group);
 
   if (result == -1)
     {
-      vty_out(vty, "%% Bad RP address specified: %s", argv[idx_ipv4]->arg);
-      return CMD_ERR_NO_MATCH;
+      vty_out (vty, "%% Unable to Decode specified RP");
+      return CMD_WARNING;
     }
 
   if (result == -2)
     {
-      vty_out(vty, "%% No Path to RP address specified: %s", argv[idx_ipv4]->arg);
+      vty_out (vty, "%% Unable to find specified RP");
       return CMD_WARNING;
     }
 
@@ -2463,9 +2494,7 @@ DEFUN (no_ip_pim_rp,
        "ip address of RP\n")
 {
   int idx_ipv4 = 4;
-  pim_rp_del (argv[idx_ipv4]->arg, argv[idx_ipv4 + 1]->arg);
-
-  return CMD_SUCCESS;
+  return pim_no_rp_cmd_worker (vty, argv[idx_ipv4]->arg, argv[idx_ipv4 + 1]->arg);
 }
 
 DEFUN (ip_multicast_routing,
diff --git a/pimd/pim_memory.c b/pimd/pim_memory.c
index 6014725020..9994ea5d6b 100644
--- a/pimd/pim_memory.c
+++ b/pimd/pim_memory.c
@@ -39,3 +39,4 @@ DEFINE_MTYPE(PIMD, PIM_UPSTREAM,          "PIM upstream (S,G) state")
 DEFINE_MTYPE(PIMD, PIM_SSMPINGD,          "PIM sspimgd socket")
 DEFINE_MTYPE(PIMD, PIM_STATIC_ROUTE,      "PIM Static Route")
 DEFINE_MTYPE(PIMD, PIM_BR,                "PIM Bridge Router info")
+DEFINE_MTYPE(PIMD, PIM_RP,                "PIM RP info")
diff --git a/pimd/pim_memory.h b/pimd/pim_memory.h
index 81841e58b6..3d3f14467b 100644
--- a/pimd/pim_memory.h
+++ b/pimd/pim_memory.h
@@ -38,5 +38,6 @@ DECLARE_MTYPE(PIM_UPSTREAM)
 DECLARE_MTYPE(PIM_SSMPINGD)
 DECLARE_MTYPE(PIM_STATIC_ROUTE)
 DECLARE_MTYPE(PIM_BR)
+DECLARE_MTYPE(PIM_RP)
 
 #endif /* _QUAGGA_PIM_MEMORY_H */
diff --git a/pimd/pim_register.c b/pimd/pim_register.c
index f484b5cc82..2854b3d59d 100644
--- a/pimd/pim_register.c
+++ b/pimd/pim_register.c
@@ -321,7 +321,7 @@ pim_register_recv (struct interface *ifp,
       {
 	upstream = pim_upstream_add (&sg, ifp);
 
-	pim_rp_set_upstream_addr (&upstream->upstream_addr, sg.src);
+	pim_rp_set_upstream_addr (&upstream->upstream_addr, sg.src, sg.grp);
 	pim_nexthop_lookup (&upstream->rpf.source_nexthop,
 			    upstream->upstream_addr, NULL);
 	upstream->rpf.source_nexthop.interface = ifp;
diff --git a/pimd/pim_rp.c b/pimd/pim_rp.c
index 5e2ee2e716..0af7a991ee 100644
--- a/pimd/pim_rp.c
+++ b/pimd/pim_rp.c
@@ -23,6 +23,9 @@
 #include "log.h"
 #include "network.h"
 #include "if.h"
+#include "linklist.h"
+#include "prefix.h"
+#include "memory.h"
 
 #include "pimd.h"
 #include "pim_vty.h"
@@ -31,20 +34,179 @@
 #include "pim_str.h"
 #include "pim_rpf.h"
 #include "pim_sock.h"
+#include "pim_memory.h"
 
-static int i_am_rp = 0;
+struct rp_info
+{
+  struct prefix group;
+  struct pim_rpf rp;
+  int i_am_rp;
+};
+
+static struct list *qpim_rp_list = NULL;
+static struct rp_info *tail = NULL;
+
+static void
+pim_rp_info_free (struct rp_info *rp_info)
+{
+  XFREE (MTYPE_PIM_RP, rp_info);
+}
+
+static int
+pim_rp_list_cmp (void *v1, void *v2)
+{
+  struct rp_info *rp1 = (struct rp_info *)v1;
+  struct rp_info *rp2 = (struct rp_info *)v2;
+
+  if (rp1 == rp2)
+    return 0;
+
+  if (!rp1 && rp2)
+    return -1;
+
+  if (rp1 && !rp2)
+    return 1;
+
+  if (rp1 == tail)
+    return 1;
+
+  return -1;
+}
+
+void
+pim_rp_init (void)
+{
+  struct rp_info *rp_info;
+
+  qpim_rp_list = list_new ();
+  qpim_rp_list->del = (void (*)(void *))pim_rp_info_free;
+  qpim_rp_list->cmp = pim_rp_list_cmp;
+
+  rp_info = XCALLOC (MTYPE_PIM_RP, sizeof (*rp_info));
+
+  if (!rp_info)
+    return;
+
+  str2prefix ("224.0.0.0/4", &rp_info->group);
+  rp_info->rp.rpf_addr.s_addr = INADDR_NONE;
+  tail = rp_info;
+
+  listnode_add (qpim_rp_list, rp_info);
+}
+
+void
+pim_rp_free (void)
+{
+  if (qpim_rp_list)
+    list_free (qpim_rp_list);
+}
+
+static struct rp_info *
+pim_rp_find_exact (struct in_addr rp, struct prefix *group)
+{
+  struct listnode *node;
+  struct rp_info *rp_info;
+
+  for (ALL_LIST_ELEMENTS_RO (qpim_rp_list, node, rp_info))
+    {
+      if (rp.s_addr == rp_info->rp.rpf_addr.s_addr &&
+	  prefix_same (&rp_info->group, group))
+       return rp_info;
+    }
+
+  return NULL;
+}
+
+static struct rp_info *
+pim_rp_find_match (struct in_addr rp, struct prefix *group)
+{
+  struct listnode *node;
+  struct rp_info *rp_info;
+
+  for (ALL_LIST_ELEMENTS_RO (qpim_rp_list, node, rp_info))
+    {
+      if (rp.s_addr == rp_info->rp.rpf_addr.s_addr &&
+          prefix_match (&rp_info->group, group))
+       return rp_info;
+    }
 
-static struct pim_rpf  qpim_rp = { .rpf_addr.s_addr = INADDR_NONE };
+  return NULL;
+}
+
+static struct rp_info *
+pim_rp_find_match_group (struct prefix *group)
+{
+  struct listnode *node;
+  struct rp_info *rp_info;
+
+  for (ALL_LIST_ELEMENTS_RO (qpim_rp_list, node, rp_info))
+    {
+      if (prefix_match (&rp_info->group, group))
+       return rp_info;
+    }
+
+  return NULL;
+}
 
 int
 pim_rp_new (const char *rp, const char *group_range)
 {
   int result;
+  struct rp_info *rp_info;
+  struct rp_info *rp_all;
+  struct prefix group_all;
+
+  str2prefix ("224.0.0.0/4", &group_all);
+  rp_all = pim_rp_find_match_group(&group_all);
+
+  rp_info = XCALLOC (MTYPE_PIM_RP, sizeof (*rp_info));
+  if (!rp)
+    return -1;
+
+  if (group_range == NULL)
+    result = str2prefix ("224.0.0.0/4", &rp_info->group);
+  else
+    result = str2prefix (group_range, &rp_info->group);
+
+  if (!result)
+    return -1;
 
-  result = inet_pton (AF_INET, rp, &qpim_rp.rpf_addr.s_addr);
+  result = inet_pton (AF_INET, rp, &rp_info->rp.rpf_addr.s_addr);
   if (result <= 0)
     return -1;
 
+  /*
+   * Take over the 224.0.0.0/4 group if the rp is INADDR_NONE
+   */
+  if (prefix_same (&rp_all->group, &rp_info->group) &&
+      rp_all->rp.rpf_addr.s_addr == INADDR_NONE)
+    {
+      rp_all->rp.rpf_addr = rp_info->rp.rpf_addr;
+      XFREE (MTYPE_PIM_RP, rp_info);
+      if (!pim_rp_setup ())
+        return -2;
+      return 0;
+    }
+
+  if (pim_rp_find_exact (rp_info->rp.rpf_addr, &rp_info->group))
+    {
+      XFREE (MTYPE_PIM_RP, rp_info);
+      return 0;
+    }
+
+  if (pim_rp_find_match (rp_info->rp.rpf_addr, &rp_info->group))
+    {
+      if (prefix_same (&group_all, &rp_info->group))
+        {
+          return 0;
+        }
+
+      XFREE (MTYPE_PIM_RP, rp_info);
+      return -3;
+    }
+
+  listnode_add_sort (qpim_rp_list, rp_info);
+
   if (!pim_rp_setup ())
     return -2;
 
@@ -54,20 +216,62 @@ pim_rp_new (const char *rp, const char *group_range)
 int
 pim_rp_del (const char *rp, const char *group_range)
 {
-  qpim_rp.rpf_addr.s_addr = INADDR_NONE;
+  struct prefix group;
+  struct in_addr rp_addr;
+  struct prefix g_all;
+  struct rp_info *rp_info;
+  struct rp_info *rp_all;
+  int result;
+
+  str2prefix ("224.0.0.0/4", &g_all);
+  if (group_range == NULL)
+    result = str2prefix ("224.0.0.0/4", &group);
+  else
+    result = str2prefix (group_range, &group);
+
+  if (!result)
+    return -1;
+
+  rp_all = pim_rp_find_match_group (&g_all);
+
+  result = inet_pton (AF_INET, rp, &rp_addr);
+  if (result <= 0)
+    return -1;
+
+  rp_info = pim_rp_find_exact (rp_addr, &group);
+  if (!rp_info)
+    return -2;
+
+  if (rp_all == rp_info)
+    {
+      rp_all->rp.rpf_addr.s_addr = INADDR_NONE;
+      rp_all->i_am_rp = 0;
+      return 0;
+    }
 
+  listnode_delete (qpim_rp_list, rp_info);
   return 0;
 }
 
 int
 pim_rp_setup (void)
 {
-  if (pim_nexthop_lookup (&qpim_rp.source_nexthop, qpim_rp.rpf_addr, NULL) != 0)
+  struct listnode *node;
+  struct rp_info *rp_info;
+  int ret = 0;
+
+  for (ALL_LIST_ELEMENTS_RO (qpim_rp_list, node, rp_info))
     {
-      zlog_err ("Unable to lookup nexthop for rp specified");
-      return 0;
+      if (pim_nexthop_lookup (&rp_info->rp.source_nexthop, rp_info->rp.rpf_addr, NULL) != 0)
+        {
+          zlog_err ("Unable to lookup nexthop for rp specified");
+          ret++;
+        }
     }
 
+  if (ret)
+    return 0;
+
   return 1;
 }
 
@@ -77,29 +281,35 @@ pim_rp_setup (void)
 void
 pim_rp_check_rp (struct in_addr old, struct in_addr new)
 {
-  if (PIM_DEBUG_ZEBRA) {
-    char sold[100];
-    char snew[100];
-    char rp[100];
-    pim_inet4_dump("<rp?>", qpim_rp.rpf_addr, rp, sizeof(rp));
-    pim_inet4_dump("<old?>", old, sold, sizeof(sold));
-    pim_inet4_dump("<new?>", new, snew, sizeof(snew));
-    zlog_debug("%s: %s for old %s new %s", __func__, rp, sold, snew );
-  }
-
-  if (qpim_rp.rpf_addr.s_addr == INADDR_NONE)
+  struct listnode *node;
+  struct rp_info *rp_info;
+
+  if (qpim_rp_list == NULL)
     return;
 
-  if (new.s_addr == qpim_rp.rpf_addr.s_addr)
+  for (ALL_LIST_ELEMENTS_RO (qpim_rp_list, node, rp_info))
     {
-      i_am_rp = 1;
-      return;
-    }
+      if (PIM_DEBUG_ZEBRA) {
+        char sold[100];
+        char snew[100];
+        char rp[100];
+        pim_inet4_dump("<rp?>", rp_info->rp.rpf_addr, rp, sizeof(rp));
+        pim_inet4_dump("<old?>", old, sold, sizeof(sold));
+        pim_inet4_dump("<new?>", new, snew, sizeof(snew));
+        zlog_debug("%s: %s for old %s new %s", __func__, rp, sold, snew );
+      }
+      if (rp_info->rp.rpf_addr.s_addr == INADDR_NONE)
+        continue;
 
-  if (old.s_addr == qpim_rp.rpf_addr.s_addr)
-    {
-      i_am_rp = 0;
-      return;
+      if (new.s_addr == rp_info->rp.rpf_addr.s_addr)
+        {
+	  rp_info->i_am_rp = 1;
+        }
+
+      if (old.s_addr == rp_info->rp.rpf_addr.s_addr)
+        {
+          rp_info->i_am_rp = 0;
+        }
     }
 }
 
@@ -112,7 +322,20 @@ pim_rp_check_rp (struct in_addr old, struct in_addr new)
 int
 pim_rp_i_am_rp (struct in_addr group)
 {
-  return i_am_rp;
+  struct prefix g;
+  struct rp_info *rp_info;
+
+  memset (&g, 0, sizeof (g));
+  g.family = AF_INET;
+  g.prefixlen = 32;
+  g.u.prefix4 = group;
+
+  rp_info = pim_rp_find_match_group (&g);
+
+  if (rp_info)
+    return rp_info->i_am_rp;
+
+  return 0;
 }
 
 /*
@@ -123,11 +346,24 @@ pim_rp_i_am_rp (struct in_addr group)
 struct pim_rpf *
 pim_rp_g (struct in_addr group)
 {
-  /*
-   * For staticly configured RP, it is always the qpim_rp
-   */
-  pim_nexthop_lookup(&qpim_rp.source_nexthop, qpim_rp.rpf_addr, NULL);
-  return(&qpim_rp);
+  struct prefix g;
+  struct rp_info *rp_info;
+
+  memset (&g, 0, sizeof (g));
+  g.family = AF_INET;
+  g.prefixlen = 32;
+  g.u.prefix4 = group;
+
+  rp_info = pim_rp_find_match_group (&g);
+
+  if (rp_info)
+    {
+      pim_nexthop_lookup(&rp_info->rp.source_nexthop, rp_info->rp.rpf_addr, NULL);
+      return (&rp_info->rp);
+    }
+
+  // About to Go Down
+  return NULL;
 }
 
 /*
@@ -139,16 +375,26 @@ pim_rp_g (struct in_addr group)
  *
  */
 int
-pim_rp_set_upstream_addr (struct in_addr *up, struct in_addr source)
+pim_rp_set_upstream_addr (struct in_addr *up, struct in_addr source, struct in_addr group)
 {
-  if ((qpim_rp.rpf_addr.s_addr == INADDR_NONE) && (source.s_addr == INADDR_ANY))
+  struct rp_info *rp_info;
+  struct prefix g;
+
+  memset (&g, 0, sizeof (g));
+  g.family = AF_INET;
+  g.prefixlen = 32;
+  g.u.prefix4 = group;
+
+  rp_info = pim_rp_find_match_group (&g);
+
+  if ((rp_info->rp.rpf_addr.s_addr == INADDR_NONE) && (source.s_addr == INADDR_ANY))
     {
       if (PIM_DEBUG_PIM_TRACE)
 	zlog_debug("%s: Received a (*,G) with no RP configured", __PRETTY_FUNCTION__);
       return 0;
     }
 
-  *up = (source.s_addr == INADDR_ANY) ? qpim_rp.rpf_addr : source;
+  *up = (source.s_addr == INADDR_ANY) ? rp_info->rp.rpf_addr : source;
 
   return 1;
 }
@@ -156,20 +402,40 @@ pim_rp_set_upstream_addr (struct in_addr *up, struct in_addr source)
 int
 pim_rp_config_write (struct vty *vty)
 {
+  struct listnode *node;
+  struct rp_info *rp_info;
   char buffer[32];
+  int count = 0;
 
-  if (qpim_rp.rpf_addr.s_addr != INADDR_NONE)
+  for (ALL_LIST_ELEMENTS_RO (qpim_rp_list, node, rp_info))
     {
-      vty_out(vty, "ip pim rp %s%s", inet_ntop(AF_INET, &qpim_rp.rpf_addr, buffer, 32), VTY_NEWLINE);
-      return 1;
+      if (rp_info->rp.rpf_addr.s_addr == INADDR_NONE)
+        continue;
+
+      if (rp_info->rp.rpf_addr.s_addr != INADDR_NONE)
+        {
+	  char buf[32];
+          vty_out(vty, "ip pim rp %s %s%s", inet_ntop(AF_INET, &rp_info->rp.rpf_addr, buffer, 32),
+		  prefix2str(&rp_info->group, buf, 32), VTY_NEWLINE);
+          count++;
+        }
     }
 
-  return 0;
+  return count;
 }
 
 int
 pim_rp_check_is_my_ip_address (struct in_addr group, struct in_addr dest_addr)
 {
+  struct rp_info *rp_info;
+  struct prefix g;
+
+  memset (&g, 0, sizeof (g));
+  g.family = AF_INET;
+  g.prefixlen = 32;
+  g.u.prefix4 = group;
+
+  rp_info = pim_rp_find_match_group (&g);
   /*
    * See if we can short-cut some?
    * This might not make sense if we ever leave a static RP
@@ -178,7 +444,7 @@ pim_rp_check_is_my_ip_address (struct in_addr group, struct in_addr dest_addr)
    */
   if (I_am_RP(group))
     {
-     if (dest_addr.s_addr == qpim_rp.rpf_addr.s_addr)
+     if (dest_addr.s_addr == rp_info->rp.rpf_addr.s_addr)
        return 1;
     }
 
diff --git a/pimd/pim_rp.h b/pimd/pim_rp.h
index 6299971567..be50bbb18b 100644
--- a/pimd/pim_rp.h
+++ b/pimd/pim_rp.h
@@ -21,6 +21,9 @@
 #ifndef PIM_RP_H
 #define PIM_RP_H
 
+void pim_rp_init (void);
+void pim_rp_free (void);
+
 int pim_rp_new (const char *rp, const char *group);
 int pim_rp_del (const char *rp, const char *group);
 
@@ -33,7 +36,7 @@ int pim_rp_i_am_rp (struct in_addr group);
 
 int pim_rp_check_is_my_ip_address (struct in_addr group, struct in_addr dest_addr);
 
-int pim_rp_set_upstream_addr (struct in_addr *up, struct in_addr source);
+int pim_rp_set_upstream_addr (struct in_addr *up, struct in_addr source, struct in_addr group);
 
 struct pim_rpf *pim_rp_g (struct in_addr group);
 
diff --git a/pimd/pim_upstream.c b/pimd/pim_upstream.c
index 0d77783738..5b7f67c703 100644
--- a/pimd/pim_upstream.c
+++ b/pimd/pim_upstream.c
@@ -430,7 +430,7 @@ static struct pim_upstream *pim_upstream_new(struct prefix_sg *sg,
   }
   
   up->sg                          = *sg;
-  if (!pim_rp_set_upstream_addr (&up->upstream_addr, sg->src))
+  if (!pim_rp_set_upstream_addr (&up->upstream_addr, sg->src, sg->grp))
     {
       if (PIM_DEBUG_PIM_TRACE)
 	zlog_debug("%s: Received a (*,G) with no RP configured", __PRETTY_FUNCTION__);
diff --git a/pimd/pim_zebra.c b/pimd/pim_zebra.c
index 046628de26..c6ca5189c1 100644
--- a/pimd/pim_zebra.c
+++ b/pimd/pim_zebra.c
@@ -388,7 +388,7 @@ pim_scan_individual_oil (struct channel_oil *c_oil)
   int input_iface_vif_index;
   int old_vif_index;
 
-  if (!pim_rp_set_upstream_addr (&vif_source, c_oil->oil.mfcc_origin))
+  if (!pim_rp_set_upstream_addr (&vif_source, c_oil->oil.mfcc_origin, c_oil->oil.mfcc_mcastgrp))
     return;
 
   input_iface_vif_index = fib_lookup_if_vif_index (vif_source);
@@ -976,7 +976,7 @@ void igmp_source_forward_start(struct igmp_source *source)
     struct in_addr vif_source;
     struct pim_interface *pim_oif;
 
-    if (!pim_rp_set_upstream_addr (&vif_source, source->source_addr))
+    if (!pim_rp_set_upstream_addr (&vif_source, source->source_addr, sg.grp))
       return;
 
     int input_iface_vif_index = fib_lookup_if_vif_index(vif_source);
diff --git a/pimd/pimd.c b/pimd/pimd.c
index 709c367135..fa80aec5a2 100644
--- a/pimd/pimd.c
+++ b/pimd/pimd.c
@@ -36,6 +36,7 @@
 #include "pim_rpf.h"
 #include "pim_ssmpingd.h"
 #include "pim_static.h"
+#include "pim_rp.h"
 
 const char *const PIM_ALL_SYSTEMS      = MCAST_ALL_SYSTEMS;
 const char *const PIM_ALL_ROUTERS      = MCAST_ALL_ROUTERS;
@@ -87,12 +88,16 @@ static void pim_free()
      list_free(qpim_static_route_list);
 
   pim_route_map_terminate();
+
+  pim_rp_free ();
 }
 
 void pim_init()
 {
   srandom(time(NULL));
 
+  pim_rp_init ();
+
   if (!inet_aton(PIM_ALL_PIM_ROUTERS, &qpim_all_pim_routers_addr)) {
     zlog_err("%s %s: could not solve %s to group address: errno=%d: %s",
 	     __FILE__, __PRETTY_FUNCTION__,