pim-msdp: part-2: SA cache support

This commit includes -
1. Maintaining SA cache with local and remote entries.
2. Local SA entries - there are two cases where we pick up these -
   - We are RP and got a source-register from the FHR.
   - We are RP and FHR and learnt a new directly connected source on a
     DR interface.
3. Local entries are pushed to peers immediately on addition and
   periodically. An immediate push is also done when peer session is
   established.
4. Remote SA entries - from other peers in the mesh group and passed
   peer-RPF checks.
5. Remote entries are aged out. No other way to del them
   currently. In the future we may add a knob to flush entries on
   peer-down.

Testing done -
Misc topologies with CL routers plus basic interop with another vendor (
we can process their SA updates and they ours).

Sample output -
root@rp:~# vtysh -c "show ip msdp sa"
Source                     Group               RP    Uptime
33.1.1.1               239.1.1.2            local  00:02:34
33.1.1.1               239.1.1.3            local  00:02:19
44.1.1.1               239.1.1.4        100.1.3.1  00:01:12
44.1.1.1               239.1.1.5        100.1.3.1  00:00:55
root@rp:~#

Ticket: CM-13306

Signed-off-by: Anuradha Karuppiah <anuradhak@cumulusnetworks.com>
Acked-by: Donald Sharp <sharpd@cumulusnetworks.com>

13 files changed, 892 insertions, 56 deletions

diff --git a/pimd/pim_cmd.c b/pimd/pim_cmd.c
index 52d1157f2..23e8ebd77 100644
--- a/pimd/pim_cmd.c
+++ b/pimd/pim_cmd.c
@@ -5274,6 +5274,53 @@ DEFUN (show_ip_msdp_peer,
   return CMD_SUCCESS;
 }
 
+static void
+ip_msdp_show_sa(struct vty *vty, u_char uj)
+{
+  struct listnode *sanode;
+  struct pim_msdp_sa *sa;
+  char src_str[INET_ADDRSTRLEN];
+  char grp_str[INET_ADDRSTRLEN];
+  char rp_str[INET_ADDRSTRLEN];
+  char timebuf[PIM_MSDP_UPTIME_STRLEN];
+  int64_t now;
+
+  if (uj) {
+    // XXX: blah
+    return;
+  } else {
+    vty_out(vty, "Source                     Group               RP    Uptime%s", VTY_NEWLINE);
+    for (ALL_LIST_ELEMENTS_RO(msdp->sa_list, sanode, sa)) {
+      now = pim_time_monotonic_sec();
+      pim_time_uptime(timebuf, sizeof(timebuf), now - sa->uptime);
+      pim_inet4_dump("<src?>", sa->sg.src, src_str, sizeof(src_str));
+      pim_inet4_dump("<grp?>", sa->sg.grp, grp_str, sizeof(grp_str));
+      if (sa->flags & PIM_MSDP_SAF_LOCAL) {
+        strcpy(rp_str, "local");
+      } else {
+        pim_inet4_dump("<rp?>", sa->rp, rp_str, sizeof(rp_str));
+      }
+      vty_out(vty, "%-15s  %15s  %15s  %8s%s",
+          src_str, grp_str, rp_str, timebuf, VTY_NEWLINE);
+    }
+  }
+}
+
+DEFUN (show_ip_msdp_sa,
+       show_ip_msdp_sa_cmd,
+       "show ip msdp sa [json]",
+       SHOW_STR
+       IP_STR
+       MSDP_STR
+       "MSDP active-source information\n"
+       "JavaScript Object Notation\n")
+{
+  u_char uj = use_json(argc, argv);
+  ip_msdp_show_sa(vty, uj);
+
+  return CMD_SUCCESS;
+}
+
 void pim_cmd_init()
 {
   install_node (&pim_global_node, pim_global_config_write);       /* PIM_NODE */
@@ -5351,6 +5398,7 @@ void pim_cmd_init()
   install_element (VIEW_NODE, &show_ip_rib_cmd);
   install_element (VIEW_NODE, &show_ip_ssmpingd_cmd);
   install_element (VIEW_NODE, &show_ip_msdp_peer_cmd);
+  install_element (VIEW_NODE, &show_ip_msdp_sa_cmd);
   install_element (VIEW_NODE, &show_debugging_pim_cmd);
 
   install_element (ENABLE_NODE, &clear_ip_interfaces_cmd);
diff --git a/pimd/pim_memory.c b/pimd/pim_memory.c
index 30a5446dd..da37da699 100644
--- a/pimd/pim_memory.c
+++ b/pimd/pim_memory.c
@@ -43,3 +43,4 @@ DEFINE_MTYPE(PIMD, PIM_RP,                "PIM RP info")
 DEFINE_MTYPE(PIMD, PIM_FILTER_NAME,       "PIM RP filter info")
 DEFINE_MTYPE(PIMD, PIM_MSDP_PEER,         "PIM MSDP peer")
 DEFINE_MTYPE(PIMD, PIM_MSDP_PEER_MG_NAME, "PIM MSDP peer mesh-group")
+DEFINE_MTYPE(PIMD, PIM_MSDP_SA,           "PIM MSDP source-active cache")
diff --git a/pimd/pim_memory.h b/pimd/pim_memory.h
index 2b7e6ad37..434de0b19 100644
--- a/pimd/pim_memory.h
+++ b/pimd/pim_memory.h
@@ -42,5 +42,6 @@ DECLARE_MTYPE(PIM_RP)
 DECLARE_MTYPE(PIM_FILTER_NAME)
 DECLARE_MTYPE(PIM_MSDP_PEER)
 DECLARE_MTYPE(PIM_MSDP_PEER_MG_NAME)
+DECLARE_MTYPE(PIM_MSDP_SA)
 
 #endif /* _QUAGGA_PIM_MEMORY_H */
diff --git a/pimd/pim_mroute.c b/pimd/pim_mroute.c
index 48890d32f..9cd323a2d 100644
--- a/pimd/pim_mroute.c
+++ b/pimd/pim_mroute.c
@@ -151,7 +151,7 @@ pim_mroute_msg_nocache (int fd, struct interface *ifp, const struct igmpmsg *msg
     return 0;
   }
   PIM_UPSTREAM_FLAG_SET_SRC_STREAM(up->flags);
-  PIM_UPSTREAM_FLAG_SET_CREATED_BY_UPSTREAM(up->flags);
+  pim_upstream_set_created_by_upstream(up);
 
   pim_upstream_keep_alive_timer_start (up, qpim_keep_alive_time);
 
@@ -394,7 +394,7 @@ pim_mroute_msg_wrvifwhole (int fd, struct interface *ifp, const char *buf)
 	  return -2;
 	}
       PIM_UPSTREAM_FLAG_SET_SRC_STREAM(up->flags);
-      PIM_UPSTREAM_FLAG_SET_CREATED_BY_UPSTREAM(up->flags);
+      pim_upstream_set_created_by_upstream(up);
 
       pim_upstream_keep_alive_timer_start (up, qpim_keep_alive_time);
       up->channel_oil = oil;
diff --git a/pimd/pim_msdp.c b/pimd/pim_msdp.c
index 2805a0609..9ca924482 100644
--- a/pimd/pim_msdp.c
+++ b/pimd/pim_msdp.c
@@ -23,13 +23,17 @@
 #include <lib/hash.h>
 #include <lib/jhash.h>
 #include <lib/log.h>
+#include <lib/prefix.h>
 #include <lib/sockunion.h>
 #include <lib/stream.h>
 #include <lib/thread.h>
+#include <lib/vty.h>
+#include <lib/plist.h>
 
 #include "pimd.h"
 #include "pim_cmd.h"
 #include "pim_memory.h"
+#include "pim_rp.h"
 #include "pim_str.h"
 #include "pim_time.h"
 
@@ -44,7 +48,376 @@ static void pim_msdp_peer_cr_timer_setup(struct pim_msdp_peer *mp, bool start);
 static void pim_msdp_peer_ka_timer_setup(struct pim_msdp_peer *mp, bool start);
 static void pim_msdp_peer_hold_timer_setup(struct pim_msdp_peer *mp, bool start);
 static void pim_msdp_peer_free(struct pim_msdp_peer *mp);
+static void pim_msdp_enable(void);
+static void pim_msdp_sa_adv_timer_setup(bool start);
+static void pim_msdp_sa_deref(struct pim_msdp_sa *sa, enum pim_msdp_sa_flags flags);
 
+/************************ SA cache management ******************************/
+char *
+pim_msdp_sa_key_dump(struct pim_msdp_sa *sa, char *buf, int buf_size, bool long_format)
+{
+  char rp_str[INET_ADDRSTRLEN];
+
+  if (long_format && (sa->flags & PIM_MSDP_SAF_PEER)) {
+    pim_inet4_dump("<rp?>", sa->rp, rp_str, sizeof(rp_str));
+    snprintf(buf, buf_size, "MSDP SA %s rp %s",
+        pim_str_sg_dump(&sa->sg), rp_str);
+  } else {
+    snprintf(buf, buf_size, "MSDP SA %s", pim_str_sg_dump(&sa->sg));
+  }
+
+  return buf;
+}
+
+static void
+pim_msdp_sa_timer_expiry_log(struct pim_msdp_sa *sa, const char *timer_str)
+{
+  char key_str[PIM_MSDP_SA_KEY_STRLEN];
+
+  pim_msdp_sa_key_dump(sa, key_str, sizeof(key_str), false);
+  zlog_debug("%s %s timer expired", key_str, timer_str);
+}
+
+/* RFC-3618:Sec-5.1 - global active source advertisement timer */
+static int
+pim_msdp_sa_adv_timer_cb(struct thread *t)
+{
+  if (PIM_DEBUG_MSDP_INTERNAL) {
+    zlog_debug("MSDP SA advertisment timer expired");
+  }
+
+  pim_msdp_pkt_sa_tx();
+  pim_msdp_sa_adv_timer_setup(true /* start */);
+  return 0;
+}
+static void
+pim_msdp_sa_adv_timer_setup(bool start)
+{
+  THREAD_OFF(msdp->sa_adv_timer);
+  if (start) {
+    THREAD_TIMER_ON(msdp->master, msdp->sa_adv_timer,
+        pim_msdp_sa_adv_timer_cb, NULL, PIM_MSDP_SA_ADVERTISMENT_TIME);
+  }
+}
+
+/* RFC-3618:Sec-5.3 - SA cache state timer */
+static int
+pim_msdp_sa_state_timer_cb(struct thread *t)
+{
+  struct pim_msdp_sa *sa;
+
+  zassert(t);
+  sa = THREAD_ARG(t);
+  zassert(sa);
+
+  if (PIM_DEBUG_MSDP_EVENTS) {
+    pim_msdp_sa_timer_expiry_log(sa, "state");
+  }
+
+  pim_msdp_sa_deref(sa, PIM_MSDP_SAF_PEER);
+  return 0;
+}
+static void
+pim_msdp_sa_state_timer_setup(struct pim_msdp_sa *sa, bool start)
+{
+  THREAD_OFF(sa->sa_state_timer);
+  if (start) {
+    THREAD_TIMER_ON(msdp->master, sa->sa_state_timer,
+        pim_msdp_sa_state_timer_cb, sa, PIM_MSDP_SA_HOLD_TIME);
+  }
+}
+
+/* release all mem associated with a sa */
+static void
+pim_msdp_sa_free(struct pim_msdp_sa *sa)
+{
+  XFREE(MTYPE_PIM_MSDP_SA, sa);
+}
+
+static struct pim_msdp_sa *
+pim_msdp_sa_new(struct prefix_sg *sg, struct in_addr rp)
+{
+  struct pim_msdp_sa *sa;
+
+  pim_msdp_enable();
+
+  sa = XCALLOC(MTYPE_PIM_MSDP_SA, sizeof(*sa));
+  if (!sa) {
+    zlog_err("%s: PIM XCALLOC(%zu) failure",
+             __PRETTY_FUNCTION__, sizeof(*sa));
+    return NULL;
+  }
+
+  sa->sg = *sg;
+  sa->rp = rp;
+  sa->uptime = pim_time_monotonic_sec();
+
+  /* insert into misc tables for easy access */
+  sa = hash_get(msdp->sa_hash, sa, hash_alloc_intern);
+  if (!sa) {
+    zlog_err("%s: PIM hash get failure", __PRETTY_FUNCTION__);
+    pim_msdp_sa_free(sa);
+    return NULL;
+  }
+  listnode_add_sort(msdp->sa_list, sa);
+
+  if (PIM_DEBUG_MSDP_EVENTS) {
+    char key_str[PIM_MSDP_SA_KEY_STRLEN];
+
+    pim_msdp_sa_key_dump(sa, key_str, sizeof(key_str), true);
+    zlog_debug("%s created", key_str);
+  }
+
+  return sa;
+}
+
+static struct pim_msdp_sa *
+pim_msdp_sa_find(struct prefix_sg *sg)
+{
+  struct pim_msdp_sa lookup;
+
+  lookup.sg = *sg;
+  return hash_lookup(msdp->sa_hash, &lookup);
+}
+
+static struct pim_msdp_sa *
+pim_msdp_sa_add(struct prefix_sg *sg, struct in_addr rp)
+{
+  struct pim_msdp_sa *sa;
+
+  sa = pim_msdp_sa_find(sg);
+  if (sa) {
+    return sa;
+  }
+
+  return pim_msdp_sa_new(sg, rp);
+}
+
+static void
+pim_msdp_sa_del(struct pim_msdp_sa * sa)
+{
+  /* stop timers */
+  pim_msdp_sa_state_timer_setup(sa, false /* start */);
+
+  /* remove the entry from various tables */
+  listnode_delete(msdp->sa_list, sa);
+  hash_release(msdp->sa_hash, sa);
+
+  if (PIM_DEBUG_MSDP_EVENTS) {
+    char key_str[PIM_MSDP_SA_KEY_STRLEN];
+
+    pim_msdp_sa_key_dump(sa, key_str, sizeof(key_str), true /* long */);
+    zlog_debug("%s deleted", key_str);
+  }
+
+  /* free up any associated memory */
+  pim_msdp_sa_free(sa);
+}
+
+/* When a local active-source is removed there is no way to withdraw the
+ * source from peers. We will simply remove it from the SA cache so it will
+ * not be sent in supsequent SA updates. Peers will consequently timeout the
+ * SA.
+ * Similarly a "peer-added" SA is never explicitly deleted. It is simply
+ * aged out overtime if not seen in the SA updates from the peers. 
+ * XXX: should we provide a knob to drop entries learnt from a peer when the
+ * peer goes down? */
+static void
+pim_msdp_sa_deref(struct pim_msdp_sa *sa, enum pim_msdp_sa_flags flags)
+{
+  char key_str[PIM_MSDP_SA_KEY_STRLEN];
+
+  pim_msdp_sa_key_dump(sa, key_str, sizeof(key_str), true);
+
+  if ((sa->flags &PIM_MSDP_SAF_LOCAL)) {
+    if (flags & PIM_MSDP_SAF_LOCAL) {
+      zlog_debug("%s local reference removed", key_str);
+      if (msdp->local_cnt)
+        --msdp->local_cnt;
+    }
+  }
+
+  if ((sa->flags &PIM_MSDP_SAF_PEER)) {
+    if (flags & PIM_MSDP_SAF_PEER) {
+      zlog_debug("%s peer reference removed", key_str);
+      pim_msdp_sa_state_timer_setup(sa, false /* start */);
+    }
+  }
+
+  sa->flags &= ~flags;
+  if (!(sa->flags & PIM_MSDP_SAF_REF)) {
+    pim_msdp_sa_del(sa);
+  }
+}
+
+void
+pim_msdp_sa_ref(struct pim_msdp_peer *mp, struct prefix_sg *sg,
+                struct in_addr rp)
+{
+  struct pim_msdp_sa *sa;
+  char key_str[PIM_MSDP_SA_KEY_STRLEN];
+
+  sa = pim_msdp_sa_add(sg, rp);
+  if (!sa) {
+    return;
+  }
+
+  if (PIM_DEBUG_MSDP_EVENTS) {
+    pim_msdp_sa_key_dump(sa, key_str, sizeof(key_str), true);
+  }
+
+  /* reference it */
+  if (mp) {
+    if (!(sa->flags & PIM_MSDP_SAF_PEER)) {
+      sa->flags |= PIM_MSDP_SAF_PEER;
+      if (PIM_DEBUG_MSDP_EVENTS) {
+        zlog_debug("%s added by peer", key_str);
+      }
+    }
+    sa->peer = mp->peer;
+    /* start/re-start the state timer to prevent cache expiry */
+    pim_msdp_sa_state_timer_setup(sa, true /* start */);
+  } else {
+    if (!(sa->flags & PIM_MSDP_SAF_LOCAL)) {
+      sa->flags |= PIM_MSDP_SAF_LOCAL;
+      ++msdp->local_cnt;
+      if (PIM_DEBUG_MSDP_EVENTS) {
+        zlog_debug("%s added locally", key_str);
+      }
+      /* send an immeidate SA update to peers */
+      pim_msdp_pkt_sa_tx_one(sa);
+    }
+    sa->flags &= ~PIM_MSDP_SAF_STALE;
+  }
+}
+
+void
+pim_msdp_sa_local_add(struct prefix_sg *sg)
+{
+  struct in_addr rp;
+
+  if (!(msdp->flags & PIM_MSDPF_ENABLE)) {
+    /* if the feature is not enabled do nothing; we will collect all local
+     * sources whenever it is */
+    return;
+  }
+
+  /* check if I am RP for this group. XXX: is this check really needed? */
+  if (!I_am_RP(sg->grp)) {
+    return;
+  }
+  rp.s_addr = 0;
+  pim_msdp_sa_ref(NULL /* mp */, sg, rp);
+}
+
+void
+pim_msdp_sa_local_del(struct prefix_sg *sg)
+{
+  struct pim_msdp_sa *sa;
+
+  if (!(msdp->flags & PIM_MSDPF_ENABLE)) {
+    /* if the feature is not enabled do nothing; we will collect all local
+     * sources whenever it is */
+    return;
+  }
+
+  sa = pim_msdp_sa_find(sg);
+  if (sa) {
+    pim_msdp_sa_deref(sa, PIM_MSDP_SAF_LOCAL);
+  }
+}
+
+static void
+pim_msdp_sa_local_setup(void)
+{
+  struct pim_upstream *up;
+  struct listnode *up_node;
+
+  for (ALL_LIST_ELEMENTS_RO(pim_upstream_list, up_node, up)) {
+    if (PIM_UPSTREAM_FLAG_TEST_CREATED_BY_UPSTREAM(up->flags)) {
+      pim_msdp_sa_local_add(&up->sg);
+    }
+  }
+}
+
+/* whenever the RP changes we need to re-evaluate the "local"
+ * SA-cache */
+/* XXX: need to call this from thr right places. also needs more testing */
+void
+pim_msdp_i_am_rp_changed(void)
+{
+  struct listnode *sanode;
+  struct pim_msdp_sa *sa;
+
+  /* mark all local entries as stale */
+  for (ALL_LIST_ELEMENTS_RO(msdp->sa_list, sanode, sa)) {
+    sa->flags |= PIM_MSDP_SAF_STALE;
+  }
+
+  /* re-setup local SA entries */
+  pim_msdp_sa_local_setup();
+
+  /* purge stale SA entries */
+  for (ALL_LIST_ELEMENTS_RO(msdp->sa_list, sanode, sa)) {
+    if (sa->flags & PIM_MSDP_SAF_STALE) {
+      pim_msdp_sa_deref(sa, PIM_MSDP_SAF_LOCAL);
+    }
+  }
+}
+
+/* sa hash and peer list helpers */
+static unsigned int
+pim_msdp_sa_hash_key_make(void *p)
+{
+  struct pim_msdp_sa *sa = p;
+
+  return (jhash_2words(sa->sg.src.s_addr, sa->sg.grp.s_addr, 0));
+}
+
+static int
+pim_msdp_sa_hash_eq(const void *p1, const void *p2)
+{
+  const struct pim_msdp_sa *sa1 = p1;
+  const struct pim_msdp_sa *sa2 = p2;
+
+  return ((sa1->sg.src.s_addr == sa2->sg.src.s_addr) &&
+          (sa1->sg.grp.s_addr == sa2->sg.grp.s_addr));
+}
+
+static int
+pim_msdp_sa_comp(const void *p1, const void *p2)
+{
+  const struct pim_msdp_sa *sa1 = p1;
+  const struct pim_msdp_sa *sa2 = p2;
+
+  if (ntohl(sa1->sg.grp.s_addr) < ntohl(sa2->sg.grp.s_addr))
+    return -1;
+
+  if (ntohl(sa1->sg.grp.s_addr) > ntohl(sa2->sg.grp.s_addr))
+    return 1;
+
+  if (ntohl(sa1->sg.src.s_addr) < ntohl(sa2->sg.src.s_addr))
+    return -1;
+
+  if (ntohl(sa1->sg.src.s_addr) > ntohl(sa2->sg.src.s_addr))
+    return 1;
+
+  return 0;
+}
+
+/* RFC-3618:Sec-10.1.3 - Peer-RPF forwarding */
+/* XXX: this can use a bit of refining and extensions */
+bool
+pim_msdp_peer_rpf_check(struct pim_msdp_peer *mp, struct in_addr rp)
+{
+  if (mp->peer.s_addr == rp.s_addr) {
+    return true;
+  }
+
+  return false;
+}
+  
+/************************ Peer session management **************************/
 char *
 pim_msdp_state_dump(enum pim_msdp_peer_state state, char *buf, int buf_size)
 {
@@ -148,6 +521,8 @@ pim_msdp_peer_established(struct pim_msdp_peer *mp)
   pim_msdp_peer_ka_timer_setup(mp, true /* start */);
   pim_msdp_peer_hold_timer_setup(mp, true /* start */);
 
+  pim_msdp_pkt_sa_tx_to_one_peer(mp);
+
   PIM_MSDP_PEER_WRITE_ON(mp);
   PIM_MSDP_PEER_READ_ON(mp);
 }
@@ -211,7 +586,7 @@ pim_msdp_peer_timer_expiry_log(struct pim_msdp_peer *mp, const char *timer_str)
   char key_str[PIM_MSDP_PEER_KEY_STRLEN];
 
   pim_msdp_peer_key_dump(mp, key_str, sizeof(key_str), false);
-  zlog_debug("%s timer %s expired", key_str, timer_str);
+  zlog_debug("%s %s timer expired", key_str, timer_str);
 }
 
 /* RFC-3618:Sec-5.4 - peer hold timer */
@@ -263,10 +638,6 @@ pim_msdp_peer_ka_timer_cb(struct thread *t)
     pim_msdp_peer_timer_expiry_log(mp, "ka");
   }
 
-  if (mp->state != PIM_MSDP_ESTABLISHED) {
-    return 0;
-  }
-
   pim_msdp_pkt_ka_tx(mp);
   pim_msdp_peer_ka_timer_setup(mp, true /* start */);
   return 0;
@@ -358,6 +729,16 @@ pim_msdp_peer_pkt_rxed(struct pim_msdp_peer *mp)
   }
 }
 
+/* if a valid packet is txed to the peer we can restart ka timer and avoid
+ * unnecessary ka noise in the network */
+void
+pim_msdp_peer_pkt_txed(struct pim_msdp_peer *mp)
+{
+  if (mp->state == PIM_MSDP_ESTABLISHED) {
+    pim_msdp_peer_ka_timer_setup(mp, true /* start */);
+  }
+}
+
 static void pim_msdp_addr2su(union sockunion *su, struct in_addr addr)
 {
   sockunion_init(su);
@@ -375,6 +756,8 @@ pim_msdp_peer_new(struct in_addr peer_addr, struct in_addr local_addr,
 {
   struct pim_msdp_peer *mp;
 
+  pim_msdp_enable();
+
   mp = XCALLOC(MTYPE_PIM_MSDP_PEER, sizeof(*mp));
   if (!mp) {
     zlog_err("%s: PIM XCALLOC(%zu) failure",
@@ -385,6 +768,8 @@ pim_msdp_peer_new(struct in_addr peer_addr, struct in_addr local_addr,
   mp->peer = peer_addr;
   pim_msdp_addr2su(&mp->su_peer, mp->peer);
   mp->local = local_addr;
+  /* XXX: originator_id setting needs to move to the mesh group */
+  msdp->originator_id = local_addr;
   pim_msdp_addr2su(&mp->su_local, mp->local);
   mp->mesh_group_name = XSTRDUP(MTYPE_PIM_MSDP_PEER_MG_NAME, mesh_group_name);
   mp->state = PIM_MSDP_INACTIVE;
@@ -394,13 +779,6 @@ pim_msdp_peer_new(struct in_addr peer_addr, struct in_addr local_addr,
     mp->flags |= PIM_MSDP_PEERF_LISTENER;
   }
 
-  if (PIM_DEBUG_MSDP_EVENTS) {
-    char key_str[PIM_MSDP_PEER_KEY_STRLEN];
-
-    pim_msdp_peer_key_dump(mp, key_str, sizeof(key_str), true);
-    zlog_debug("%s created", key_str);
-  }
-
   /* setup packet buffers */
   mp->ibuf = stream_new(PIM_MSDP_MAX_PACKET_SIZE);
   mp->obuf = stream_fifo_new();
@@ -415,8 +793,14 @@ pim_msdp_peer_new(struct in_addr peer_addr, struct in_addr local_addr,
   listnode_add_sort(msdp->peer_list, mp);
 
   if (PIM_DEBUG_MSDP_EVENTS) {
+    char key_str[PIM_MSDP_PEER_KEY_STRLEN];
+
+    pim_msdp_peer_key_dump(mp, key_str, sizeof(key_str), true);
+    zlog_debug("%s created", key_str);
+
     pim_msdp_peer_state_chg_log(mp);
   }
+
   /* fireup the connect state machine */
   if (PIM_MSDP_PEER_IS_LISTENER(mp)) {
     pim_msdp_peer_listen(mp);
@@ -531,6 +915,42 @@ pim_msdp_peer_comp(const void *p1, const void *p2)
   return 0;
 }
 
+/*********************** MSDP feature APIs *********************************/
+int
+pim_msdp_config_write(struct vty *vty)
+{
+  struct listnode *mpnode;
+  struct pim_msdp_peer *mp;
+  char peer_str[INET_ADDRSTRLEN];
+  char local_str[INET_ADDRSTRLEN];
+  int count = 0;
+
+  for (ALL_LIST_ELEMENTS_RO(msdp->peer_list, mpnode, mp)) {
+    pim_inet4_dump("<peer?>", mp->peer, peer_str, sizeof(peer_str));
+    pim_inet4_dump("<local?>", mp->local, local_str, sizeof(local_str));
+    vty_out(vty, "ip msdp peer %s source %s%s",
+        peer_str, local_str, VTY_NEWLINE);
+    ++count;
+  }
+  return count;
+}
+
+/* Enable feature including active/periodic timers etc. on the first peer
+ * config. Till then MSDP should just stay quiet. */
+static void
+pim_msdp_enable(void)
+{
+  if (msdp->flags & PIM_MSDPF_ENABLE) {
+    /* feature is already enabled */
+    return;
+  }
+  msdp->flags |= PIM_MSDPF_ENABLE;
+  msdp->work_obuf = stream_new(PIM_MSDP_MAX_PACKET_SIZE);
+  pim_msdp_sa_adv_timer_setup(true /* start */);
+  /* setup sa cache based on local sources */
+  pim_msdp_sa_local_setup();
+}
+
 /* MSDP init */
 void
 pim_msdp_init(struct thread_master *master)
@@ -539,12 +959,19 @@ pim_msdp_init(struct thread_master *master)
    * complete */
   PIM_DO_DEBUG_MSDP_INTERNAL;
 
+  msdp->master = master;
+
   msdp->peer_hash = hash_create(pim_msdp_peer_hash_key_make,
                                  pim_msdp_peer_hash_eq);
   msdp->peer_list = list_new();
   msdp->peer_list->del = (void (*)(void *))pim_msdp_peer_free;
   msdp->peer_list->cmp = (int (*)(void *, void *))pim_msdp_peer_comp;
-  msdp->master = master;
+
+  msdp->sa_hash = hash_create(pim_msdp_sa_hash_key_make,
+                                 pim_msdp_sa_hash_eq);
+  msdp->sa_list = list_new();
+  msdp->sa_list->del = (void (*)(void *))pim_msdp_sa_free;
+  msdp->sa_list->cmp = (int (*)(void *, void *))pim_msdp_sa_comp;
 }
 
 /* counterpart to MSDP init; XXX: unused currently */
diff --git a/pimd/pim_msdp.h b/pimd/pim_msdp.h
index 6f8561f3c..2af453f68 100644
--- a/pimd/pim_msdp.h
+++ b/pimd/pim_msdp.h
@@ -50,14 +50,44 @@ enum pim_msdp_err {
 
 #define PIM_MSDP_STATE_STRLEN 16
 #define PIM_MSDP_PEER_KEY_STRLEN 80
+#define PIM_MSDP_SA_KEY_STRLEN 80
 #define PIM_MSDP_UPTIME_STRLEN 80
 #define PIM_MSDP_TCP_PORT 639
 #define PIM_MSDP_SOCKET_SNDBUF_SIZE 65536
 
-#define PIM_MSDP_PEER_IS_LISTENER(mp) (mp->flags & PIM_MSDP_PEERF_LISTENER)
+enum pim_msdp_sa_flags {
+  PIM_MSDP_SAF_NONE = 0,
+  /* There are two cases where we can pickup an active source locally -
+   * 1. We are RP and got a source-register from the FHR
+   * 2. We are RP and FHR and learnt a new directly connected source on a
+   * DR interface */
+  PIM_MSDP_SAF_LOCAL = (1 << 0),
+  /* We got this in the MSDP SA TLV from a peer (and this passed peer-RPF
+   * checks) */
+  PIM_MSDP_SAF_PEER = (1 << 1),
+  PIM_MSDP_SAF_REF = (PIM_MSDP_SAF_LOCAL | PIM_MSDP_SAF_PEER),
+  PIM_MSDP_SAF_STALE = (1 << 2) /* local entries can get kicked out on
+                                 * misc pim events such as RP change */
+};
+
+struct pim_msdp_sa {
+  struct prefix_sg sg;
+  struct in_addr rp; /* Last RP address associated with this SA */
+  struct in_addr peer; /* last peer from who we heard this SA */
+  enum pim_msdp_sa_flags flags;
+
+  /* rfc-3618 is missing default value for SA-hold-down-Period. pulled
+   * this number from industry-standards */
+#define PIM_MSDP_SA_HOLD_TIME ((3*60)+30)
+  struct thread *sa_state_timer;  // 5.6
+  int64_t uptime;
+};
+
 enum pim_msdp_peer_flags {
   PIM_MSDP_PEERF_NONE = 0,
-  PIM_MSDP_PEERF_LISTENER = (1 << 0)
+  PIM_MSDP_PEERF_LISTENER = (1 << 0),
+#define PIM_MSDP_PEER_IS_LISTENER(mp) (mp->flags & PIM_MSDP_PEERF_LISTENER)
+  PIM_MSDP_PEERF_SA_JUST_SENT = (1 << 1)
 };
 
 struct pim_msdp_peer {
@@ -84,6 +114,7 @@ struct pim_msdp_peer {
   struct thread *cr_timer;  // 5.6
 
   /* packet thread and buffers */
+  uint32_t packet_size;
   struct stream *ibuf;
   struct stream_fifo *obuf;
   struct thread *t_read;
@@ -102,7 +133,8 @@ struct pim_msdp_peer {
 
 enum pim_msdp_flags {
   PIM_MSDPF_NONE = 0,
-  PIM_MSDPF_LISTENER = (1 << 0)
+  PIM_MSDPF_ENABLE = (1 << 0),
+  PIM_MSDPF_LISTENER = (1 << 1)
 };
 
 struct pim_msdp_listener {
@@ -113,11 +145,25 @@ struct pim_msdp_listener {
 
 struct pim_msdp {
   enum pim_msdp_flags flags;
-  struct hash *peer_hash;
-  struct list *peer_list;
-  struct pim_msdp_listener listener;
   struct thread_master *master;
+  struct pim_msdp_listener listener;
   uint32_t rejected_accepts;
+
+  /* MSDP peer info */
+  struct hash *peer_hash;
+  struct list *peer_list;
+
+  /* MSDP active-source info */
+#define PIM_MSDP_SA_ADVERTISMENT_TIME 60
+  struct thread *sa_adv_timer;  // 5.6
+  struct hash *sa_hash;
+  struct list *sa_list;
+  uint32_t local_cnt;
+
+  /* keep a scratch pad for building SA TLVs */
+  struct stream *work_obuf;
+
+  struct in_addr originator_id;
 };
 
 #define PIM_MSDP_PEER_READ_ON(mp) THREAD_READ_ON(msdp->master, mp->t_read, pim_msdp_read, mp, mp->fd);
@@ -139,5 +185,12 @@ void pim_msdp_peer_stop_tcp_conn(struct pim_msdp_peer *mp, bool chg_state);
 void pim_msdp_peer_reset_tcp_conn(struct pim_msdp_peer *mp, const char *rc_str);
 int pim_msdp_write(struct thread *thread);
 char *pim_msdp_peer_key_dump(struct pim_msdp_peer *mp, char *buf, int buf_size, bool long_format);
-
+int pim_msdp_config_write(struct vty *vty);
+void pim_msdp_peer_pkt_txed(struct pim_msdp_peer *mp);
+char *pim_msdp_sa_key_dump(struct pim_msdp_sa *sa, char *buf, int buf_size, bool long_format);
+void pim_msdp_sa_ref(struct pim_msdp_peer *mp, struct prefix_sg *sg, struct in_addr rp);
+void pim_msdp_sa_local_add(struct prefix_sg *sg);
+void pim_msdp_sa_local_del(struct prefix_sg *sg);
+void pim_msdp_i_am_rp_changed(void);
+bool pim_msdp_peer_rpf_check(struct pim_msdp_peer *mp, struct in_addr rp);
 #endif
diff --git a/pimd/pim_msdp_packet.c b/pimd/pim_msdp_packet.c
index 458d5e462..d6b476571 100644
--- a/pimd/pim_msdp_packet.c
+++ b/pimd/pim_msdp_packet.c
@@ -23,6 +23,7 @@
 #include <lib/network.h>
 #include <lib/stream.h>
 #include <lib/thread.h>
+#include <lib/vty.h>
 
 #include "pimd.h"
 #include "pim_str.h"
@@ -129,6 +130,12 @@ pim_msdp_pkt_delete(struct pim_msdp_peer *mp)
 }
 
 static void
+pim_msdp_pkt_add(struct pim_msdp_peer *mp, struct stream *s)
+{
+  stream_fifo_push(mp->obuf, s);
+}
+
+static void
 pim_msdp_write_proceed_actions(struct pim_msdp_peer *mp)
 {
   if (stream_fifo_head(mp->obuf)) {
@@ -194,10 +201,17 @@ pim_msdp_write(struct thread *thread)
     if (num != writenum) {
       /* Partial write */
       stream_forward_getp(s, num);
+      if (PIM_DEBUG_MSDP_INTERNAL) {
+        char key_str[PIM_MSDP_PEER_KEY_STRLEN];
+
+        pim_msdp_peer_key_dump(mp, key_str, sizeof(key_str), false);
+        zlog_debug("%s pim_msdp_partial_write", key_str);
+      }
       break;
     }
 
     /* Retrieve msdp packet type. */
+    stream_set_getp(s,0);
     type = stream_getc(s);
     switch (type)
     {
@@ -230,23 +244,20 @@ static void
 pim_msdp_pkt_send(struct pim_msdp_peer *mp, struct stream *s)
 {
   /* Add packet to the end of list. */
-  stream_fifo_push(mp->obuf, s);
+  pim_msdp_pkt_add(mp, s);
 
   PIM_MSDP_PEER_WRITE_ON(mp);
 }
 
-/* Make keepalive packet and send it to the peer
- 0                   1                   2                   3
- 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-|       4      |              3                 |
-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-*/
 void
 pim_msdp_pkt_ka_tx(struct pim_msdp_peer *mp)
 {
   struct stream *s;
 
+  if (mp->state != PIM_MSDP_ESTABLISHED) {
+    /* don't tx anything unless a session is established */
+    return;
+  }
   s = stream_new(PIM_MSDP_KA_TLV_MAX_SIZE);
   stream_putc(s, PIM_MSDP_KEEPALIVE);
   stream_putw(s, PIM_MSDP_KA_TLV_MAX_SIZE);
@@ -255,6 +266,142 @@ pim_msdp_pkt_ka_tx(struct pim_msdp_peer *mp)
 }
 
 static void
+pim_msdp_pkt_sa_push_to_one_peer(struct pim_msdp_peer *mp)
+{
+  struct stream *s;
+
+  if (mp->state != PIM_MSDP_ESTABLISHED) {
+    /* don't tx anything unless a session is established */
+    return;
+  }
+  s = stream_dup(msdp->work_obuf);
+  if (s) {
+    pim_msdp_pkt_send(mp, s);
+    mp->flags |= PIM_MSDP_PEERF_SA_JUST_SENT;
+  }
+}
+
+/* push the stream into the obuf fifo of all the peers */
+static void
+pim_msdp_pkt_sa_push(struct pim_msdp_peer *mp)
+{
+  struct listnode *mpnode;
+
+  if (mp) {
+    pim_msdp_pkt_sa_push_to_one_peer(mp);
+  } else {
+    for (ALL_LIST_ELEMENTS_RO(msdp->peer_list, mpnode, mp)) {
+      if (PIM_DEBUG_MSDP_INTERNAL) {
+        char key_str[PIM_MSDP_PEER_KEY_STRLEN];
+
+        pim_msdp_peer_key_dump(mp, key_str, sizeof(key_str), false);
+        zlog_debug("%s pim_msdp_pkt_sa_push", key_str);
+      }
+      pim_msdp_pkt_sa_push_to_one_peer(mp);
+    }
+  }
+}
+
+static int
+pim_msdp_pkt_sa_fill_hdr(int local_cnt)
+{
+  int curr_tlv_ecnt;
+
+  stream_reset(msdp->work_obuf);
+  curr_tlv_ecnt = local_cnt>PIM_MSDP_SA_MAX_ENTRY_CNT?PIM_MSDP_SA_MAX_ENTRY_CNT:local_cnt;
+  local_cnt -= curr_tlv_ecnt;
+  stream_putc(msdp->work_obuf, PIM_MSDP_V4_SOURCE_ACTIVE);
+  stream_putw(msdp->work_obuf, PIM_MSDP_SA_ENTRY_CNT2SIZE(curr_tlv_ecnt));
+  stream_putc(msdp->work_obuf, curr_tlv_ecnt);
+  stream_put_ipv4(msdp->work_obuf, msdp->originator_id.s_addr);
+
+  return local_cnt;
+}
+
+static void
+pim_msdp_pkt_sa_fill_one(struct pim_msdp_sa *sa)
+{
+  stream_put3(msdp->work_obuf, 0 /* reserved */);
+  stream_putc(msdp->work_obuf, 32 /* sprefix len */);
+  stream_put_ipv4(msdp->work_obuf, sa->sg.grp.s_addr);
+  stream_put_ipv4(msdp->work_obuf, sa->sg.src.s_addr);
+}
+
+static void
+pim_msdp_pkt_sa_gen(struct pim_msdp_peer *mp)
+{
+  struct listnode *sanode;
+  struct pim_msdp_sa *sa;
+  int sa_count;
+  int local_cnt = msdp->local_cnt;
+
+  sa_count = 0;
+  local_cnt = pim_msdp_pkt_sa_fill_hdr(local_cnt);
+
+  for (ALL_LIST_ELEMENTS_RO(msdp->sa_list, sanode, sa)) {
+    if (!(sa->flags & PIM_MSDP_SAF_LOCAL)) {
+      /* current implementation of MSDP is for anycast i.e. full mesh. so
+       * no re-forwarding of SAs that we learnt from other peers */
+      continue;
+    }
+    /* add sa into scratch pad */
+    pim_msdp_pkt_sa_fill_one(sa);
+    ++sa_count;
+    if (sa_count >= PIM_MSDP_SA_MAX_ENTRY_CNT) {
+      pim_msdp_pkt_sa_push(mp);
+      /* reset headers */
+      sa_count = 0;
+      local_cnt = pim_msdp_pkt_sa_fill_hdr(local_cnt);
+    }
+  }
+
+  if (sa_count) {
+    pim_msdp_pkt_sa_push(mp);
+  }
+  return;
+}
+
+static void
+pim_msdp_pkt_sa_tx_done(void)
+{
+  struct listnode *mpnode;
+  struct pim_msdp_peer *mp;
+
+  /* if SA were sent to the peers we restart ka timer and avoid
+   * unnecessary ka noise */
+  for (ALL_LIST_ELEMENTS_RO(msdp->peer_list, mpnode, mp)) {
+    if (mp->flags & PIM_MSDP_PEERF_SA_JUST_SENT) {
+      mp->flags &= ~PIM_MSDP_PEERF_SA_JUST_SENT;
+      pim_msdp_peer_pkt_txed(mp);
+    }
+  }
+}
+
+void
+pim_msdp_pkt_sa_tx(void)
+{
+  pim_msdp_pkt_sa_gen(NULL /* mp */);
+  pim_msdp_pkt_sa_tx_done();
+}
+
+void
+pim_msdp_pkt_sa_tx_one(struct pim_msdp_sa *sa)
+{
+  pim_msdp_pkt_sa_fill_hdr(1 /* cnt */);
+  pim_msdp_pkt_sa_fill_one(sa);
+  pim_msdp_pkt_sa_push(NULL);
+  pim_msdp_pkt_sa_tx_done();
+}
+
+/* when a connection is first established we push all SAs immediately */
+void
+pim_msdp_pkt_sa_tx_to_one_peer(struct pim_msdp_peer *mp)
+{
+  pim_msdp_pkt_sa_gen(mp);
+  pim_msdp_pkt_sa_tx_done();
+}
+
+static void
 pim_msdp_pkt_rxed_with_fatal_error(struct pim_msdp_peer *mp)
 {
   /* XXX:revisit; reset TCP connection */
@@ -273,23 +420,85 @@ pim_msdp_pkt_ka_rx(struct pim_msdp_peer *mp, int len)
 }
 
 static void
+pim_msdp_pkt_sa_rx_one(struct pim_msdp_peer *mp, struct in_addr rp)
+{
+  int prefix_len;
+  struct prefix_sg sg;
+
+  /* just throw away the three reserved bytes */
+  stream_get3(mp->ibuf);
+  prefix_len = stream_getc(mp->ibuf);
+
+  memset(&sg, 0, sizeof (struct prefix_sg));
+  sg.grp.s_addr = stream_get_ipv4(mp->ibuf);
+  sg.src.s_addr = stream_get_ipv4(mp->ibuf);
+
+  if (prefix_len != 32) {
+    /* ignore SA update if the prefix length is not 32 */
+    zlog_err("rxed sa update with invalid prefix length %d", prefix_len);
+    return;
+  }
+  if (PIM_DEBUG_MSDP_PACKETS) {
+    zlog_debug("  sg %s", pim_str_sg_dump(&sg));
+  }
+  pim_msdp_sa_ref(mp, &sg, rp);
+}
+
+static void
 pim_msdp_pkt_sa_rx(struct pim_msdp_peer *mp, int len)
 {
+  int entry_cnt;
+  int i;
+  struct in_addr rp; /* Last RP address associated with this SA */
+
   mp->sa_rx_cnt++;
-  /* XXX: proc SA ... */
+
+  if (len <  PIM_MSDP_SA_TLV_MIN_SIZE) {
+    pim_msdp_pkt_rxed_with_fatal_error(mp);
+    return;
+  }
+
+  entry_cnt = stream_getc(mp->ibuf);
+  /* some vendors include the actual multicast data in the tlv (at the end).
+   * we will ignore such data. in the future we may consider pushing it down
+   * the RPT */
+  if (len < PIM_MSDP_SA_ENTRY_CNT2SIZE(entry_cnt)) {
+    pim_msdp_pkt_rxed_with_fatal_error(mp);
+    return;
+  }
+  rp.s_addr = stream_get_ipv4(mp->ibuf);
+
+  if (PIM_DEBUG_MSDP_PACKETS) {
+    char rp_str[INET_ADDRSTRLEN];
+    pim_inet4_dump("<rp?>", rp, rp_str, sizeof(rp_str));
+    zlog_debug("  entry_cnt %d rp %s", entry_cnt, rp_str);
+  }
+
+  if (!pim_msdp_peer_rpf_check(mp, rp)) {
+    /* if peer-RPF check fails don't process the packet any further */
+    if (PIM_DEBUG_MSDP_PACKETS) {
+      zlog_debug("  peer RPF check failed");
+    }
+    return;
+  }
+
   pim_msdp_peer_pkt_rxed(mp);
+
+  /* update SA cache */
+  for (i = 0; i < entry_cnt; ++i) {
+    pim_msdp_pkt_sa_rx_one(mp, rp);
+  }
 }
 
-/* Theoretically you could have different tlv types in the same message.
- * For the time being I am assuming one; will revisit before 3.2 - XXX */
 static void
-pim_msdp_pkt_rx(struct pim_msdp_peer *mp, int nbytes)
+pim_msdp_pkt_rx(struct pim_msdp_peer *mp)
 {
   enum pim_msdp_tlv type;
   int len;
 
-  type = stream_getc(mp->ibuf);
-  len = stream_getw(mp->ibuf);
+  /* re-read type and len */
+  type = stream_getc_from(mp->ibuf, 0);
+  len = stream_getw_from(mp->ibuf, 1);
   if (len <  PIM_MSDP_HEADER_SIZE) {
     pim_msdp_pkt_rxed_with_fatal_error(mp);
     return;
@@ -300,12 +509,6 @@ pim_msdp_pkt_rx(struct pim_msdp_peer *mp, int nbytes)
     return;
   }
 
-  if (len > nbytes) {
-    /* we got a partial read or the packet is malformed */
-    pim_msdp_pkt_rxed_with_fatal_error(mp);
-    return;
-  }
-
   if (PIM_DEBUG_MSDP_PACKETS) {
     pim_msdp_pkt_dump(mp, type, len, true /*rx*/);
   }
@@ -321,7 +524,6 @@ pim_msdp_pkt_rx(struct pim_msdp_peer *mp, int nbytes)
       default:
         mp->unk_rx_cnt++;
   }
-  /* XXX: process next tlv*/
 }
 
 /* pim msdp read utility function. */
@@ -329,9 +531,16 @@ static int
 pim_msdp_read_packet(struct pim_msdp_peer *mp)
 {
   int nbytes;
-  /* Read packet from fd. */
-  nbytes = stream_read_try(mp->ibuf, mp->fd, PIM_MSDP_MAX_PACKET_SIZE);
-  if (nbytes < PIM_MSDP_HEADER_SIZE) {
+  int readsize;
+
+  readsize = mp->packet_size - stream_get_endp(mp->ibuf);
+  if (!readsize) {
+    return 0;
+  }
+
+  /* Read packet from fd */
+  nbytes = stream_read_try(mp->ibuf, mp->fd, readsize);
+  if (nbytes < 0) {
     if (nbytes == -2) {
       /* transient error retry */
       return -1;
@@ -339,7 +548,17 @@ pim_msdp_read_packet(struct pim_msdp_peer *mp)
     pim_msdp_pkt_rxed_with_fatal_error(mp);
     return -1;
   }
-  return nbytes;
+
+  if (!nbytes) {
+    pim_msdp_peer_reset_tcp_conn(mp, "peer-down");
+    return -1;
+  }
+
+  /* We read partial packet. */
+  if (stream_get_endp(mp->ibuf) != mp->packet_size)
+    return -1;
+
+  return 0;
 }
 
 int
@@ -347,6 +566,7 @@ pim_msdp_read(struct thread *thread)
 {
   struct pim_msdp_peer *mp;
   int rc;
+  uint32_t len;
 
   mp = THREAD_ARG(thread);
   mp->t_read = NULL;
@@ -368,13 +588,41 @@ pim_msdp_read(struct thread *thread)
     return 0;
   }
 
-  THREAD_READ_ON(msdp->master, mp->t_read, pim_msdp_read, mp, mp->fd);
+  PIM_MSDP_PEER_READ_ON(mp);
+
+  if (!mp->packet_size) {
+    mp->packet_size = PIM_MSDP_HEADER_SIZE;
+  }
+
+  if (stream_get_endp(mp->ibuf) < PIM_MSDP_HEADER_SIZE) {
+    /* start by reading the TLV header */
+    rc = pim_msdp_read_packet(mp);
+    if (rc < 0) {
+      goto pim_msdp_read_end;
+    }
+
+    /* Find TLV type and len  */
+    stream_getc(mp->ibuf);
+    len = stream_getw(mp->ibuf);
+    if (len < PIM_MSDP_HEADER_SIZE) {
+      pim_msdp_pkt_rxed_with_fatal_error(mp);
+      goto pim_msdp_read_end;
+    }
+    /* read complete TLV */
+    mp->packet_size = len;
+  }
 
   rc = pim_msdp_read_packet(mp);
-  if (rc > 0) {
-    pim_msdp_pkt_rx(mp, rc);
+  if (rc < 0) {
+    goto pim_msdp_read_end;
   }
 
+  pim_msdp_pkt_rx(mp);
+
+  /* reset input buffers and get ready for the next packet */
+  mp->packet_size = 0;
   stream_reset(mp->ibuf);
+
+pim_msdp_read_end:
   return 0;
 }
diff --git a/pimd/pim_msdp_packet.h b/pimd/pim_msdp_packet.h
index 7f9ed9f68..30221a399 100644
--- a/pimd/pim_msdp_packet.h
+++ b/pimd/pim_msdp_packet.h
@@ -22,8 +22,41 @@
 
 /* type and length of a single tlv can be consider packet header */
 #define PIM_MSDP_HEADER_SIZE 3
-#define PIM_MSDP_SA_TLV_MAX_SIZE 9192
+
+/* Keepalive TLV
+ 0                   1                   2                   3
+ 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+|       4      |              3                 |
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+*/
 #define PIM_MSDP_KA_TLV_MAX_SIZE PIM_MSDP_HEADER_SIZE
+
+/* Source-Active TLV (x=8, y=12xEntryCount)
+ 0 1 2 3
+ 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+|       1       |           x + y               |  Entry Count  |
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+|                         RP Address                            |
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+|                          Reserved             |  Sprefix Len  | \
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+  \
+|                        Group Address                          |   ) z
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+  /
+|                        Source Address                         | /
++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+*/
+#define PIM_MSDP_SA_TLV_MAX_SIZE 9192
+#define PIM_MSDP_SA_X_SIZE 8
+#define PIM_MSDP_SA_ONE_ENTRY_SIZE 12
+#define PIM_MSDP_SA_Y_SIZE(entry_cnt) (PIM_MSDP_SA_ONE_ENTRY_SIZE * entry_cnt)
+#define PIM_MSDP_SA_ENTRY_CNT2SIZE(entry_cnt) (PIM_MSDP_SA_X_SIZE +\
+                                    PIM_MSDP_SA_Y_SIZE(entry_cnt))
+/* SA TLV has to have atleast only one entry in it so x=8 + y=12 */
+#define PIM_MSDP_SA_TLV_MIN_SIZE PIM_MSDP_SA_ENTRY_CNT2SIZE(1)
+#define PIM_MSDP_SA_MAX_ENTRY_CNT ((PIM_MSDP_SA_TLV_MAX_SIZE - PIM_MSDP_SA_X_SIZE)/PIM_MSDP_SA_ONE_ENTRY_SIZE)
+
 /* XXX: this is just a guesstimate - need to revist */
 #define PIM_MSDP_MAX_PACKET_SIZE (PIM_MSDP_SA_TLV_MAX_SIZE + PIM_MSDP_KA_TLV_MAX_SIZE)
 
@@ -31,4 +64,8 @@
 
 void pim_msdp_pkt_ka_tx(struct pim_msdp_peer *mp);
 int pim_msdp_read(struct thread *thread);
+void pim_msdp_pkt_sa_tx(void);
+void pim_msdp_pkt_sa_tx_one(struct pim_msdp_sa *sa);
+void pim_msdp_pkt_sa_tx_to_one_peer(struct pim_msdp_peer *mp);
+
 #endif
diff --git a/pimd/pim_msdp_socket.c b/pimd/pim_msdp_socket.c
index fdb77c530..f71d23e4a 100644
--- a/pimd/pim_msdp_socket.c
+++ b/pimd/pim_msdp_socket.c
@@ -22,8 +22,9 @@
 
 #include <lib/log.h>
 #include <lib/network.h>
-#include <lib/thread.h>
 #include <lib/sockunion.h>
+#include <lib/thread.h>
+#include <lib/vty.h>
 
 #include "pimd.h"
 
@@ -149,11 +150,11 @@ pim_msdp_sock_listen(void)
         safe_strerror (errno));
   }
 
-  /* bond to well known TCP port */
+  /* bind to well known TCP port */
   rc = bind(sock, (struct sockaddr *)&sin, socklen);
 
   if (pimd_privs.change(ZPRIVS_LOWER)) {
-    zlog_err ("pim_msdp_socket: could not raise privs, %s",
+    zlog_err ("pim_msdp_socket: could not lower privs, %s",
         safe_strerror (errno));
   }
 
diff --git a/pimd/pim_register.c b/pimd/pim_register.c
index 0c6a759dc..017d21b1b 100644
--- a/pimd/pim_register.c
+++ b/pimd/pim_register.c
@@ -343,7 +343,7 @@ pim_register_recv (struct interface *ifp,
             zlog_warn ("Failure to create upstream state");
             return 1;
           }
-	PIM_UPSTREAM_FLAG_SET_CREATED_BY_UPSTREAM(upstream->flags);
+        pim_upstream_set_created_by_upstream(upstream);
 
         upstream->upstream_register = src_addr;
 	pim_rp_set_upstream_addr (&upstream->upstream_addr, sg.src, sg.grp);
diff --git a/pimd/pim_upstream.c b/pimd/pim_upstream.c
index 84503eb3e..3cfe0887e 100644
--- a/pimd/pim_upstream.c
+++ b/pimd/pim_upstream.c
@@ -51,6 +51,7 @@
 #include "pim_rp.h"
 #include "pim_br.h"
 #include "pim_register.h"
+#include "pim_msdp.h"
 
 struct hash *pim_upstream_hash = NULL;
 struct list *pim_upstream_list = NULL;
@@ -111,6 +112,20 @@ pim_upstream_find_new_children (struct pim_upstream *up)
     }
 }
 
+void
+pim_upstream_set_created_by_upstream(struct pim_upstream *up)
+{
+  PIM_UPSTREAM_FLAG_SET_CREATED_BY_UPSTREAM(up->flags);
+  pim_msdp_sa_local_add(&up->sg);
+}
+
+static void
+pim_upstream_unset_created_by_upstream(struct pim_upstream *up)
+{
+  PIM_UPSTREAM_FLAG_UNSET_CREATED_BY_UPSTREAM(up->flags);
+  pim_msdp_sa_local_del(&up->sg);
+}
+
 /*
  * If we have a (*,*) || (S,*) there is no parent
  * If we have a (S,G), find the (*,G)
@@ -175,6 +190,7 @@ pim_upstream_del(struct pim_upstream *up, const char *name)
   if (up->sg.src.s_addr != INADDR_ANY)
     wheel_remove_item (pim_upstream_sg_wheel, up);
 
+  pim_msdp_sa_local_del(&up->sg);
   pim_upstream_remove_children (up);
   pim_mroute_del (up->channel_oil);
   upstream_channel_oil_detach(up);
@@ -913,7 +929,7 @@ pim_upstream_keep_alive_timer (struct thread *t)
       PIM_UPSTREAM_FLAG_UNSET_SRC_STREAM (up->flags);
       if (PIM_UPSTREAM_FLAG_TEST_CREATED_BY_UPSTREAM(up->flags))
 	{
-	  PIM_UPSTREAM_FLAG_UNSET_CREATED_BY_UPSTREAM(up->flags);
+    pim_upstream_unset_created_by_upstream(up);
 	  pim_upstream_del (up, __PRETTY_FUNCTION__);
 	}
     }
diff --git a/pimd/pim_upstream.h b/pimd/pim_upstream.h
index a91a9ae37..7f8c0c99c 100644
--- a/pimd/pim_upstream.h
+++ b/pimd/pim_upstream.h
@@ -163,4 +163,5 @@ void pim_upstream_find_new_rpf (void);
 
 void pim_upstream_init (void);
 void pim_upstream_terminate (void);
+void pim_upstream_set_created_by_upstream(struct pim_upstream *up);
 #endif /* PIM_UPSTREAM_H */
diff --git a/pimd/pim_vty.c b/pimd/pim_vty.c
index c9dca5333..d336051f2 100644
--- a/pimd/pim_vty.c
+++ b/pimd/pim_vty.c
@@ -38,6 +38,7 @@
 #include "pim_oil.h"
 #include "pim_static.h"
 #include "pim_rp.h"
+#include "pim_msdp.h"
 
 int
 pim_debug_config_write (struct vty *vty)
@@ -142,6 +143,8 @@ int pim_global_config_write(struct vty *vty)
 {
   int writes = 0;
 
+  writes += pim_msdp_config_write (vty);
+
   if (PIM_MROUTE_IS_ENABLED) {
     vty_out(vty, "ip multicast-routing%s", VTY_NEWLINE);
     ++writes;