pimd: Add AutoRP functionality to PIMD

Perform AutoRP discovery and candidate RP announcements using the
AutoRP protocol.
Mapping agent is not yet implemented, but this feature is not
necessary for FRR to support AutoRP as we only need one AutoRP
mapping agent in the network.

Signed-off-by: Nathan Bahr <nbahr@atcorp.com>

11 files changed, 1374 insertions, 6 deletions

diff --git a/pimd/pim_autorp.c b/pimd/pim_autorp.c
new file mode 100644
index 000000000..cf0b9d555
--- /dev/null
+++ b/pimd/pim_autorp.c
@@ -0,0 +1,1147 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * pim_autorp.c: PIM AutoRP handling routines
+ *
+ * Copyright (C) 2024 ATCorp
+ * Nathan Bahr
+ */
+
+#include <zebra.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include "lib/plist.h"
+#include "lib/plist_int.h"
+#include "lib/sockopt.h"
+#include "lib/network.h"
+#include "lib/termtable.h"
+#include "lib/json.h"
+
+#include "pimd.h"
+#include "pim_iface.h"
+#include "pim_rp.h"
+#include "pim_sock.h"
+#include "pim_instance.h"
+#include "pim_autorp.h"
+
+DEFINE_MTYPE_STATIC(PIMD, PIM_AUTORP, "PIM AutoRP info");
+DEFINE_MTYPE_STATIC(PIMD, PIM_AUTORP_RP, "PIM AutoRP advertised RP info");
+DEFINE_MTYPE_STATIC(PIMD, PIM_AUTORP_CRP, "PIM AutoRP candidate RP info");
+DEFINE_MTYPE_STATIC(PIMD, PIM_AUTORP_ANNOUNCE, "PIM AutoRP announcement packet");
+
+static const char *PIM_AUTORP_ANNOUNCEMENT_GRP = "224.0.1.39";
+static const char *PIM_AUTORP_DISCOVERY_GRP = "224.0.1.40";
+static const in_port_t PIM_AUTORP_PORT = 496;
+
+static int pim_autorp_rp_cmp(const struct pim_autorp_rp *l,
+			     const struct pim_autorp_rp *r)
+{
+	return pim_addr_cmp(l->addr, r->addr);
+}
+
+DECLARE_SORTLIST_UNIQ(pim_autorp_rp, struct pim_autorp_rp, list,
+		      pim_autorp_rp_cmp);
+
+static void pim_autorp_rp_free(struct pim_autorp_rp *rp)
+{
+	event_cancel(&rp->hold_timer);
+
+	/* Clean up installed RP info */
+	if (pim_rp_del(rp->autorp->pim, rp->addr, rp->grp,
+		       (strlen(rp->grplist) ? rp->grplist : NULL),
+		       RP_SRC_AUTORP))
+		if (PIM_DEBUG_AUTORP)
+			zlog_err("%s: Failed to delete RP %pI4", __func__,
+				 &rp->addr);
+
+	XFREE(MTYPE_PIM_AUTORP_RP, rp);
+}
+
+static void pim_autorp_rplist_free(struct pim_autorp_rp_head *head)
+{
+	struct pim_autorp_rp *rp;
+
+	while ((rp = pim_autorp_rp_pop(head)))
+		pim_autorp_rp_free(rp);
+}
+
+static void pim_autorp_rplist_cfree(struct pim_autorp_rp_head *head)
+{
+	struct pim_autorp_rp *rp;
+
+	while ((rp = pim_autorp_rp_pop(head)))
+		XFREE(MTYPE_PIM_AUTORP_CRP, rp);
+}
+
+static void pim_autorp_free(struct pim_autorp *autorp)
+{
+	pim_autorp_rplist_free(&(autorp->discovery_rp_list));
+	pim_autorp_rp_fini(&(autorp->discovery_rp_list));
+
+	pim_autorp_rplist_cfree(&(autorp->candidate_rp_list));
+	pim_autorp_rp_fini(&(autorp->candidate_rp_list));
+}
+
+static bool pim_autorp_join_groups(struct interface *ifp)
+{
+	struct pim_interface *pim_ifp;
+	struct pim_instance *pim;
+	struct pim_autorp *autorp;
+	pim_addr grp;
+
+	pim_ifp = ifp->info;
+	pim = pim_ifp->pim;
+	autorp = pim->autorp;
+
+	inet_pton(PIM_AF, PIM_AUTORP_DISCOVERY_GRP, &grp);
+	if (pim_socket_join(autorp->sock, grp, pim_ifp->primary_address,
+			    ifp->ifindex, pim_ifp)) {
+		zlog_err("Failed to join group %pI4 on interface %s", &grp,
+			 ifp->name);
+		return false;
+	}
+
+	/* TODO: Future Mapping agent implementation
+	 *  Join announcement group for AutoRP mapping agent
+	 * inet_pton(PIM_AF, PIM_AUTORP_ANNOUNCEMENT_GRP, &grp);
+	 * if (pim_socket_join(pim->autorp->sock, grp,
+	 *              pim_ifp->primary_address,
+	 *              ifp->ifindex, pim_ifp)) {
+	 *  zlog_err("Failed to join group %pI4 on interface %s",
+	 *          &grp, ifp->name);
+	 *  return errno;
+	 * }
+	 */
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: Joined AutoRP groups on interface %s", __func__,
+			   ifp->name);
+
+	return true;
+}
+
+static bool pim_autorp_leave_groups(struct interface *ifp)
+{
+	struct pim_interface *pim_ifp;
+	struct pim_instance *pim;
+	struct pim_autorp *autorp;
+	pim_addr grp;
+
+	pim_ifp = ifp->info;
+	pim = pim_ifp->pim;
+	autorp = pim->autorp;
+
+	inet_pton(PIM_AF, PIM_AUTORP_DISCOVERY_GRP, &grp);
+	if (pim_socket_leave(autorp->sock, grp, pim_ifp->primary_address,
+			     ifp->ifindex, pim_ifp)) {
+		zlog_err("Failed to leave group %pI4 on interface %s", &grp,
+			 ifp->name);
+		return false;
+	}
+
+	/* TODO: Future Mapping agent implementation
+	 *  Leave announcement group for AutoRP mapping agent
+	 * inet_pton(PIM_AF, PIM_AUTORP_ANNOUNCEMENT_GRP, &grp);
+	 * if (pim_socket_leave(pim->autorp->sock, grp,
+	 *              pim_ifp->primary_address,
+	 *              ifp->ifindex, pim_ifp)) {
+	 *  zlog_err("Failed to leave group %pI4 on interface %s",
+	 *           &grp, ifp->name);
+	 *  return errno;
+	 * }
+	 */
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: Left AutoRP groups on interface %s", __func__,
+			   ifp->name);
+
+	return true;
+}
+
+static bool pim_autorp_setup(struct pim_autorp *autorp)
+{
+#if defined(HAVE_IP_PKTINFO)
+	int data;
+	socklen_t data_len = sizeof(data);
+#endif
+
+	struct sockaddr_in autorp_addr = { .sin_family = AF_INET,
+					   .sin_addr = { .s_addr = INADDR_ANY },
+					   .sin_port = htons(PIM_AUTORP_PORT) };
+
+	setsockopt_so_recvbuf(autorp->sock, 1024 * 1024 * 8);
+
+#if defined(HAVE_IP_PKTINFO)
+	/* Linux and Solaris IP_PKTINFO */
+	data = 1;
+	if (setsockopt(autorp->sock, PIM_IPPROTO, IP_PKTINFO, &data, data_len)) {
+		zlog_err("Could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
+			 autorp->sock, errno, safe_strerror(errno));
+		return false;
+	}
+#endif
+
+	if (set_nonblocking(autorp->sock) < 0) {
+		zlog_err("Could not set non blocking on socket fd=%d: errno=%d: %s",
+			 autorp->sock, errno, safe_strerror(errno));
+		return false;
+	}
+
+	if (sockopt_reuseaddr(autorp->sock)) {
+		zlog_err("Could not set reuse addr on socket fd=%d: errno=%d: %s",
+			 autorp->sock, errno, safe_strerror(errno));
+		return false;
+	}
+
+	if (bind(autorp->sock, (const struct sockaddr *)&autorp_addr,
+		 sizeof(autorp_addr)) < 0) {
+		zlog_err("Could not bind socket: %pSUp, fd=%d, errno=%d, %s",
+			 (union sockunion *)&autorp_addr, autorp->sock, errno,
+			 safe_strerror(errno));
+		return false;
+	}
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP finished setup", __func__);
+
+	return true;
+}
+
+static bool pim_autorp_announcement(struct pim_autorp *autorp, uint8_t rpcnt,
+				    uint16_t holdtime, char *buf,
+				    size_t buf_size)
+{
+	/* TODO: Future Mapping agent implementation
+	 *  Implement AutoRP mapping agent logic using received announcement messages
+	 */
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP processed announcement message",
+			   __func__);
+	return true;
+}
+
+static void autorp_rp_holdtime(struct event *evt)
+{
+	/* RP hold time expired, remove the RP */
+	struct pim_autorp_rp *rp = EVENT_ARG(evt);
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP hold time expired, RP removed: addr=%pI4, grp=%pFX, grplist=%s",
+			   __func__, &rp->addr, &rp->grp,
+			   (strlen(rp->grplist) ? rp->grplist : "NONE"));
+
+	pim_autorp_rp_del(&(rp->autorp->discovery_rp_list), rp);
+	pim_autorp_rp_free(rp);
+}
+
+static bool pim_autorp_add_rp(struct pim_autorp *autorp, pim_addr rpaddr,
+			      struct prefix grp, char *listname,
+			      uint16_t holdtime)
+{
+	struct pim_autorp_rp *rp;
+	struct pim_autorp_rp *trp = NULL;
+
+	if (pim_rp_new(autorp->pim, rpaddr, grp, listname, RP_SRC_AUTORP)) {
+		zlog_err("%s: Failed to add new RP addr=%pI4, grp=%pFX, grplist=%s",
+			 __func__, &rpaddr, &grp,
+			 (listname ? listname : "NONE"));
+		return false;
+	}
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: Added new AutoRP learned RP addr=%pI4, grp=%pFX, grplist=%s",
+			   __func__, &rpaddr, &grp,
+			   (listname ? listname : "NONE"));
+
+	rp = XCALLOC(MTYPE_PIM_AUTORP_RP, sizeof(*rp));
+	rp->autorp = autorp;
+	memcpy(&(rp->addr), &rpaddr, sizeof(pim_addr));
+	prefix_copy(&(rp->grp), &grp);
+	if (listname)
+		snprintf(rp->grplist, sizeof(rp->grplist), "%s", listname);
+	else
+		rp->grplist[0] = '\0';
+
+	rp->holdtime = holdtime;
+	rp->hold_timer = NULL;
+	trp = pim_autorp_rp_add(&(autorp->discovery_rp_list), rp);
+	if (trp == NULL) {
+		/* RP was brand new */
+		trp = pim_autorp_rp_find(&(autorp->discovery_rp_list),
+					 (const struct pim_autorp_rp *)rp);
+	} else {
+		/* RP already existed */
+		XFREE(MTYPE_PIM_AUTORP_RP, rp);
+		event_cancel(&trp->hold_timer);
+
+		/* We know the address matches, but these values may have changed */
+		trp->holdtime = holdtime;
+		prefix_copy(&(trp->grp), &grp);
+		if (listname) {
+			snprintf(trp->grplist, sizeof(trp->grplist), "%s",
+				 listname);
+		} else {
+			trp->grplist[0] = '\0';
+		}
+	}
+
+	if (holdtime > 0) {
+		event_add_timer(router->master, autorp_rp_holdtime, trp,
+				holdtime, &(trp->hold_timer));
+		if (PIM_DEBUG_AUTORP)
+			zlog_debug("%s: Started %u second hold timer for RP %pI4",
+				   __func__, holdtime, &rp->addr);
+	} else {
+		/* If hold time is zero, make sure there doesn't exist a hold timer for it already */
+		event_cancel(&trp->hold_timer);
+	}
+
+	return true;
+}
+
+static bool pim_autorp_discovery(struct pim_autorp *autorp, uint8_t rpcnt,
+				 uint16_t holdtime, char *buf, size_t buf_size)
+{
+	int i, j;
+	struct autorp_pkt_rp *rp;
+	struct autorp_pkt_grp *grp;
+	size_t offset = 0;
+	pim_addr rp_addr;
+	struct prefix grppfix;
+	char plname[32];
+	struct prefix_list *pl;
+	struct prefix_list_entry *ple;
+	int64_t seq = 1;
+	bool success = true;
+
+	for (i = 0; i < rpcnt; ++i) {
+		if ((buf_size - offset) < AUTORP_RPLEN)
+			return false;
+
+		rp = (struct autorp_pkt_rp *)(buf + offset);
+		offset += AUTORP_RPLEN;
+
+		rp_addr.s_addr = rp->addr;
+
+		/* Ignore RP's limited to PIM version 1 or with an unknown version */
+		if (rp->pimver == PIM_V1 || rp->pimver == PIM_VUNKNOWN) {
+			zlog_warn("%s: Ignoring unsupported PIM version in AutoRP Discovery for RP %pI4",
+				  __func__, (in_addr_t *)&(rp->addr));
+			/* Update the offset to skip past the groups advertised for this RP */
+			offset += (AUTORP_GRPLEN * rp->grpcnt);
+			continue;
+		}
+
+
+		if (rp->grpcnt == 0) {
+			/* No groups?? */
+			zlog_warn("%s: Discovery message has no groups for RP %pI4",
+				  __func__, (in_addr_t *)&(rp->addr));
+			continue;
+		}
+
+		if ((buf_size - offset) < AUTORP_GRPLEN) {
+			zlog_warn("%s: Buffer underrun parsing groups for RP %pI4",
+				  __func__, (in_addr_t *)&(rp->addr));
+			return false;
+		}
+
+		grp = (struct autorp_pkt_grp *)(buf + offset);
+		offset += AUTORP_GRPLEN;
+
+		if (rp->grpcnt == 1 && grp->negprefix == 0) {
+			/* Only one group with positive prefix, we can use the standard RP API */
+			grppfix.family = AF_INET;
+			grppfix.prefixlen = grp->masklen;
+			grppfix.u.prefix4.s_addr = grp->addr;
+			if (!pim_autorp_add_rp(autorp, rp_addr, grppfix, NULL,
+					       holdtime))
+				success = false;
+		} else {
+			/* More than one grp, or the only group is a negative prefix, need to make a prefix list for this RP */
+			snprintfrr(plname, sizeof(plname), "__AUTORP_%pI4__",
+				   &rp_addr);
+			pl = prefix_list_get(AFI_IP, 0, plname);
+
+			for (j = 0; j < rp->grpcnt; ++j) {
+				/* grp is already pointing at the first group in the buffer */
+				ple = prefix_list_entry_new();
+				ple->pl = pl;
+				ple->seq = seq;
+				seq += 5;
+				memset(&ple->prefix, 0, sizeof(ple->prefix));
+				prefix_list_entry_update_start(ple);
+				ple->type = (grp->negprefix ? PREFIX_DENY
+							    : PREFIX_PERMIT);
+				ple->prefix.family = AF_INET;
+				ple->prefix.prefixlen = grp->masklen;
+				ple->prefix.u.prefix4.s_addr = grp->addr;
+				ple->any = false;
+				ple->ge = 0;
+				ple->le = 32;
+				prefix_list_entry_update_finish(ple);
+
+				if ((buf_size - offset) < AUTORP_GRPLEN)
+					return false;
+
+				grp = (struct autorp_pkt_grp *)(buf + offset);
+				offset += AUTORP_GRPLEN;
+			}
+
+			if (!pim_autorp_add_rp(autorp, rp_addr, grppfix, plname,
+					       holdtime))
+				success = false;
+		}
+	}
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: Processed AutoRP Discovery message", __func__);
+
+	return success;
+}
+
+static bool pim_autorp_msg(struct pim_autorp *autorp, char *buf, size_t buf_size)
+{
+	struct autorp_pkt_hdr *h;
+
+	if (buf_size < AUTORP_HDRLEN)
+		return false;
+
+	h = (struct autorp_pkt_hdr *)buf;
+
+	if (h->version != AUTORP_VERSION)
+		return false;
+
+	if (h->type == AUTORP_ANNOUNCEMENT_TYPE &&
+	    !pim_autorp_announcement(autorp, h->rpcnt, htons(h->holdtime),
+				     buf + AUTORP_HDRLEN,
+				     buf_size - AUTORP_HDRLEN))
+		return false;
+
+	if (h->type == AUTORP_DISCOVERY_TYPE &&
+	    !pim_autorp_discovery(autorp, h->rpcnt, htons(h->holdtime),
+				  buf + AUTORP_HDRLEN, buf_size - AUTORP_HDRLEN))
+		return false;
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: Processed AutoRP packet", __func__);
+
+	return true;
+}
+
+static void autorp_read(struct event *t);
+
+static void autorp_read_on(struct pim_autorp *autorp)
+{
+	event_add_read(router->master, autorp_read, autorp, autorp->sock,
+		       &(autorp->read_event));
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP socket read enabled", __func__);
+}
+
+static void autorp_read_off(struct pim_autorp *autorp)
+{
+	event_cancel(&(autorp->read_event));
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP socket read disabled", __func__);
+}
+
+static void autorp_read(struct event *evt)
+{
+	struct pim_autorp *autorp = evt->arg;
+	int fd = evt->u.fd;
+	char buf[10000];
+	int rd;
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: Reading from AutoRP socket", __func__);
+
+	while (1) {
+		rd = pim_socket_recvfromto(fd, (uint8_t *)buf, sizeof(buf),
+					   NULL, NULL, NULL, NULL, NULL);
+		if (rd <= 0) {
+			if (errno == EINTR)
+				continue;
+			if (errno == EWOULDBLOCK || errno == EAGAIN)
+				break;
+
+			zlog_warn("%s: Failure reading rd=%d: fd=%d: errno=%d: %s",
+				  __func__, rd, fd, errno, safe_strerror(errno));
+			goto err;
+		}
+
+		if (!pim_autorp_msg(autorp, buf, rd))
+			zlog_err("%s: Failure parsing AutoRP message", __func__);
+		/* Keep reading until would block */
+	}
+
+	/* No error, enable read again */
+	autorp_read_on(autorp);
+
+err:
+	return;
+}
+
+static bool pim_autorp_socket_enable(struct pim_autorp *autorp)
+{
+	int fd;
+
+	frr_with_privs (&pimd_privs) {
+		fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
+		if (fd < 0) {
+			zlog_warn("Could not create autorp socket: errno=%d: %s",
+				  errno, safe_strerror(errno));
+			return false;
+		}
+
+		autorp->sock = fd;
+		if (!pim_autorp_setup(autorp)) {
+			zlog_warn("Could not setup autorp socket fd=%d: errno=%d: %s",
+				  fd, errno, safe_strerror(errno));
+			close(fd);
+			autorp->sock = -1;
+			return false;
+		}
+	}
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP socket enabled", __func__);
+
+	return true;
+}
+
+static bool pim_autorp_socket_disable(struct pim_autorp *autorp)
+{
+	if (close(autorp->sock)) {
+		zlog_warn("Failure closing autorp socket: fd=%d errno=%d: %s",
+			  autorp->sock, errno, safe_strerror(errno));
+		return false;
+	}
+
+	autorp_read_off(autorp);
+	autorp->sock = -1;
+
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP socket disabled", __func__);
+
+	return true;
+}
+
+static void autorp_send_announcement(struct event *evt)
+{
+	struct pim_autorp *autorp = EVENT_ARG(evt);
+	struct interface *ifp;
+	struct pim_interface *pim_ifp;
+	struct sockaddr_in announceGrp;
+
+	announceGrp.sin_family = AF_INET;
+	announceGrp.sin_port = htons(PIM_AUTORP_PORT);
+	inet_pton(PIM_AF, PIM_AUTORP_ANNOUNCEMENT_GRP, &announceGrp.sin_addr);
+
+	if (autorp->annouce_pkt_sz >= MIN_AUTORP_PKT_SZ) {
+		setsockopt(autorp->sock, IPPROTO_IP, IP_MULTICAST_TTL,
+			   &(autorp->announce_scope),
+			   sizeof(autorp->announce_scope));
+
+		FOR_ALL_INTERFACES (autorp->pim->vrf, ifp) {
+			pim_ifp = ifp->info;
+			/* Only send on active interfaces with full pim enabled, non-passive
+			 * and have a primary address set.
+			 */
+			if (CHECK_FLAG(ifp->status, ZEBRA_INTERFACE_ACTIVE) &&
+			    pim_ifp && pim_ifp->pim_enable &&
+			    !pim_ifp->pim_passive_enable &&
+			    !pim_addr_is_any(pim_ifp->primary_address)) {
+				setsockopt(autorp->sock, IPPROTO_IP,
+					   IP_MULTICAST_IF,
+					   &(pim_ifp->primary_address),
+					   sizeof(pim_ifp->primary_address));
+				sendto(autorp->sock, autorp->annouce_pkt,
+				       autorp->annouce_pkt_sz, 0,
+				       (struct sockaddr *)&announceGrp,
+				       sizeof(announceGrp));
+			}
+		}
+	}
+
+	/* Start the new timer for the entire announce interval */
+	event_add_timer(router->master, autorp_send_announcement, autorp,
+			autorp->announce_interval, &(autorp->announce_timer));
+}
+
+static void autorp_announcement_on(struct pim_autorp *autorp)
+{
+	int interval = 5;
+
+	if (interval > autorp->announce_interval) {
+		/* If the configured interval is less than 5 seconds, then just use that */
+		interval = autorp->announce_interval;
+	}
+	event_add_timer(router->master, autorp_send_announcement, autorp,
+			interval, &(autorp->announce_timer));
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP announcement sending enabled", __func__);
+}
+
+static void autorp_announcement_off(struct pim_autorp *autorp)
+{
+	event_cancel(&(autorp->announce_timer));
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP announcement sending disabled", __func__);
+}
+
+/* Pack the groups of the RP
+ *   rp - Pointer to the RP
+ *   buf - Pointer to the buffer where to start packing groups
+ *   returns - Total group count packed
+ */
+static uint8_t pim_autorp_new_announcement_rp_grps(struct pim_autorp_rp *rp,
+						   uint8_t *buf)
+{
+	struct prefix_list *plist;
+	struct prefix_list_entry *ple;
+	struct autorp_pkt_grp *grpp = (struct autorp_pkt_grp *)buf;
+	uint8_t cnt = 0;
+	in_addr_t taddr;
+
+	if (is_default_prefix(&(rp->grp))) {
+		/* No group so pack from the prefix list
+		 * The grplist should be set and the prefix list exist with at least one group address
+		 */
+		plist = prefix_list_lookup(AFI_IP, rp->grplist);
+		for (ple = plist->head; ple; ple = ple->next) {
+			taddr = ntohl(ple->prefix.u.prefix4.s_addr);
+			if ((taddr & 0xF0000000) == 0xE0000000) {
+				grpp->addr = ple->prefix.u.prefix4.s_addr;
+				grpp->masklen = ple->prefix.prefixlen;
+				grpp->negprefix =
+					(ple->type == PREFIX_PERMIT ? 0 : 1);
+				grpp->reserved = 0;
+
+				++cnt;
+				grpp = (struct autorp_pkt_grp
+						*)(buf +
+						   (sizeof(struct autorp_pkt_grp) *
+						    cnt));
+			}
+		}
+
+		return cnt;
+	}
+
+	/* Only one of group or prefix list should be defined */
+	grpp->addr = rp->grp.u.prefix4.s_addr;
+	grpp->masklen = rp->grp.prefixlen;
+	grpp->negprefix = 0;
+	grpp->reserved = 0;
+	return 1;
+}
+
+/* Pack a single candidate RP
+ *   rp - Pointer to the RP to pack
+ *   buf - Pointer to the buffer where to start packing the RP
+ *   returns - Buffer pointer pointing to the start of the next RP
+ */
+static uint8_t *pim_autorp_new_announcement_rp(struct pim_autorp_rp *rp,
+					       uint8_t *buf)
+{
+	struct autorp_pkt_rp *brp = (struct autorp_pkt_rp *)buf;
+
+	/* Since this is an in_addr, assume it's already the right byte order */
+	brp->addr = rp->addr.s_addr;
+	brp->pimver = PIM_V2;
+	brp->reserved = 0;
+	brp->grpcnt =
+		pim_autorp_new_announcement_rp_grps(rp,
+						    buf + sizeof(struct autorp_pkt_rp));
+	return buf + sizeof(struct autorp_pkt_rp) +
+	       (brp->grpcnt * sizeof(struct autorp_pkt_grp));
+}
+
+/* Pack the candidate RP's on the announcement packet
+ *   autorp - Pointer to the AutoRP instance
+ *   buf - Pointer to the buffer where to start packing the first RP
+ *   bufsz - Output parameter to track size of packed bytes
+ *   returns - Total count of RP's packed
+ */
+static int pim_autorp_new_announcement_rps(struct pim_autorp *autorp,
+					   uint8_t *buf, uint16_t *bufsz)
+{
+	int cnt = 0;
+	struct pim_autorp_rp *rp;
+	/* Keep the original buffer pointer to calculate final size after packing */
+	uint8_t *obuf = buf;
+	struct prefix_list *plist;
+	struct prefix_list_entry *ple;
+	in_addr_t taddr;
+
+	frr_each_safe (pim_autorp_rp, &(autorp->candidate_rp_list), rp) {
+		/* We must have an rp address and either group or list in order to pack this RP, so skip this one */
+		if (pim_addr_is_any(rp->addr) ||
+		    (is_default_prefix(&(rp->grp)) && strlen(rp->grplist) == 0))
+			continue;
+
+		/* Group is net set, so list must be set, make sure the prefix list exists and has valid multicast groups */
+		if (is_default_prefix(&(rp->grp))) {
+			plist = prefix_list_lookup(AFI_IP, rp->grplist);
+			if (plist == NULL)
+				continue;
+			plist = prefix_list_lookup(AFI_IP, rp->grplist);
+			for (ple = plist->head; ple; ple = ple->next) {
+				taddr = ntohl(ple->prefix.u.prefix4.s_addr);
+				if ((taddr & 0xF0000000) == 0xE0000000)
+					break;
+			}
+
+			/* If we went through the entire list without finding a multicast prefix, then skip this RP */
+			if (ple == NULL)
+				continue;
+		}
+
+		/* Now we know for sure we will pack this RP, so count it */
+		++cnt;
+		/* This will return the buffer pointer at the location to start packing the next RP */
+		buf = pim_autorp_new_announcement_rp(rp, buf);
+	}
+
+	if (cnt > 0)
+		*bufsz = buf - obuf;
+
+	return cnt;
+}
+
+/* Build the new announcement packet. If there is a packet to send, restart the send timer with a short wait */
+static void pim_autorp_new_announcement(struct pim_instance *pim)
+{
+	struct pim_autorp *autorp = pim->autorp;
+	struct autorp_pkt_hdr *hdr;
+	int32_t holdtime;
+
+	/* First disable any existing send timer */
+	autorp_announcement_off(autorp);
+
+	if (!autorp->annouce_pkt) {
+		/*
+		 * First time building, allocate the space
+		 * Allocate the max packet size of 65536 so we don't need to resize later.
+		 * This should be ok since we are only allocating the memory once for a single packet (potentially per vrf)
+		 */
+		autorp->annouce_pkt = XCALLOC(MTYPE_PIM_AUTORP_ANNOUNCE, 65536);
+	}
+
+	autorp->annouce_pkt_sz = 0;
+
+	holdtime = autorp->announce_holdtime;
+	if (holdtime == DEFAULT_ANNOUNCE_HOLDTIME)
+		holdtime = autorp->announce_interval * 3;
+	if (holdtime > UINT16_MAX)
+		holdtime = UINT16_MAX;
+
+	hdr = (struct autorp_pkt_hdr *)autorp->annouce_pkt;
+	hdr->version = AUTORP_VERSION;
+	hdr->type = AUTORP_ANNOUNCEMENT_TYPE;
+	hdr->holdtime = htons((uint16_t)holdtime);
+	hdr->reserved = 0;
+	hdr->rpcnt =
+		pim_autorp_new_announcement_rps(autorp,
+						autorp->annouce_pkt +
+							sizeof(struct autorp_pkt_hdr),
+						&(autorp->annouce_pkt_sz));
+
+	/* Still need to add on the size of the header */
+	autorp->annouce_pkt_sz += sizeof(struct autorp_pkt_hdr);
+
+	/* Only turn on the announcement timer if we have a packet to send */
+	if (autorp->annouce_pkt_sz >= MIN_AUTORP_PKT_SZ)
+		autorp_announcement_on(autorp);
+}
+
+bool pim_autorp_rm_candidate_rp(struct pim_instance *pim, pim_addr rpaddr)
+{
+	struct pim_autorp *autorp = pim->autorp;
+	struct pim_autorp_rp *rp;
+	struct pim_autorp_rp find = { .addr = rpaddr };
+
+	rp = pim_autorp_rp_find(&(autorp->candidate_rp_list),
+				(const struct pim_autorp_rp *)&find);
+	if (!rp)
+		return false;
+
+	pim_autorp_rp_del(&(autorp->candidate_rp_list), rp);
+	pim_autorp_rp_free(rp);
+	pim_autorp_new_announcement(pim);
+	return true;
+}
+
+void pim_autorp_add_candidate_rp_group(struct pim_instance *pim,
+				       pim_addr rpaddr, struct prefix group)
+{
+	struct pim_autorp *autorp = pim->autorp;
+	struct pim_autorp_rp *rp;
+	struct pim_autorp_rp find = { .addr = rpaddr };
+
+	rp = pim_autorp_rp_find(&(autorp->candidate_rp_list),
+				(const struct pim_autorp_rp *)&find);
+	if (!rp) {
+		rp = XCALLOC(MTYPE_PIM_AUTORP_CRP, sizeof(*rp));
+		memset(rp, 0, sizeof(struct pim_autorp_rp));
+		rp->autorp = autorp;
+		memcpy(&(rp->addr), &rpaddr, sizeof(pim_addr));
+		pim_autorp_rp_add(&(autorp->candidate_rp_list), rp);
+	}
+
+	apply_mask(&group);
+	prefix_copy(&(rp->grp), &group);
+	/* A new group prefix implies that any previous prefix list is now invalid */
+	rp->grplist[0] = '\0';
+
+	pim_autorp_new_announcement(pim);
+}
+
+bool pim_autorp_rm_candidate_rp_group(struct pim_instance *pim, pim_addr rpaddr,
+				      struct prefix group)
+{
+	struct pim_autorp *autorp = pim->autorp;
+	struct pim_autorp_rp *rp;
+	struct pim_autorp_rp find = { .addr = rpaddr };
+
+	rp = pim_autorp_rp_find(&(autorp->candidate_rp_list),
+				(const struct pim_autorp_rp *)&find);
+	if (!rp)
+		return false;
+
+	memset(&(rp->grp), 0, sizeof(rp->grp));
+	pim_autorp_new_announcement(pim);
+	return true;
+}
+
+void pim_autorp_add_candidate_rp_plist(struct pim_instance *pim,
+				       pim_addr rpaddr, const char *plist)
+{
+	struct pim_autorp *autorp = pim->autorp;
+	struct pim_autorp_rp *rp;
+	struct pim_autorp_rp find = { .addr = rpaddr };
+
+	rp = pim_autorp_rp_find(&(autorp->candidate_rp_list),
+				(const struct pim_autorp_rp *)&find);
+	if (!rp) {
+		rp = XCALLOC(MTYPE_PIM_AUTORP_CRP, sizeof(*rp));
+		memset(rp, 0, sizeof(struct pim_autorp_rp));
+		rp->autorp = autorp;
+		memcpy(&(rp->addr), &rpaddr, sizeof(pim_addr));
+		pim_autorp_rp_add(&(autorp->candidate_rp_list), rp);
+	}
+
+	snprintf(rp->grplist, sizeof(rp->grplist), "%s", plist);
+	/* A new group prefix list implies that any previous group prefix is now invalid */
+	memset(&(rp->grp), 0, sizeof(rp->grp));
+
+	pim_autorp_new_announcement(pim);
+}
+
+bool pim_autorp_rm_candidate_rp_plist(struct pim_instance *pim, pim_addr rpaddr,
+				      const char *plist)
+{
+	struct pim_autorp *autorp = pim->autorp;
+	struct pim_autorp_rp *rp;
+	struct pim_autorp_rp find = { .addr = rpaddr };
+
+	rp = pim_autorp_rp_find(&(autorp->candidate_rp_list),
+				(const struct pim_autorp_rp *)&find);
+	if (!rp)
+		return false;
+
+	rp->grplist[0] = '\0';
+	pim_autorp_new_announcement(pim);
+	return true;
+}
+
+void pim_autorp_announce_scope(struct pim_instance *pim, uint8_t scope)
+{
+	struct pim_autorp *autorp = pim->autorp;
+
+	scope = (scope == 0 ? DEFAULT_ANNOUNCE_SCOPE : scope);
+	if (autorp->announce_scope != scope) {
+		autorp->announce_scope = scope;
+		pim_autorp_new_announcement(pim);
+	}
+}
+
+void pim_autorp_announce_interval(struct pim_instance *pim, uint16_t interval)
+{
+	struct pim_autorp *autorp = pim->autorp;
+
+	interval = (interval == 0 ? DEFAULT_ANNOUNCE_INTERVAL : interval);
+	if (autorp->announce_interval != interval) {
+		autorp->announce_interval = interval;
+		pim_autorp_new_announcement(pim);
+	}
+}
+
+void pim_autorp_announce_holdtime(struct pim_instance *pim, int32_t holdtime)
+{
+	struct pim_autorp *autorp = pim->autorp;
+
+	if (autorp->announce_holdtime != holdtime) {
+		autorp->announce_holdtime = holdtime;
+		pim_autorp_new_announcement(pim);
+	}
+}
+
+void pim_autorp_add_ifp(struct interface *ifp)
+{
+	/* Add a new interface for autorp
+	 *   When autorp is enabled, we must join the autorp groups on all
+	 *   pim/multicast interfaces. When autorp first starts, if finds all
+	 *   current multicast interfaces and joins on them. If a new interface
+	 *   comes up or is configured for multicast after autorp is running, then
+	 *   this method will add it for autorp->
+	 * This is called even when adding a new pim interface that is not yet
+	 * active, so make sure the check, it'll call in again once the interface is up.
+	 */
+	struct pim_instance *pim;
+	struct pim_interface *pim_ifp;
+
+	pim_ifp = ifp->info;
+	if (CHECK_FLAG(ifp->status, ZEBRA_INTERFACE_ACTIVE) && pim_ifp &&
+	    pim_ifp->pim_enable) {
+		pim = pim_ifp->pim;
+		if (pim && pim->autorp && pim->autorp->do_discovery) {
+			if (PIM_DEBUG_AUTORP)
+				zlog_debug("%s: Adding interface %s to AutoRP, joining AutoRP groups",
+					   __func__, ifp->name);
+			if (!pim_autorp_join_groups(ifp)) {
+				zlog_err("Could not join AutoRP groups, errno=%d, %s",
+					 errno, safe_strerror(errno));
+			}
+		}
+	}
+}
+
+void pim_autorp_rm_ifp(struct interface *ifp)
+{
+	/* Remove interface for autorp
+	 *   When an interface is no longer enabled for multicast, or at all, then
+	 *   we should leave the AutoRP groups on this interface.
+	 */
+	struct pim_instance *pim;
+	struct pim_interface *pim_ifp;
+
+	pim_ifp = ifp->info;
+	if (CHECK_FLAG(ifp->status, ZEBRA_INTERFACE_ACTIVE) && pim_ifp) {
+		pim = pim_ifp->pim;
+		if (pim && pim->autorp) {
+			if (PIM_DEBUG_AUTORP)
+				zlog_debug("%s: Removing interface %s from AutoRP, leaving AutoRP groups",
+					   __func__, ifp->name);
+			if (!pim_autorp_leave_groups(ifp)) {
+				zlog_err("Could not leave AutoRP groups, errno=%d, %s",
+					 errno, safe_strerror(errno));
+			}
+		}
+	}
+}
+
+void pim_autorp_start_discovery(struct pim_instance *pim)
+{
+	struct interface *ifp;
+	struct pim_autorp *autorp = pim->autorp;
+
+	if (!autorp->do_discovery) {
+		autorp->do_discovery = true;
+		autorp_read_on(autorp);
+
+		FOR_ALL_INTERFACES (autorp->pim->vrf, ifp) {
+			pim_autorp_add_ifp(ifp);
+		}
+
+		if (PIM_DEBUG_AUTORP)
+			zlog_debug("%s: AutoRP Discovery started", __func__);
+	}
+}
+
+void pim_autorp_stop_discovery(struct pim_instance *pim)
+{
+	struct interface *ifp;
+	struct pim_autorp *autorp = pim->autorp;
+
+	if (autorp->do_discovery) {
+		FOR_ALL_INTERFACES (autorp->pim->vrf, ifp) {
+			pim_autorp_rm_ifp(ifp);
+		}
+
+		autorp->do_discovery = false;
+		autorp_read_off(autorp);
+
+		if (PIM_DEBUG_AUTORP)
+			zlog_debug("%s: AutoRP Discovery stopped", __func__);
+	}
+}
+
+void pim_autorp_init(struct pim_instance *pim)
+{
+	struct pim_autorp *autorp;
+
+	autorp = XCALLOC(MTYPE_PIM_AUTORP, sizeof(*autorp));
+	autorp->pim = pim;
+	autorp->sock = -1;
+	autorp->read_event = NULL;
+	autorp->announce_timer = NULL;
+	autorp->do_discovery = false;
+	pim_autorp_rp_init(&(autorp->discovery_rp_list));
+	pim_autorp_rp_init(&(autorp->candidate_rp_list));
+	autorp->announce_scope = DEFAULT_ANNOUNCE_SCOPE;
+	autorp->announce_interval = DEFAULT_ANNOUNCE_INTERVAL;
+	autorp->announce_holdtime = DEFAULT_ANNOUNCE_HOLDTIME;
+
+	if (!pim_autorp_socket_enable(autorp)) {
+		zlog_err("%s: AutoRP failed to initialize", __func__);
+		return;
+	}
+
+	pim->autorp = autorp;
+	if (PIM_DEBUG_AUTORP)
+		zlog_debug("%s: AutoRP Initialized", __func__);
+
+	/* Start AutoRP discovery by default on startup */
+	pim_autorp_start_discovery(pim);
+}
+
+void pim_autorp_finish(struct pim_instance *pim)
+{
+	struct pim_autorp *autorp = pim->autorp;
+
+	autorp_read_off(autorp);
+	pim_autorp_free(autorp);
+	if (pim_autorp_socket_disable(autorp)) {
+		if (PIM_DEBUG_AUTORP)
+			zlog_debug("%s: AutoRP Finished", __func__);
+	} else
+		zlog_err("%s: AutoRP failed to finish", __func__);
+
+	XFREE(MTYPE_PIM_AUTORP, pim->autorp);
+}
+
+int pim_autorp_config_write(struct pim_instance *pim, struct vty *vty)
+{
+	struct pim_autorp_rp *rp;
+	struct pim_autorp *autorp = pim->autorp;
+	char interval_str[16] = { 0 };
+	char scope_str[16] = { 0 };
+	char holdtime_str[32] = { 0 };
+	char grp_str[64] = { 0 };
+	int writes = 0;
+
+	if (!autorp->do_discovery) {
+		vty_out(vty, " no autorp discovery\n");
+		++writes;
+	}
+
+	if (autorp->announce_interval != DEFAULT_ANNOUNCE_INTERVAL) {
+		snprintf(interval_str, sizeof(interval_str), " interval %u",
+			 autorp->announce_interval);
+	}
+
+	if (autorp->announce_scope != DEFAULT_ANNOUNCE_SCOPE) {
+		snprintf(scope_str, sizeof(scope_str), " scope %u",
+			 autorp->announce_scope);
+	}
+
+	if (autorp->announce_holdtime != DEFAULT_ANNOUNCE_HOLDTIME) {
+		snprintf(holdtime_str, sizeof(holdtime_str), " holdtime %u",
+			 autorp->announce_holdtime);
+	}
+
+	if (strlen(interval_str) || strlen(scope_str) || strlen(holdtime_str)) {
+		vty_out(vty, " autorp announce%s%s%s\n", interval_str,
+			scope_str, holdtime_str);
+		++writes;
+	}
+
+	frr_each_safe (pim_autorp_rp, &(autorp->candidate_rp_list), rp) {
+		/* Only print candidate RP's that have all the information needed to be announced */
+		if (pim_addr_is_any(rp->addr) ||
+		    (is_default_prefix(&(rp->grp)) && strlen(rp->grplist) == 0))
+			continue;
+
+		/* Don't make sure the prefix list has multicast groups, user may not have created it yet */
+
+		if (!is_default_prefix(&(rp->grp)))
+			snprintfrr(grp_str, sizeof(grp_str), "%pFX", &(rp->grp));
+		else
+			snprintfrr(grp_str, sizeof(grp_str), "group-list %s",
+				   rp->grplist);
+
+		vty_out(vty, " autorp announce %pI4 %s\n", &(rp->addr), grp_str);
+		++writes;
+	}
+
+	return writes;
+}
+
+void pim_autorp_show_autorp(struct vty *vty, struct pim_instance *pim,
+			    json_object *json)
+{
+	struct pim_autorp_rp *rp;
+	struct pim_autorp *autorp = pim->autorp;
+	struct ttable *tt = NULL;
+	char *table = NULL;
+	char grp_str[64] = { 0 };
+	char plist_str[64] = { 0 };
+	json_object *annouce_jobj;
+
+	/* Prepare table. */
+	tt = ttable_new(&ttable_styles[TTSTYLE_BLANK]);
+	ttable_add_row(tt, "RP address|group|prefix-list");
+	tt->style.cell.rpad = 2;
+	tt->style.corner = '+';
+	ttable_restyle(tt);
+
+	frr_each_safe (pim_autorp_rp, &(autorp->candidate_rp_list), rp) {
+		if (!is_default_prefix(&(rp->grp)))
+			snprintfrr(grp_str, sizeof(grp_str), "%pFX", &(rp->grp));
+		else
+			snprintfrr(plist_str, sizeof(plist_str), "%s",
+				   rp->grplist);
+
+		ttable_add_row(tt, "%pI4|%s|%s", &(rp->addr), grp_str,
+			       plist_str);
+	}
+
+	if (json) {
+		json_object_boolean_add(json, "discoveryEnabled",
+					autorp->do_discovery);
+
+		annouce_jobj = json_object_new_object();
+		json_object_int_add(annouce_jobj, "scope",
+				    autorp->announce_scope);
+		json_object_int_add(annouce_jobj, "interval",
+				    autorp->announce_interval);
+		json_object_int_add(annouce_jobj, "holdtime",
+				    autorp->announce_holdtime);
+		json_object_object_add(annouce_jobj, "rpList",
+				       ttable_json_with_json_text(
+					       tt, "sss",
+					       "rpAddress|group|prefixList"));
+
+		json_object_object_add(json, "announce", annouce_jobj);
+	} else {
+		vty_out(vty, "AutoRP Discovery is %sabled\n",
+			(autorp->do_discovery ? "en" : "dis"));
+		vty_out(vty, "AutoRP Candidate RPs\n");
+		vty_out(vty, "  interval %us, scope %u, holdtime %us\n",
+			autorp->announce_interval, autorp->announce_scope,
+			(autorp->announce_holdtime == DEFAULT_ANNOUNCE_HOLDTIME
+				 ? (autorp->announce_interval * 3)
+				 : autorp->announce_holdtime));
+
+		vty_out(vty, "\n");
+
+		table = ttable_dump(tt, "\n");
+		vty_out(vty, "%s\n", table);
+		XFREE(MTYPE_TMP, table);
+	}
+
+	ttable_del(tt);
+}
diff --git a/pimd/pim_autorp.h b/pimd/pim_autorp.h
new file mode 100644
index 000000000..a0b029d00
--- /dev/null
+++ b/pimd/pim_autorp.h
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * pim_autorp.h: PIM Auto RP handling related
+ *
+ * Copyright (C) 20224 ATCorp.
+ * Nathan Bahr
+ */
+
+#ifndef __PIM_AUTORP_H__
+#define __PIM_AUTORP_H__
+
+#include <typesafe.h>
+
+#define AUTORP_VERSION		 1
+#define AUTORP_ANNOUNCEMENT_TYPE 1
+#define AUTORP_DISCOVERY_TYPE	 2
+#define PIM_VUNKNOWN		 0
+#define PIM_V1			 1
+#define PIM_V2			 2
+#define PIM_V1_2		 3
+
+#define DEFAULT_ANNOUNCE_INTERVAL 60
+#define DEFAULT_ANNOUNCE_SCOPE	  31
+#define DEFAULT_ANNOUNCE_HOLDTIME -1
+
+PREDECL_SORTLIST_UNIQ(pim_autorp_rp);
+
+struct autorp_pkt_grp {
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	uint8_t negprefix : 1;
+	uint8_t reserved : 7;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+	uint8_t reserved : 7;
+	uint8_t negprefix : 1;
+#else
+#error "Please fix <bits/endian.h>"
+#endif
+	uint8_t masklen;
+	uint32_t addr;
+} __attribute__((__packed__));
+
+struct autorp_pkt_rp {
+	uint32_t addr;
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	uint8_t pimver : 2;
+	uint8_t reserved : 6;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+	uint8_t reserved : 6;
+	uint8_t pimver : 2;
+#else
+#error "Please fix <bits/endian.h>"
+#endif
+	uint8_t grpcnt;
+} __attribute__((__packed__));
+
+struct autorp_pkt_hdr {
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	uint8_t type : 4;
+	uint8_t version : 4;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+	uint8_t version : 4;
+	uint8_t type : 4;
+#else
+#error "Please fix <bits/endian.h>"
+#endif
+	uint8_t rpcnt;
+	uint16_t holdtime;
+	uint32_t reserved;
+} __attribute__((__packed__));
+
+#define MIN_AUTORP_PKT_SZ                                                      \
+	(sizeof(struct autorp_pkt_hdr) + sizeof(struct autorp_pkt_rp) +        \
+	 sizeof(struct autorp_pkt_grp))
+
+struct pim_autorp_rp {
+	struct pim_autorp *autorp;
+	struct in_addr addr;
+	uint16_t holdtime;
+	struct event *hold_timer;
+	struct prefix grp;
+	char grplist[32];
+	struct pim_autorp_rp_item list;
+};
+
+struct pim_autorp {
+	/* backpointer to pim instance */
+	struct pim_instance *pim;
+
+	/* UDP socket bound to AutoRP port, used for sending and receiving all AutoRP packets */
+	int sock;
+
+	/* Event for reading AutoRP packets */
+	struct event *read_event;
+
+	/* Event for sending announcement packets */
+	struct event *announce_timer;
+
+	/* Event for sending discovery packets*/
+	/* struct event *discovery_timer; */
+
+	/* Flag enabling reading discovery packets */
+	bool do_discovery;
+
+	/* Flag enabling mapping agent (reading announcements and sending discovery)*/
+	/* bool do_mapping; */
+
+	/* List of RP's in received discovery packets */
+	struct pim_autorp_rp_head discovery_rp_list;
+
+	/* List of configured candidate RP's to send in announcement packets */
+	struct pim_autorp_rp_head candidate_rp_list;
+
+	/* List of announced RP's to send in discovery packets */
+	/* struct pim_autorp_rp_head mapping_rp_list; */
+
+	/* Packet parameters for sending announcement packets */
+	uint8_t announce_scope;
+	uint16_t announce_interval;
+	int32_t announce_holdtime;
+
+	/* Pre-built announcement packet, only changes when configured RP's or packet parameters change */
+	uint8_t *annouce_pkt;
+	uint16_t annouce_pkt_sz;
+
+	/* TODO: Packet parameters for sending discovery packets
+	 * int discovery_scope;
+	 * int discovery_interval;
+	 * int discovery_holdtime;
+	 */
+};
+
+#define AUTORP_GRPLEN 6
+#define AUTORP_RPLEN  6
+#define AUTORP_HDRLEN 8
+
+bool pim_autorp_rm_candidate_rp(struct pim_instance *pim, pim_addr rpaddr);
+void pim_autorp_add_candidate_rp_group(struct pim_instance *pim,
+				       pim_addr rpaddr, struct prefix group);
+bool pim_autorp_rm_candidate_rp_group(struct pim_instance *pim, pim_addr rpaddr,
+				      struct prefix group);
+void pim_autorp_add_candidate_rp_plist(struct pim_instance *pim,
+				       pim_addr rpaddr, const char *plist);
+bool pim_autorp_rm_candidate_rp_plist(struct pim_instance *pim, pim_addr rpaddr,
+				      const char *plist);
+void pim_autorp_announce_scope(struct pim_instance *pim, uint8_t scope);
+void pim_autorp_announce_interval(struct pim_instance *pim, uint16_t interval);
+void pim_autorp_announce_holdtime(struct pim_instance *pim, int32_t holdtime);
+void pim_autorp_add_ifp(struct interface *ifp);
+void pim_autorp_rm_ifp(struct interface *ifp);
+void pim_autorp_start_discovery(struct pim_instance *pim);
+void pim_autorp_stop_discovery(struct pim_instance *pim);
+void pim_autorp_init(struct pim_instance *pim);
+void pim_autorp_finish(struct pim_instance *pim);
+int pim_autorp_config_write(struct pim_instance *pim, struct vty *vty);
+void pim_autorp_show_autorp(struct vty *vty, struct pim_instance *pim,
+			    json_object *json);
+
+#endif
diff --git a/pimd/pim_iface.c b/pimd/pim_iface.c
index 7f873f45d..1dc9307e4 100644
--- a/pimd/pim_iface.c
+++ b/pimd/pim_iface.c
@@ -1914,6 +1914,12 @@ static int pim_ifp_up(struct interface *ifp)
 		}
 	}
 
+#if PIM_IPV == 4
+	if (pim->autorp && pim->autorp->do_discovery && pim_ifp &&
+	    pim_ifp->pim_enable)
+		pim_autorp_add_ifp(ifp);
+#endif
+
 	pim_cand_addrs_changed();
 	return 0;
 }
@@ -1951,6 +1957,10 @@ static int pim_ifp_down(struct interface *ifp)
 		pim_ifstat_reset(ifp);
 	}
 
+#if PIM_IPV == 4
+	pim_autorp_rm_ifp(ifp);
+#endif
+
 	pim_cand_addrs_changed();
 	return 0;
 }
@@ -2023,6 +2033,11 @@ void pim_pim_interface_delete(struct interface *ifp)
 	if (!pim_ifp)
 		return;
 
+#if PIM_IPV == 4
+	if (pim_ifp->pim_enable)
+		pim_autorp_rm_ifp(ifp);
+#endif
+
 	pim_ifp->pim_enable = false;
 
 	pim_if_membership_clear(ifp);
diff --git a/pimd/pim_instance.c b/pimd/pim_instance.c
index 9a697c920..f7c5ea3bc 100644
--- a/pimd/pim_instance.c
+++ b/pimd/pim_instance.c
@@ -57,6 +57,10 @@ static void pim_instance_terminate(struct pim_instance *pim)
 
 	pim_mroute_socket_disable(pim);
 
+#if PIM_IPV == 4
+	pim_autorp_finish(pim);
+#endif
+
 	XFREE(MTYPE_PIM_PLIST_NAME, pim->spt.plist);
 	XFREE(MTYPE_PIM_PLIST_NAME, pim->register_plist);
 
@@ -125,6 +129,10 @@ static struct pim_instance *pim_instance_init(struct vrf *vrf)
 	pim->msdp.keep_alive = PIM_MSDP_PEER_KA_TIME;
 	pim->msdp.connection_retry = PIM_MSDP_PEER_CONNECT_RETRY_TIME;
 
+#if PIM_IPV == 4
+	pim_autorp_init(pim);
+#endif
+
 	return pim;
 }
 
diff --git a/pimd/pim_instance.h b/pimd/pim_instance.h
index ec331332c..f484d847b 100644
--- a/pimd/pim_instance.h
+++ b/pimd/pim_instance.h
@@ -17,6 +17,7 @@
 #include "pim_oil.h"
 #include "pim_upstream.h"
 #include "pim_mroute.h"
+#include "pim_autorp.h"
 
 enum pim_spt_switchover {
 	PIM_SPT_IMMEDIATE,
@@ -152,6 +153,8 @@ struct pim_instance {
 	struct pim_msdp msdp;
 	struct pim_vxlan_instance vxlan;
 
+	struct pim_autorp *autorp;
+
 	struct list *ssmpingd_list;
 	pim_addr ssmpingd_group_addr;
 
diff --git a/pimd/pim_rp.c b/pimd/pim_rp.c
index a2ddc8216..0c47bc158 100644
--- a/pimd/pim_rp.c
+++ b/pimd/pim_rp.c
@@ -1140,7 +1140,8 @@ int pim_rp_config_write(struct pim_instance *pim, struct vty *vty)
 		if (pim_rpf_addr_is_inaddr_any(&rp_info->rp))
 			continue;
 
-		if (rp_info->rp_src == RP_SRC_BSR)
+		if (rp_info->rp_src != RP_SRC_NONE &&
+		    rp_info->rp_src != RP_SRC_STATIC)
 			continue;
 
 		rp_addr = rp_info->rp.rpf_addr;
@@ -1200,6 +1201,8 @@ void pim_rp_show_information(struct pim_instance *pim, struct prefix *range,
 			strlcpy(source, "Static", sizeof(source));
 		else if (rp_info->rp_src == RP_SRC_BSR)
 			strlcpy(source, "BSR", sizeof(source));
+		else if (rp_info->rp_src == RP_SRC_AUTORP)
+			strlcpy(source, "AutoRP", sizeof(source));
 		else
 			strlcpy(source, "None", sizeof(source));
 		if (json) {
diff --git a/pimd/pim_rp.h b/pimd/pim_rp.h
index 32c630674..24832d0db 100644
--- a/pimd/pim_rp.h
+++ b/pimd/pim_rp.h
@@ -16,11 +16,7 @@
 
 struct pim_interface;
 
-enum rp_source {
-	RP_SRC_NONE = 0,
-	RP_SRC_STATIC,
-	RP_SRC_BSR
-};
+enum rp_source { RP_SRC_NONE = 0, RP_SRC_STATIC, RP_SRC_BSR, RP_SRC_AUTORP };
 
 struct rp_info {
 	struct prefix group;
diff --git a/pimd/pim_sock.c b/pimd/pim_sock.c
index 3476c177b..8900652df 100644
--- a/pimd/pim_sock.c
+++ b/pimd/pim_sock.c
@@ -292,6 +292,36 @@ int pim_socket_join(int fd, pim_addr group, pim_addr ifaddr, ifindex_t ifindex,
 	return ret;
 }
 
+int pim_socket_leave(int fd, pim_addr group, pim_addr ifaddr, ifindex_t ifindex,
+		     struct pim_interface *pim_ifp)
+{
+	int ret;
+
+#if PIM_IPV == 4
+	ret = setsockopt_ipv4_multicast(fd, IP_DROP_MEMBERSHIP, ifaddr,
+					group.s_addr, ifindex);
+#else
+	struct ipv6_mreq opt;
+
+	memcpy(&opt.ipv6mr_multiaddr, &group, 16);
+	opt.ipv6mr_interface = ifindex;
+	ret = setsockopt(fd, IPPROTO_IPV6, IPV6_LEAVE_GROUP, &opt, sizeof(opt));
+#endif
+
+	if (ret) {
+		flog_err(EC_LIB_SOCKET,
+			 "Failure socket leaving fd=%d group %pPAs on interface address %pPAs: %m",
+			 fd, &group, &ifaddr);
+		pim_ifp->igmp_ifstat_joins_failed++;
+		return ret;
+	}
+
+	if (PIM_DEBUG_TRACE)
+		zlog_debug("Socket fd=%d left group %pPAs on interface address %pPAs",
+			   fd, &group, &ifaddr);
+	return ret;
+}
+
 #if PIM_IPV == 4
 static void cmsg_getdstaddr(struct msghdr *mh, struct sockaddr_storage *dst,
 			    ifindex_t *ifindex)
diff --git a/pimd/pim_sock.h b/pimd/pim_sock.h
index 1cf01b31d..0a81c6943 100644
--- a/pimd/pim_sock.h
+++ b/pimd/pim_sock.h
@@ -32,6 +32,8 @@ int pim_socket_mcast(int protocol, pim_addr ifaddr, struct interface *ifp,
 		     uint8_t loop);
 int pim_socket_join(int fd, pim_addr group, pim_addr ifaddr, ifindex_t ifindex,
 		    struct pim_interface *pim_ifp);
+int pim_socket_leave(int fd, pim_addr group, pim_addr ifaddr, ifindex_t ifindex,
+		     struct pim_interface *pim_ifp);
 int pim_socket_recvfromto(int fd, uint8_t *buf, size_t len,
 			  struct sockaddr_storage *from, socklen_t *fromlen,
 			  struct sockaddr_storage *to, socklen_t *tolen,
diff --git a/pimd/pimd.h b/pimd/pimd.h
index 3d9318953..461b7d08a 100644
--- a/pimd/pimd.h
+++ b/pimd/pimd.h
@@ -95,6 +95,7 @@
 #define PIM_MASK_VXLAN               (1 << 26)
 #define PIM_MASK_BSM_PROC            (1 << 27)
 #define PIM_MASK_MLAG                (1 << 28)
+#define PIM_MASK_AUTORP		     (1 << 29)
 /* Remember 32 bits!!! */
 
 /* PIM error codes */
@@ -167,6 +168,7 @@ extern uint8_t qpim_ecmp_rebalance_enable;
 #define PIM_DEBUG_MTRACE (router->debugs & PIM_MASK_MTRACE)
 #define PIM_DEBUG_VXLAN (router->debugs & PIM_MASK_VXLAN)
 #define PIM_DEBUG_BSM	(router->debugs & PIM_MASK_BSM_PROC)
+#define PIM_DEBUG_AUTORP	 (router->debugs & PIM_MASK_AUTORP)
 
 #define PIM_DEBUG_EVENTS                                                       \
 	(router->debugs & (PIM_MASK_PIM_EVENTS | PIM_MASK_GM_EVENTS |          \
@@ -209,6 +211,7 @@ extern uint8_t qpim_ecmp_rebalance_enable;
 #define PIM_DO_DEBUG_PIM_NHT_RP (router->debugs |= PIM_MASK_PIM_NHT_RP)
 #define PIM_DO_DEBUG_MTRACE (router->debugs |= PIM_MASK_MTRACE)
 #define PIM_DO_DEBUG_VXLAN (router->debugs |= PIM_MASK_VXLAN)
+#define PIM_DO_DEBUG_AUTORP	    (router->debugs |= PIM_MASK_AUTORP)
 
 #define PIM_DONT_DEBUG_PIM_EVENTS (router->debugs &= ~PIM_MASK_PIM_EVENTS)
 #define PIM_DONT_DEBUG_PIM_PACKETS (router->debugs &= ~PIM_MASK_PIM_PACKETS)
@@ -243,6 +246,7 @@ extern uint8_t qpim_ecmp_rebalance_enable;
 #define PIM_DONT_DEBUG_MTRACE (router->debugs &= ~PIM_MASK_MTRACE)
 #define PIM_DONT_DEBUG_VXLAN (router->debugs &= ~PIM_MASK_VXLAN)
 #define PIM_DONT_DEBUG_BSM (router->debugs &= ~PIM_MASK_BSM_PROC)
+#define PIM_DONT_DEBUG_AUTORP	  (router->debugs &= ~PIM_MASK_AUTORP)
 
 /* RFC 3376: 8.1. Robustness Variable - Default: 2 for IGMP */
 /* RFC 2710: 7.1. Robustness Variable - Default: 2 for MLD */
diff --git a/pimd/subdir.am b/pimd/subdir.am
index 48f1e3b72..bda594e5c 100644
--- a/pimd/subdir.am
+++ b/pimd/subdir.am
@@ -59,6 +59,7 @@ pim_common = \
 
 pimd_pimd_SOURCES = \
 	$(pim_common) \
+	pimd/pim_autorp.c \
 	pimd/pim_cmd.c \
 	pimd/pim_igmp.c \
 	pimd/pim_igmp_mtrace.c \
@@ -98,6 +99,7 @@ nodist_pimd_pim6d_SOURCES = \
 noinst_HEADERS += \
 	pimd/pim_addr.h \
 	pimd/pim_assert.h \
+	pimd/pim_autorp.h \
 	pimd/pim_bfd.h \
 	pimd/pim_bsm.h \
 	pimd/pim_cmd.h \