pim6d: Mroute changes

Mroute and supporting changes

Signed-off-by: plsaranya <Saranya_Panjarathina@dell.com>

11 files changed, 659 insertions, 425 deletions

diff --git a/pimd/pim6_mroute_msg.c b/pimd/pim6_mroute_msg.c
new file mode 100644
index 000000000..d67490a1f
--- /dev/null
+++ b/pimd/pim6_mroute_msg.c
@@ -0,0 +1,196 @@
+/*
+ * PIM for Quagga
+ * Copyright (C) 2022  Dell Technologies Ltd
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; see the file COPYING; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <zebra.h>
+#include "log.h"
+#include "privs.h"
+#include "if.h"
+#include "prefix.h"
+#include "vty.h"
+#include "plist.h"
+#include "sockopt.h"
+#include "lib_errors.h"
+#include "lib/network.h"
+
+#include "pimd.h"
+#include "pim_mroute.h"
+#include "pim_oil.h"
+#include "pim_str.h"
+#include "pim_time.h"
+#include "pim_iface.h"
+#include "pim_macro.h"
+#include "pim_rp.h"
+#include "pim_oil.h"
+#include "pim_ssm.h"
+#include "pim_sock.h"
+
+int pim_mroute_set(struct pim_instance *pim, int enable)
+{
+	int err;
+	int opt, data;
+	socklen_t data_len = sizeof(data);
+	static const struct sock_filter filter[] = {
+		BPF_STMT(BPF_LD+BPF_B+BPF_ABS, 0),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, 0, 0, 1),
+		BPF_STMT(BPF_RET | BPF_K, 0xffff),
+		BPF_STMT(BPF_RET | BPF_K, 0),
+	};
+
+	static const struct sock_fprog bpf = {
+		.len = array_size(filter),
+		.filter = (struct sock_filter *)filter,
+	};
+
+	/*
+	 * We need to create the VRF table for the pim mroute_socket
+	 */
+	if (pim->vrf->vrf_id != VRF_DEFAULT) {
+		frr_with_privs (&pimd_privs) {
+
+			data = pim->vrf->data.l.table_id;
+			err = setsockopt(pim->mroute_socket, PIM_IPPROTO,
+					 MRT6_TABLE, &data, data_len);
+			if (err) {
+				zlog_warn(
+					"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO, MRT6_TABLE=%d): errno=%d: %s",
+					__FILE__, __func__, pim->mroute_socket,
+					data, errno, safe_strerror(errno));
+				return -1;
+			}
+		}
+	}
+
+	frr_with_privs (&pimd_privs) {
+		opt = enable ? MRT6_INIT : MRT6_DONE;
+		/*
+		 * *BSD *cares* about what value we pass down
+		 * here
+		 */
+		data = 1;
+		err = setsockopt(pim->mroute_socket, PIM_IPPROTO, opt, &data,
+				 data_len);
+		if (err) {
+			zlog_warn(
+				"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO,%s=%d): errno=%d: %s",
+				__FILE__, __func__, pim->mroute_socket,
+				enable ? "MRT6_INIT" : "MRT6_DONE", data, errno,
+				safe_strerror(errno));
+			return -1;
+		}
+	}
+
+	if (enable) {
+		/* Linux and Solaris IPV6_PKTINFO */
+		data = 1;
+		if (setsockopt(pim->mroute_socket, PIM_IPPROTO, IPV6_PKTINFO,
+			       &data, data_len)) {
+			zlog_warn(
+				"Could not set IPV6_PKTINFO on socket fd=%d: errno=%d: %s",
+				pim->mroute_socket, errno,
+				safe_strerror(errno));
+		}
+	}
+
+	setsockopt_so_recvbuf(pim->mroute_socket, 1024 * 1024 * 8);
+
+	if (set_nonblocking (pim->mroute_socket) < 0) {
+		zlog_warn(
+			"Could not set non blocking on socket fd=%d: errno=%d: %s",
+			pim->mroute_socket, errno,
+			safe_strerror(errno));
+	}
+
+	if (enable) {
+#if defined linux
+		int upcalls = MRT6MSG_WRMIFWHOLE;
+		opt = MRT6_PIM;
+
+		err = setsockopt(pim->mroute_socket, PIM_IPPROTO, opt, &upcalls,
+				 sizeof(upcalls));
+		if (err) {
+			zlog_warn(
+				"Failure to register for WHOLE and WRONGMIF upcalls %d %s",
+				errno, safe_strerror(errno));
+			return -1;
+		}
+#else
+		zlog_warn(
+			"PIM-SM will not work properly on this platform, until the ability to receive the WHOLEPKT upcall");
+#endif
+		if (setsockopt(pim->mroute_socket, SOL_SOCKET, SO_ATTACH_FILTER, &bpf, sizeof(bpf))) {
+			zlog_warn("Failure to attach SO_ATTACH_FILTER on fd %d: %d %s",
+					pim->mroute_socket, errno, safe_strerror(errno));
+		}
+	}
+
+	return 0;
+}
+static const char *const mrt6msgtype2str[MRT6MSG_WRMIFWHOLE + 1] = {
+	"<unknown_upcall?>", "NOCACHE", "WRONGMIF", "WHOLEPKT", "WRMIFWHOLE"};
+
+int pim_mroute_msg(struct pim_instance *pim, const char *buf,
+			  size_t buf_size, ifindex_t ifindex)
+{
+	struct interface *ifp;
+	const struct ip6_hdr *ip6_hdr;
+	const struct mrt6msg *msg;
+
+	if (buf_size < (int)sizeof(struct ip6_hdr))
+		return 0;
+
+	ip6_hdr = (const struct ip6_hdr *)buf;
+
+	if ((ip6_hdr->ip6_vfc & 0xf) == 0) {
+		msg = (const struct mrt6msg *)buf;
+
+		ifp = pim_if_find_by_vif_index(pim, msg->im6_mif);
+
+		if (!ifp)
+			return 0;
+		if (PIM_DEBUG_MROUTE) {
+			zlog_debug(
+				"%s: pim kernel upcall %s type=%d ip_p=%d from fd=%d for (S,G)=(%pPAs,%pPAs) on %s mifi=%d  size=%ld",
+				__func__, mrt6msgtype2str[msg->im6_msgtype],
+				msg->im6_msgtype, ip6_hdr->ip6_nxt,
+				pim->mroute_socket, &msg->im6_src,
+				&msg->im6_dst, ifp->name, msg->im6_mif,
+				(long int)buf_size);
+		}
+
+		switch (msg->im6_msgtype) {
+		case MRT6MSG_WRONGMIF:
+			return pim_mroute_msg_wrongvif(pim->mroute_socket, ifp,
+							msg);
+		case MRT6MSG_NOCACHE:
+			return pim_mroute_msg_nocache(pim->mroute_socket, ifp,
+						       msg);
+		case MRT6MSG_WHOLEPKT:
+			return pim_mroute_msg_wholepkt(pim->mroute_socket, ifp,
+							(const char *)msg);
+		case MRT6MSG_WRMIFWHOLE:
+			return pim_mroute_msg_wrvifwhole(
+				pim->mroute_socket, ifp, (const char *)msg);
+		default:
+			break;
+		}
+	} 
+
+	return 0;
+}
+
diff --git a/pimd/pim6_stubs.c b/pimd/pim6_stubs.c
index e689c7aca..f2781a3ce 100644
--- a/pimd/pim6_stubs.c
+++ b/pimd/pim6_stubs.c
@@ -121,3 +121,12 @@ struct bsgrp_node *pim_bsm_get_bsgrp_node(struct bsm_scope *scope,
 void pim_bsm_write_config(struct vty *vty, struct interface *ifp)
 {
 }
+void pim_register_send(const uint8_t *buf, int buf_size, pim_addr src,
+		       struct pim_rpf *rpg, int null_register,
+		       struct pim_upstream *up)
+{
+}
+void pim_register_stop_send(struct interface *ifp, pim_sgaddr *sg, pim_addr src,
+			    pim_addr originator)
+{
+}
diff --git a/pimd/pim_instance.h b/pimd/pim_instance.h
index 4ac9ef7f5..f8323deda 100644
--- a/pimd/pim_instance.h
+++ b/pimd/pim_instance.h
@@ -30,18 +30,7 @@
 #include "pim_vxlan_instance.h"
 #include "pim_oil.h"
 #include "pim_upstream.h"
-
-#if defined(HAVE_LINUX_MROUTE_H)
-#include <linux/mroute.h>
-#else
-/*
-  Below: from <linux/mroute.h>
-*/
-
-#ifndef MAXVIFS
-#define MAXVIFS (256)
-#endif
-#endif
+#include "pim_mroute.h"
 
 enum pim_spt_switchover {
 	PIM_SPT_IMMEDIATE,
diff --git a/pimd/pim_mroute.c b/pimd/pim_mroute.c
index bce319b3a..7fc4f12d2 100644
--- a/pimd/pim_mroute.c
+++ b/pimd/pim_mroute.c
@@ -43,121 +43,19 @@
 #include "pim_ssm.h"
 #include "pim_sock.h"
 #include "pim_vxlan.h"
+#include "pim_msg.h"
 
 static void mroute_read_on(struct pim_instance *pim);
 
-static int pim_mroute_set(struct pim_instance *pim, int enable)
-{
-	int err;
-	int opt, data;
-	socklen_t data_len = sizeof(data);
-	long flags;
-
-	/*
-	 * We need to create the VRF table for the pim mroute_socket
-	 */
-	if (pim->vrf->vrf_id != VRF_DEFAULT) {
-		frr_with_privs(&pimd_privs) {
-
-			data = pim->vrf->data.l.table_id;
-			err = setsockopt(pim->mroute_socket, IPPROTO_IP,
-					 MRT_TABLE,
-					 &data, data_len);
-			if (err) {
-				zlog_warn(
-					"%s %s: failure: setsockopt(fd=%d,IPPROTO_IP, MRT_TABLE=%d): errno=%d: %s",
-					__FILE__, __func__, pim->mroute_socket,
-					data, errno, safe_strerror(errno));
-				return -1;
-			}
-
-		}
-	}
-
-	frr_with_privs(&pimd_privs) {
-		opt = enable ? MRT_INIT : MRT_DONE;
-		/*
-		 * *BSD *cares* about what value we pass down
-		 * here
-		 */
-		data = 1;
-		err = setsockopt(pim->mroute_socket, IPPROTO_IP,
-				 opt, &data, data_len);
-		if (err) {
-			zlog_warn(
-				"%s %s: failure: setsockopt(fd=%d,IPPROTO_IP,%s=%d): errno=%d: %s",
-				__FILE__, __func__, pim->mroute_socket,
-				enable ? "MRT_INIT" : "MRT_DONE", data, errno,
-				safe_strerror(errno));
-			return -1;
-		}
-	}
-
-#if defined(HAVE_IP_PKTINFO)
-	if (enable) {
-		/* Linux and Solaris IP_PKTINFO */
-		data = 1;
-		if (setsockopt(pim->mroute_socket, IPPROTO_IP, IP_PKTINFO,
-			       &data, data_len)) {
-			zlog_warn(
-				"Could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
-				pim->mroute_socket, errno,
-				safe_strerror(errno));
-		}
-	}
-#endif
-
-	setsockopt_so_recvbuf(pim->mroute_socket, 1024 * 1024 * 8);
-
-	flags = fcntl(pim->mroute_socket, F_GETFL, 0);
-	if (flags < 0) {
-		zlog_warn("Could not get flags on socket fd:%d %d %s",
-			  pim->mroute_socket, errno, safe_strerror(errno));
-		close(pim->mroute_socket);
-		return -1;
-	}
-	if (fcntl(pim->mroute_socket, F_SETFL, flags | O_NONBLOCK)) {
-		zlog_warn("Could not set O_NONBLOCK on socket fd:%d %d %s",
-			  pim->mroute_socket, errno, safe_strerror(errno));
-		close(pim->mroute_socket);
-		return -1;
-	}
-
-	if (enable) {
-#if defined linux
-		int upcalls = IGMPMSG_WRVIFWHOLE;
-		opt = MRT_PIM;
-
-		err = setsockopt(pim->mroute_socket, IPPROTO_IP, opt, &upcalls,
-				 sizeof(upcalls));
-		if (err) {
-			zlog_warn(
-				"Failure to register for VIFWHOLE and WRONGVIF upcalls %d %s",
-				errno, safe_strerror(errno));
-			return -1;
-		}
-#else
-		zlog_warn(
-			"PIM-SM will not work properly on this platform, until the ability to receive the WRVIFWHOLE upcall");
-#endif
-	}
-
-	return 0;
-}
-
-#if PIM_IPV == 4
-static const char *const igmpmsgtype2str[IGMPMSG_WRVIFWHOLE + 1] = {
-	"<unknown_upcall?>", "NOCACHE", "WRONGVIF", "WHOLEPKT", "WRVIFWHOLE"};
 
-static int pim_mroute_msg_nocache(int fd, struct interface *ifp,
-				  const struct igmpmsg *msg)
+int pim_mroute_msg_nocache(int fd, struct interface *ifp, const kernmsg *msg)
 {
 	struct pim_interface *pim_ifp = ifp->info;
 	struct pim_upstream *up;
 	struct pim_rpf *rpg;
 	pim_sgaddr sg;
 
-	rpg = pim_ifp ? RP(pim_ifp->pim, msg->im_dst) : NULL;
+	rpg = pim_ifp ? RP(pim_ifp->pim, msg->msg_im_dst) : NULL;
 	/*
 	 * If the incoming interface is unknown OR
 	 * the Interface type is SSM we don't need to
@@ -176,7 +74,7 @@ static int pim_mroute_msg_nocache(int fd, struct interface *ifp,
 	 * If we've received a multicast packet that isn't connected to
 	 * us
 	 */
-	if (!pim_if_connected_to_source(ifp, msg->im_src)) {
+	if (!pim_if_connected_to_source(ifp, msg->msg_im_src)) {
 		if (PIM_DEBUG_MROUTE_DETAIL)
 			zlog_debug(
 				"%s: Received incoming packet that doesn't originate on our seg",
@@ -185,13 +83,14 @@ static int pim_mroute_msg_nocache(int fd, struct interface *ifp,
 	}
 
 	memset(&sg, 0, sizeof(sg));
-	sg.src = msg->im_src;
-	sg.grp = msg->im_dst;
+	sg.src = msg->msg_im_src;
+	sg.grp = msg->msg_im_dst;
 
 	if (!(PIM_I_am_DR(pim_ifp))) {
 		if (PIM_DEBUG_MROUTE_DETAIL)
-			zlog_debug("%s: Interface is not the DR blackholing incoming traffic for %pSG",
-				   __func__, &sg);
+			zlog_debug(
+				"%s: Interface is not the DR blackholing incoming traffic for %pSG",
+				__func__, &sg);
 
 		/*
 		 * We are not the DR, but we are still receiving packets
@@ -238,22 +137,21 @@ static int pim_mroute_msg_nocache(int fd, struct interface *ifp,
 	return 0;
 }
 
-static int pim_mroute_msg_wholepkt(int fd, struct interface *ifp,
-				   const char *buf)
+int pim_mroute_msg_wholepkt(int fd, struct interface *ifp, const char *buf)
 {
 	struct pim_interface *pim_ifp;
 	pim_sgaddr sg;
 	struct pim_rpf *rpg;
-	const struct ip *ip_hdr;
+	const ipv_hdr *ip_hdr;
 	struct pim_upstream *up;
 
 	pim_ifp = ifp->info;
 
-	ip_hdr = (const struct ip *)buf;
+	ip_hdr = (const ipv_hdr *)buf;
 
 	memset(&sg, 0, sizeof(sg));
-	sg.src = ip_hdr->ip_src;
-	sg.grp = ip_hdr->ip_dst;
+	sg.src = IPV_SRC(ip_hdr);
+	sg.grp = IPV_DST(ip_hdr);
 
 	up = pim_upstream_find(pim_ifp->pim, &sg);
 	if (!up) {
@@ -268,8 +166,9 @@ static int pim_mroute_msg_wholepkt(int fd, struct interface *ifp,
 					      __func__, NULL);
 			if (!up) {
 				if (PIM_DEBUG_MROUTE)
-					zlog_debug("%s: Unable to create upstream information for %pSG",
-						   __func__, &sg);
+					zlog_debug(
+						"%s: Unable to create upstream information for %pSG",
+						__func__, &sg);
 				return 0;
 			}
 			pim_upstream_keep_alive_timer_start(
@@ -283,8 +182,9 @@ static int pim_mroute_msg_wholepkt(int fd, struct interface *ifp,
 			return 0;
 		}
 		if (PIM_DEBUG_MROUTE_DETAIL) {
-			zlog_debug("%s: Unable to find upstream channel WHOLEPKT%pSG",
-				   __func__, &sg);
+			zlog_debug(
+				"%s: Unable to find upstream channel WHOLEPKT%pSG",
+				__func__, &sg);
 		}
 		return 0;
 	}
@@ -314,8 +214,9 @@ static int pim_mroute_msg_wholepkt(int fd, struct interface *ifp,
 	if (!up->t_rs_timer) {
 		if (pim_is_grp_ssm(pim_ifp->pim, sg.grp)) {
 			if (PIM_DEBUG_PIM_REG)
-				zlog_debug("%pSG register forward skipped as group is SSM",
-					   &sg);
+				zlog_debug(
+					"%pSG register forward skipped as group is SSM",
+					&sg);
 			return 0;
 		}
 
@@ -327,23 +228,22 @@ static int pim_mroute_msg_wholepkt(int fd, struct interface *ifp,
 			return 0;
 		}
 
-		pim_register_send((uint8_t *)buf + sizeof(struct ip),
-				  ntohs(ip_hdr->ip_len) - sizeof(struct ip),
+		pim_register_send((uint8_t *)buf + sizeof(ipv_hdr),
+				  ntohs(IPV_LEN(ip_hdr)) - sizeof(ipv_hdr),
 				  pim_ifp->primary_address, rpg, 0, up);
 	}
 	return 0;
 }
 
-static int pim_mroute_msg_wrongvif(int fd, struct interface *ifp,
-				   const struct igmpmsg *msg)
+int pim_mroute_msg_wrongvif(int fd, struct interface *ifp, const kernmsg *msg)
 {
 	struct pim_ifchannel *ch;
 	struct pim_interface *pim_ifp;
 	pim_sgaddr sg;
 
 	memset(&sg, 0, sizeof(sg));
-	sg.src = msg->im_src;
-	sg.grp = msg->im_dst;
+	sg.src = msg->msg_im_src;
+	sg.grp = msg->msg_im_dst;
 
 	/*
 	  Send Assert(S,G) on iif as response to WRONGVIF kernel upcall.
@@ -358,16 +258,18 @@ static int pim_mroute_msg_wrongvif(int fd, struct interface *ifp,
 
 	if (!ifp) {
 		if (PIM_DEBUG_MROUTE)
-			zlog_debug("%s: WRONGVIF (S,G)=%pSG could not find input interface for input_vif_index=%d",
-				   __func__, &sg, msg->im_vif);
+			zlog_debug(
+				"%s: WRONGVIF (S,G)=%pSG could not find input interface for input_vif_index=%d",
+				__func__, &sg, msg->msg_im_vif);
 		return -1;
 	}
 
 	pim_ifp = ifp->info;
 	if (!pim_ifp) {
 		if (PIM_DEBUG_MROUTE)
-			zlog_debug("%s: WRONGVIF (S,G)=%pSG multicast not enabled on interface %s",
-				   __func__, &sg, ifp->name);
+			zlog_debug(
+				"%s: WRONGVIF (S,G)=%pSG multicast not enabled on interface %s",
+				__func__, &sg, ifp->name);
 		return -2;
 	}
 
@@ -375,16 +277,17 @@ static int pim_mroute_msg_wrongvif(int fd, struct interface *ifp,
 	if (!ch) {
 		pim_sgaddr star_g = sg;
 		if (PIM_DEBUG_MROUTE)
-			zlog_debug("%s: WRONGVIF (S,G)=%pSG could not find channel on interface %s",
-				   __func__, &sg, ifp->name);
+			zlog_debug(
+				"%s: WRONGVIF (S,G)=%pSG could not find channel on interface %s",
+				__func__, &sg, ifp->name);
 
 		star_g.src = PIMADDR_ANY;
 		ch = pim_ifchannel_find(ifp, &star_g);
 		if (!ch) {
 			if (PIM_DEBUG_MROUTE)
-				zlog_debug("%s: WRONGVIF (*,G)=%pSG could not find channel on interface %s",
-					   __func__, &star_g,
-					   ifp->name);
+				zlog_debug(
+					"%s: WRONGVIF (*,G)=%pSG could not find channel on interface %s",
+					__func__, &star_g, ifp->name);
 			return -3;
 		}
 	}
@@ -433,10 +336,9 @@ static int pim_mroute_msg_wrongvif(int fd, struct interface *ifp,
 	return 0;
 }
 
-static int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp,
-				     const char *buf)
+int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp, const char *buf)
 {
-	const struct ip *ip_hdr = (const struct ip *)buf;
+	const ipv_hdr *ip_hdr = (const ipv_hdr *)buf;
 	struct pim_interface *pim_ifp;
 	struct pim_instance *pim;
 	struct pim_ifchannel *ch;
@@ -447,8 +349,8 @@ static int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp,
 	pim_ifp = ifp->info;
 
 	memset(&sg, 0, sizeof(sg));
-	sg.src = ip_hdr->ip_src;
-	sg.grp = ip_hdr->ip_dst;
+	sg.src = IPV_SRC(ip_hdr);
+	sg.grp = IPV_DST(ip_hdr);
 
 	ch = pim_ifchannel_find(ifp, &sg);
 	if (ch) {
@@ -481,8 +383,8 @@ static int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp,
 		struct pim_rpf *rpf = RP(pim_ifp->pim, sg.grp);
 
 		/* No RPF or No RPF interface or No mcast on RPF interface */
-		if (!rpf || !rpf->source_nexthop.interface
-		    || !rpf->source_nexthop.interface->info)
+		if (!rpf || !rpf->source_nexthop.interface ||
+		    !rpf->source_nexthop.interface->info)
 			return 0;
 
 		/*
@@ -512,7 +414,7 @@ static int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp,
 			 * bow out of doing a nexthop lookup and
 			 * setting the SPTBIT to true
 			 */
-			if (up->upstream_register.s_addr != INADDR_ANY &&
+			if (!(pim_addr_is_any(up->upstream_register)) &&
 			    pim_nexthop_lookup(pim_ifp->pim, &source,
 					       up->upstream_register, 0)) {
 				pim_register_stop_send(source.interface, &sg,
@@ -551,8 +453,9 @@ static int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp,
 				      NULL);
 		if (!up) {
 			if (PIM_DEBUG_MROUTE)
-				zlog_debug("%pSG: WRONGVIF%s unable to create upstream on interface",
-					   &sg, ifp->name);
+				zlog_debug(
+					"%pSG: WRONGVIF%s unable to create upstream on interface",
+					&sg, ifp->name);
 			return -2;
 		}
 		PIM_UPSTREAM_FLAG_SET_SRC_STREAM(up->flags);
@@ -577,119 +480,6 @@ static int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp,
 	return 0;
 }
 
-static int pim_mroute_msg(struct pim_instance *pim, const char *buf,
-			  int buf_size, ifindex_t ifindex)
-{
-	struct interface *ifp;
-	const struct ip *ip_hdr;
-	const struct igmpmsg *msg;
-
-	if (buf_size < (int)sizeof(struct ip))
-		return 0;
-
-	ip_hdr = (const struct ip *)buf;
-
-	if (ip_hdr->ip_p == IPPROTO_IGMP) {
-#if PIM_IPV == 4
-		struct pim_interface *pim_ifp;
-		struct in_addr ifaddr;
-		struct gm_sock *igmp;
-		const struct prefix *connected_src;
-
-		/* We have the IP packet but we do not know which interface this
-		 * packet was
-		 * received on. Find the interface that is on the same subnet as
-		 * the source
-		 * of the IP packet.
-		 */
-		ifp = if_lookup_by_index(ifindex, pim->vrf->vrf_id);
-
-		if (!ifp || !ifp->info)
-			return 0;
-
-		connected_src = pim_if_connected_to_source(ifp, ip_hdr->ip_src);
-
-		if (!connected_src && !pim_addr_is_any(ip_hdr->ip_src)) {
-			if (PIM_DEBUG_IGMP_PACKETS) {
-				zlog_debug("Recv IGMP packet on interface: %s from a non-connected source: %pI4",
-					   ifp->name, &ip_hdr->ip_src);
-			}
-			return 0;
-		}
-
-		pim_ifp = ifp->info;
-		ifaddr = connected_src ? connected_src->u.prefix4
-				       : pim_ifp->primary_address;
-		igmp = pim_igmp_sock_lookup_ifaddr(pim_ifp->gm_socket_list,
-						   ifaddr);
-
-		if (PIM_DEBUG_IGMP_PACKETS) {
-			zlog_debug(
-				"%s(%s): igmp kernel upcall on %s(%p) for %pI4 -> %pI4",
-				__func__, pim->vrf->name, ifp->name, igmp,
-				&ip_hdr->ip_src, &ip_hdr->ip_dst);
-		}
-		if (igmp)
-			pim_igmp_packet(igmp, (char *)buf, buf_size);
-		else if (PIM_DEBUG_IGMP_PACKETS) {
-			zlog_debug(
-				"No IGMP socket on interface: %s with connected source: %pI4",
-				ifp->name, &ifaddr);
-		}
-#endif
-	} else if (ip_hdr->ip_p) {
-		if (PIM_DEBUG_MROUTE_DETAIL) {
-			zlog_debug(
-				"%s: no kernel upcall proto=%d src: %pI4 dst: %pI4 msg_size=%d",
-				__func__, ip_hdr->ip_p, &ip_hdr->ip_src, &ip_hdr->ip_dst,
-				buf_size);
-		}
-
-	} else {
-		msg = (const struct igmpmsg *)buf;
-
-		ifp = pim_if_find_by_vif_index(pim, msg->im_vif);
-
-		if (!ifp)
-			return 0;
-		if (PIM_DEBUG_MROUTE) {
-			zlog_debug(
-				"%s: pim kernel upcall %s type=%d ip_p=%d from fd=%d for (S,G)=(%pI4,%pI4) on %s vifi=%d  size=%d",
-				__func__, igmpmsgtype2str[msg->im_msgtype],
-				msg->im_msgtype, ip_hdr->ip_p,
-				pim->mroute_socket, &msg->im_src, &msg->im_dst, ifp->name,
-				msg->im_vif, buf_size);
-		}
-
-		switch (msg->im_msgtype) {
-		case IGMPMSG_WRONGVIF:
-			return pim_mroute_msg_wrongvif(pim->mroute_socket, ifp,
-						       msg);
-		case IGMPMSG_NOCACHE:
-			return pim_mroute_msg_nocache(pim->mroute_socket, ifp,
-						      msg);
-		case IGMPMSG_WHOLEPKT:
-			return pim_mroute_msg_wholepkt(pim->mroute_socket, ifp,
-						       (const char *)msg);
-		case IGMPMSG_WRVIFWHOLE:
-			return pim_mroute_msg_wrvifwhole(
-				pim->mroute_socket, ifp, (const char *)msg);
-		default:
-			break;
-		}
-	}
-
-	return 0;
-}
-#else /* PIM_IPV != 4 */
-
-static int pim_mroute_msg(struct pim_instance *pim, const char *buf,
-			  int buf_size, ifindex_t ifindex)
-{
-	return 0;
-}
-#endif /* PIM_IPV != 4 */
-
 static void mroute_read(struct thread *t)
 {
 	struct pim_instance *pim;
@@ -726,6 +516,8 @@ static void mroute_read(struct thread *t)
 /* Keep reading */
 done:
 	mroute_read_on(pim);
+
+	return;
 }
 
 static void mroute_read_on(struct pim_instance *pim)
@@ -745,8 +537,11 @@ int pim_mroute_socket_enable(struct pim_instance *pim)
 
 	frr_with_privs(&pimd_privs) {
 
+#if PIM_IPV == 4
 		fd = socket(AF_INET, SOCK_RAW, IPPROTO_IGMP);
-
+#else
+		fd = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6);
+#endif
 		if (fd < 0) {
 			zlog_warn("Could not create mroute socket: errno=%d: %s",
 				  errno,
@@ -814,7 +609,7 @@ int pim_mroute_add_vif(struct interface *ifp, pim_addr ifaddr,
 		       unsigned char flags)
 {
 	struct pim_interface *pim_ifp = ifp->info;
-	struct vifctl vc;
+	pim_vifctl vc;
 	int err;
 
 	if (PIM_DEBUG_MROUTE)
@@ -823,9 +618,10 @@ int pim_mroute_add_vif(struct interface *ifp, pim_addr ifaddr,
 			   pim_ifp->pim->vrf->name);
 
 	memset(&vc, 0, sizeof(vc));
-	vc.vifc_vifi = pim_ifp->mroute_vif_index;
+	vc.vc_vifi = pim_ifp->mroute_vif_index;
+#if PIM_IPV == 4
 #ifdef VIFF_USE_IFINDEX
-	vc.vifc_lcl_ifindex = ifp->ifindex;
+	vc.vc_lcl_ifindex = ifp->ifindex;
 #else
 	if (ifaddr.s_addr == INADDR_ANY) {
 		zlog_warn(
@@ -833,24 +629,29 @@ int pim_mroute_add_vif(struct interface *ifp, pim_addr ifaddr,
 			__func__);
 		return -1;
 	}
-	memcpy(&vc.vifc_lcl_addr, &ifaddr, sizeof(vc.vifc_lcl_addr));
+	memcpy(&vc.vc_lcl_addr, &ifaddr, sizeof(vc.vc_lcl_addr));
+#endif
+#else
+	vc.vc_pifi = ifp->ifindex;
 #endif
-	vc.vifc_flags = flags;
-	vc.vifc_threshold = PIM_MROUTE_MIN_TTL;
-	vc.vifc_rate_limit = 0;
+	vc.vc_flags = flags;
+	vc.vc_threshold = PIM_MROUTE_MIN_TTL;
+	vc.vc_rate_limit = 0;
 
+#if PIM_IPV == 4
 #ifdef PIM_DVMRP_TUNNEL
-	if (vc.vifc_flags & VIFF_TUNNEL) {
-		memcpy(&vc.vifc_rmt_addr, &vif_remote_addr,
-		       sizeof(vc.vifc_rmt_addr));
+	if (vc.vc_flags & VIFF_TUNNEL) {
+		memcpy(&vc.vc_rmt_addr, &vif_remote_addr,
+		       sizeof(vc.vc_rmt_addr));
 	}
 #endif
+#endif
 
-	err = setsockopt(pim_ifp->pim->mroute_socket, IPPROTO_IP, MRT_ADD_VIF,
+	err = setsockopt(pim_ifp->pim->mroute_socket, PIM_IPPROTO, MRT_ADD_VIF,
 			 (void *)&vc, sizeof(vc));
 	if (err) {
 		zlog_warn(
-			"%s: failure: setsockopt(fd=%d,IPPROTO_IP,MRT_ADD_VIF,vif_index=%d,ifaddr=%pPAs,flag=%d): errno=%d: %s",
+			"%s: failure: setsockopt(fd=%d,PIM_IPPROTO,MRT_ADD_VIF,vif_index=%d,ifaddr=%pPAs,flag=%d): errno=%d: %s",
 			__func__, pim_ifp->pim->mroute_socket, ifp->ifindex,
 			&ifaddr, flags, errno, safe_strerror(errno));
 		return -2;
@@ -862,7 +663,7 @@ int pim_mroute_add_vif(struct interface *ifp, pim_addr ifaddr,
 int pim_mroute_del_vif(struct interface *ifp)
 {
 	struct pim_interface *pim_ifp = ifp->info;
-	struct vifctl vc;
+	pim_vifctl vc;
 	int err;
 
 	if (PIM_DEBUG_MROUTE)
@@ -871,13 +672,13 @@ int pim_mroute_del_vif(struct interface *ifp)
 			   pim_ifp->pim->vrf->name);
 
 	memset(&vc, 0, sizeof(vc));
-	vc.vifc_vifi = pim_ifp->mroute_vif_index;
+	vc.vc_vifi = pim_ifp->mroute_vif_index;
 
-	err = setsockopt(pim_ifp->pim->mroute_socket, IPPROTO_IP, MRT_DEL_VIF,
+	err = setsockopt(pim_ifp->pim->mroute_socket, PIM_IPPROTO, MRT_DEL_VIF,
 			 (void *)&vc, sizeof(vc));
 	if (err) {
 		zlog_warn(
-			"%s %s: failure: setsockopt(fd=%d,IPPROTO_IP,MRT_DEL_VIF,vif_index=%d): errno=%d: %s",
+			"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO,MRT_DEL_VIF,vif_index=%d): errno=%d: %s",
 			__FILE__, __func__, pim_ifp->pim->mroute_socket,
 			pim_ifp->mroute_vif_index, errno, safe_strerror(errno));
 		return -2;
@@ -985,20 +786,21 @@ static int pim_mroute_add(struct channel_oil *c_oil, const char *name)
 	    && *oil_parent(c_oil) != 0) {
 		*oil_parent(tmp_oil) = 0;
 	}
-	err = setsockopt(pim->mroute_socket, IPPROTO_IP, MRT_ADD_MFC,
+	/* For IPv6 MRT_ADD_MFC is defined to MRT6_ADD_MFC */
+	err = setsockopt(pim->mroute_socket, PIM_IPPROTO, MRT_ADD_MFC,
 			 &tmp_oil->oil, sizeof(tmp_oil->oil));
 
 	if (!err && !c_oil->installed
 	    && !pim_addr_is_any(*oil_origin(c_oil))
 	    && *oil_parent(c_oil) != 0) {
 		*oil_parent(tmp_oil) = *oil_parent(c_oil);
-		err = setsockopt(pim->mroute_socket, IPPROTO_IP, MRT_ADD_MFC,
+		err = setsockopt(pim->mroute_socket, PIM_IPPROTO, MRT_ADD_MFC,
 				 &tmp_oil->oil, sizeof(tmp_oil->oil));
 	}
 
 	if (err) {
 		zlog_warn(
-			"%s %s: failure: setsockopt(fd=%d,IPPROTO_IP,MRT_ADD_MFC): errno=%d: %s",
+			"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO,MRT_ADD_MFC): errno=%d: %s",
 			__FILE__, __func__, pim->mroute_socket, errno,
 			safe_strerror(errno));
 		return -2;
@@ -1174,12 +976,12 @@ int pim_mroute_del(struct channel_oil *c_oil, const char *name)
 		return -2;
 	}
 
-	err = setsockopt(pim->mroute_socket, IPPROTO_IP, MRT_DEL_MFC,
+	err = setsockopt(pim->mroute_socket, PIM_IPPROTO, MRT_DEL_MFC,
 			 &c_oil->oil, sizeof(c_oil->oil));
 	if (err) {
 		if (PIM_DEBUG_MROUTE)
 			zlog_warn(
-				"%s %s: failure: setsockopt(fd=%d,IPPROTO_IP,MRT_DEL_MFC): errno=%d: %s",
+				"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO,MRT_DEL_MFC): errno=%d: %s",
 				__FILE__, __func__, pim->mroute_socket, errno,
 				safe_strerror(errno));
 		return -2;
@@ -1201,6 +1003,7 @@ int pim_mroute_del(struct channel_oil *c_oil, const char *name)
 void pim_mroute_update_counters(struct channel_oil *c_oil)
 {
 	struct pim_instance *pim = c_oil->pim;
+	pim_sioc_sg_req sgreq;
 
 	c_oil->cc.oldpktcnt = c_oil->cc.pktcnt;
 	c_oil->cc.oldbytecnt = c_oil->cc.bytecnt;
@@ -1219,29 +1022,33 @@ void pim_mroute_update_counters(struct channel_oil *c_oil)
 		return;
 	}
 
-#if PIM_IPV == 4
-	struct sioc_sg_req sgreq;
 
 	memset(&sgreq, 0, sizeof(sgreq));
+
+#if PIM_IPV == 4
 	sgreq.src = *oil_origin(c_oil);
 	sgreq.grp = *oil_mcastgrp(c_oil);
-
 	pim_zlookup_sg_statistics(c_oil);
-	if (ioctl(pim->mroute_socket, SIOCGETSGCNT, &sgreq)) {
+#else
+	sgreq.src = c_oil->oil.mf6cc_origin;
+	sgreq.grp = c_oil->oil.mf6cc_mcastgrp;
+	/* TODO Zlookup_sg_statistics for V6 to be added */
+#endif
+	if (ioctl(pim->mroute_socket, PIM_SIOCGETSGCNT, &sgreq)) {
 		pim_sgaddr sg;
 
 		sg.src = *oil_origin(c_oil);
 		sg.grp = *oil_mcastgrp(c_oil);
 
-		zlog_warn("ioctl(SIOCGETSGCNT=%lu) failure for (S,G)=%pSG: errno=%d: %s",
-			  (unsigned long)SIOCGETSGCNT, &sg,
-			  errno, safe_strerror(errno));
+		zlog_warn(
+			"ioctl(PIM_SIOCGETSGCNT=%lu) failure for (S,G)=%pSG: errno=%d: %s",
+			(unsigned long)PIM_SIOCGETSGCNT, &sg, errno,
+			safe_strerror(errno));
 		return;
 	}
 
 	c_oil->cc.pktcnt = sgreq.pktcnt;
 	c_oil->cc.bytecnt = sgreq.bytecnt;
 	c_oil->cc.wrong_if = sgreq.wrong_if;
-#endif
 	return;
 }
diff --git a/pimd/pim_mroute.h b/pimd/pim_mroute.h
index 14b0a8cca..35ba60bf3 100644
--- a/pimd/pim_mroute.h
+++ b/pimd/pim_mroute.h
@@ -30,153 +30,107 @@
 #define __EXTENSIONS__
 #endif
 
-#include <netinet/in.h>
-#ifdef HAVE_NETINET_IP_MROUTE_H
-#include <netinet/ip_mroute.h>
-#endif
 
 #define PIM_MROUTE_MIN_TTL (1)
 
 #if PIM_IPV == 4
+
+#include <netinet/in.h>
 #if defined(HAVE_LINUX_MROUTE_H)
 #include <linux/mroute.h>
 #else
-/*
-  Below: from <linux/mroute.h>
-*/
-
-#ifndef MAXVIFS
-#define MAXVIFS (256)
+#ifndef VTYSH_EXTRACT_PL
+#include "linux/mroute.h"
 #endif
-
-#ifndef SIOCGETVIFCNT
-#define SIOCGETVIFCNT   SIOCPROTOPRIVATE        /* IP protocol privates */
-#define SIOCGETSGCNT    (SIOCPROTOPRIVATE+1)
-#define SIOCGETRPF      (SIOCPROTOPRIVATE+2)
 #endif
 
-#ifndef MRT_INIT
-#define MRT_BASE     200
-#define MRT_INIT     (MRT_BASE)      /* Activate the kernel mroute code      */
-#define MRT_DONE     (MRT_BASE+1)    /* Shutdown the kernel mroute           */
-#define MRT_ADD_VIF  (MRT_BASE+2)    /* Add a virtual interface              */
-#define MRT_DEL_VIF  (MRT_BASE+3)    /* Delete a virtual interface           */
-#define MRT_ADD_MFC  (MRT_BASE+4)    /* Add a multicast forwarding entry     */
-#define MRT_DEL_MFC  (MRT_BASE+5)    /* Delete a multicast forwarding entry  */
-#define MRT_VERSION  (MRT_BASE+6)    /* Get the kernel multicast version     */
-#define MRT_ASSERT   (MRT_BASE+7)    /* Activate PIM assert mode             */
-#define MRT_PIM      (MRT_BASE+8)    /* enable PIM code      */
-#endif
+typedef struct vifctl pim_vifctl;
+typedef struct igmpmsg kernmsg;
+typedef struct sioc_sg_req pim_sioc_sg_req;
 
-#ifndef MRT_TABLE
-#define MRT_TABLE    (209)           /* Specify mroute table ID */
-#endif
+#define vc_vifi vifc_vifi
+#define vc_flags vifc_flags
+#define vc_threshold vifc_threshold
+#define vc_rate_limit vifc_rate_limit
+#define vc_lcl_addr vifc_lcl_addr
+#define vc_lcl_ifindex vifc_lcl_ifindex
+#define vc_rmt_addr vifc_rmt_addr
 
-#ifndef HAVE_VIFI_T
-typedef unsigned short vifi_t;
-#endif
+#define msg_im_vif im_vif
+#define msg_im_src im_src
+#define msg_im_dst im_dst
 
-#ifndef HAVE_STRUCT_VIFCTL
-struct vifctl {
-	vifi_t vifc_vifi;	     /* Index of VIF */
-	unsigned char vifc_flags;     /* VIFF_ flags */
-	unsigned char vifc_threshold; /* ttl limit */
-	unsigned int vifc_rate_limit; /* Rate limiter values (NI) */
-	struct in_addr vifc_lcl_addr; /* Our address */
-	struct in_addr vifc_rmt_addr; /* IPIP tunnel addr */
-};
+#ifndef IGMPMSG_WRVIFWHOLE
+#define IGMPMSG_WRVIFWHOLE 4 /* For PIM processing */
 #endif
 
-#ifndef HAVE_STRUCT_MFCCTL
-struct mfcctl {
-	struct in_addr mfcc_origin;       /* Origin of mcast      */
-	struct in_addr mfcc_mcastgrp;     /* Group in question    */
-	vifi_t mfcc_parent;		  /* Where it arrived     */
-	unsigned char mfcc_ttls[MAXVIFS]; /* Where it is going    */
-	unsigned int mfcc_pkt_cnt;	/* pkt count for src-grp */
-	unsigned int mfcc_byte_cnt;
-	unsigned int mfcc_wrong_if;
-	int mfcc_expire;
-};
+#ifndef PIM_IPPROTO
+#define PIM_IPPROTO IPPROTO_IP
+#endif
+#ifndef PIM_SIOCGETSGCNT
+#define PIM_SIOCGETSGCNT SIOCGETSGCNT
 #endif
 
-/*
- *      Group count retrieval for mrouted
- */
-/*
-  struct sioc_sg_req sgreq;
-  memset(&sgreq, 0, sizeof(sgreq));
-  memcpy(&sgreq.src, &source_addr, sizeof(sgreq.src));
-  memcpy(&sgreq.grp, &group_addr, sizeof(sgreq.grp));
-  ioctl(mrouter_s4, SIOCGETSGCNT, &sgreq);
- */
-#ifndef HAVE_STRUCT_SIOC_SG_REQ
-struct sioc_sg_req {
-	struct in_addr src;
-	struct in_addr grp;
-	unsigned long pktcnt;
-	unsigned long bytecnt;
-	unsigned long wrong_if;
-};
+#else /* PIM_IPV != 4 */
+
+#include <netinet/ip6.h>
+
+#if defined(HAVE_LINUX_MROUTE6_H)
+#include <linux/mroute6.h>
+#else
+#ifndef VTYSH_EXTRACT_PL
+#include "linux/mroute6.h"
+#endif
 #endif
 
-/*
- *      To get vif packet counts
- */
-/*
-  struct sioc_vif_req vreq;
-  memset(&vreq, 0, sizeof(vreq));
-  vreq.vifi = vif_index;
-  ioctl(mrouter_s4, SIOCGETVIFCNT, &vreq);
- */
-#ifndef HAVE_STRUCT_SIOC_VIF_REQ
-struct sioc_vif_req {
-	vifi_t vifi;	  /* Which iface */
-	unsigned long icount; /* In packets */
-	unsigned long ocount; /* Out packets */
-	unsigned long ibytes; /* In bytes */
-	unsigned long obytes; /* Out bytes */
-};
+#ifndef MRT_INIT
+#define MRT_BASE MRT6_BASE
+#define MRT_INIT MRT6_INIT
+#define MRT_DONE MRT6_DONE
+#define MRT_ADD_VIF MRT6_ADD_MIF
+#define MRT_DEL_VIF MRT6_DEL_MIF
+#define MRT_ADD_MFC MRT6_ADD_MFC
+#define MRT_DEL_MFC MRT6_DEL_MFC
+#define MRT_VERSION MRT6_VERSION
+#define MRT_ASSERT MRT6_ASSERT
+#define MRT_PIM MRT6_PIM
 #endif
 
-/*
- *      Pseudo messages used by mrouted
- */
-#ifndef IGMPMSG_NOCACHE
-#define IGMPMSG_NOCACHE         1               /* Kern cache fill request to mrouted */
-#define IGMPMSG_WRONGVIF        2               /* For PIM assert processing (unused) */
-#define IGMPMSG_WHOLEPKT        3               /* For PIM Register processing */
+#ifndef PIM_IPPROTO
+#define PIM_IPPROTO IPPROTO_IPV6
 #endif
 
-#ifndef HAVE_STRUCT_IGMPMSG
-struct igmpmsg {
-	uint32_t unused1, unused2;
-	unsigned char im_msgtype; /* What is this */
-	unsigned char im_mbz;     /* Must be zero */
-	unsigned char im_vif;     /* Interface (this ought to be a vifi_t!) */
-	unsigned char unused3;
-	struct in_addr im_src, im_dst;
-};
+#ifndef PIM_SIOCGETSGCNT
+#define PIM_SIOCGETSGCNT SIOCGETSGCNT_IN6
 #endif
 
-#endif /* HAVE_LINUX_MROUTE_H */
+#ifndef MRT6MSG_WRMIFWHOLE
+#define MRT6MSG_WRMIFWHOLE 4 /* For PIM processing */
+#endif
 
-typedef struct mfcctl pim_mfcctl;
+typedef struct mif6ctl pim_vifctl;
+typedef struct mrt6msg kernmsg;
+typedef mifi_t vifi_t;
+typedef struct sioc_sg_req6 pim_sioc_sg_req;
 
-#else /* PIM_IPV != 4 */
-#if defined(HAVE_LINUX_MROUTE6_H)
-#include <linux/mroute6.h>
-#endif
+#define vc_vifi mif6c_mifi
+#define vc_flags mif6c_flags
+#define vc_threshold vifc_threshold
+#define vc_pifi mif6c_pifi
+#define vc_rate_limit vifc_rate_limit
 
-typedef struct mf6cctl pim_mfcctl;
+#define msg_im_vif im6_mif
+#define msg_im_src im6_src
+#define msg_im_dst im6_dst
 
+#ifndef MAXVIFS
 #define MAXVIFS IF_SETSIZE
 #endif
 
-#ifndef IGMPMSG_WRVIFWHOLE
-#define IGMPMSG_WRVIFWHOLE      4               /* For PIM processing */
+#define VIFF_REGISTER MIFF_REGISTER
 #endif
 
+
 /*
   Above: from <linux/mroute.h>
 */
@@ -201,4 +155,11 @@ int pim_mroute_del(struct channel_oil *c_oil, const char *name);
 void pim_mroute_update_counters(struct channel_oil *c_oil);
 bool pim_mroute_allow_iif_in_oil(struct channel_oil *c_oil,
 		int oif_index);
+int pim_mroute_msg(struct pim_instance *pim, const char *buf, size_t buf_size,
+		   ifindex_t ifindex);
+int pim_mroute_msg_nocache(int fd, struct interface *ifp, const kernmsg *msg);
+int pim_mroute_msg_wholepkt(int fd, struct interface *ifp, const char *buf);
+int pim_mroute_msg_wrongvif(int fd, struct interface *ifp, const kernmsg *msg);
+int pim_mroute_msg_wrvifwhole(int fd, struct interface *ifp, const char *buf);
+int pim_mroute_set(struct pim_instance *pim, int enable);
 #endif /* PIM_MROUTE_H */
diff --git a/pimd/pim_mroute_msg.c b/pimd/pim_mroute_msg.c
new file mode 100644
index 000000000..7d80488c6
--- /dev/null
+++ b/pimd/pim_mroute_msg.c
@@ -0,0 +1,239 @@
+/*
+ * PIM for Quagga
+ * Copyright (C) 2022  Dell Technologies Ltd
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; see the file COPYING; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <zebra.h>
+#include "log.h"
+#include "privs.h"
+#include "if.h"
+#include "prefix.h"
+#include "vty.h"
+#include "plist.h"
+#include "sockopt.h"
+#include "lib_errors.h"
+#include "lib/network.h"
+
+#include "pimd.h"
+#include "pim_mroute.h"
+#include "pim_oil.h"
+#include "pim_str.h"
+#include "pim_iface.h"
+#include "pim_macro.h"
+#include "pim_rp.h"
+#include "pim_oil.h"
+#include "pim_msg.h"
+#include "pim_sock.h"
+
+
+int pim_mroute_set(struct pim_instance *pim, int enable)
+{
+	int err;
+	int opt, data;
+	socklen_t data_len = sizeof(data);
+
+	/*
+	 * We need to create the VRF table for the pim mroute_socket
+	 */
+	if (pim->vrf->vrf_id != VRF_DEFAULT) {
+		frr_with_privs(&pimd_privs) {
+
+			data = pim->vrf->data.l.table_id;
+			err = setsockopt(pim->mroute_socket, PIM_IPPROTO,
+					MRT_TABLE, &data, data_len);
+			if (err) {
+				zlog_warn(
+					"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO, MRT_TABLE=%d): errno=%d: %s",
+					__FILE__, __func__, pim->mroute_socket,
+					data, errno, safe_strerror(errno));
+				return -1;
+			}
+
+		}
+	}
+
+	frr_with_privs(&pimd_privs) {
+		opt = enable ? MRT_INIT : MRT_DONE;
+		/*
+		 * *BSD *cares* about what value we pass down
+		 * here
+		 */
+		data = 1;
+		err = setsockopt(pim->mroute_socket, PIM_IPPROTO, opt, &data,
+				data_len);
+		if (err) {
+			zlog_warn(
+				"%s %s: failure: setsockopt(fd=%d,PIM_IPPROTO,%s=%d): errno=%d: %s",
+				__FILE__, __func__, pim->mroute_socket,
+				enable ? "MRT_INIT" : "MRT_DONE", data, errno,
+				safe_strerror(errno));
+			return -1;
+		}
+	}
+
+#if defined(HAVE_IP_PKTINFO)
+	if (enable) {
+		/* Linux and Solaris IP_PKTINFO */
+		data = 1;
+		if (setsockopt(pim->mroute_socket, PIM_IPPROTO, IP_PKTINFO,
+				&data, data_len)) {
+			zlog_warn(
+				"Could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
+				pim->mroute_socket, errno,
+				safe_strerror(errno));
+		}
+	}
+#endif
+
+	setsockopt_so_recvbuf(pim->mroute_socket, 1024 * 1024 * 8);
+
+	if (set_nonblocking (pim->mroute_socket) < 0) {
+		zlog_warn(
+			"Could not set non blocking on socket fd=%d: errno=%d: %s",
+			pim->mroute_socket, errno,
+			safe_strerror(errno));
+	}
+
+	if (enable) {
+#if defined linux
+		int upcalls = IGMPMSG_WRVIFWHOLE;
+		opt = MRT_PIM;
+
+		err = setsockopt(pim->mroute_socket, PIM_IPPROTO, opt, &upcalls,
+				sizeof(upcalls));
+		if (err) {
+			zlog_warn(
+				"Failure to register for VIFWHOLE and WRONGVIF upcalls %d %s",
+				errno, safe_strerror(errno));
+			return -1;
+		}
+#else
+		zlog_warn(
+			"PIM-SM will not work properly on this platform, until the ability to receive the WRVIFWHOLE upcall");
+#endif
+
+	}
+
+	return 0;
+}
+
+static const char *const igmpmsgtype2str[IGMPMSG_WRVIFWHOLE + 1] = {
+	"<unknown_upcall?>", "NOCACHE", "WRONGVIF", "WHOLEPKT", "WRVIFWHOLE"};
+
+
+int pim_mroute_msg(struct pim_instance *pim, const char *buf,
+			  size_t buf_size, ifindex_t ifindex)
+{
+	struct interface *ifp;
+	const struct ip *ip_hdr;
+	const struct igmpmsg *msg;
+
+	if (buf_size < (int)sizeof(struct ip))
+		return 0;
+
+	ip_hdr = (const struct ip *)buf;
+
+	if (ip_hdr->ip_p == IPPROTO_IGMP) {
+		struct pim_interface *pim_ifp;
+		struct in_addr ifaddr;
+		struct gm_sock *igmp;
+		const struct prefix *connected_src;
+
+		/* We have the IP packet but we do not know which interface this
+		 * packet was
+		 * received on. Find the interface that is on the same subnet as
+		 * the source
+		 * of the IP packet.
+		 */
+		ifp = if_lookup_by_index(ifindex, pim->vrf->vrf_id);
+
+		if (!ifp || !ifp->info)
+			return 0;
+
+		connected_src = pim_if_connected_to_source(ifp, ip_hdr->ip_src);
+
+		if (!connected_src) {
+			if (PIM_DEBUG_IGMP_PACKETS) {
+				zlog_debug(
+					"Recv IGMP packet on interface: %s from a non-connected source: %pI4",
+					ifp->name, &ip_hdr->ip_src);
+			}
+			return 0;
+		}
+
+		pim_ifp = ifp->info;
+		ifaddr = connected_src->u.prefix4;
+		igmp = pim_igmp_sock_lookup_ifaddr(pim_ifp->gm_socket_list,
+						   ifaddr);
+
+		if (PIM_DEBUG_IGMP_PACKETS) {
+			zlog_debug(
+				"%s(%s): igmp kernel upcall on %s(%p) for %pI4 -> %pI4",
+				__func__, pim->vrf->name, ifp->name, igmp,
+				&ip_hdr->ip_src, &ip_hdr->ip_dst);
+		}
+		if (igmp)
+			pim_igmp_packet(igmp, (char *)buf, buf_size);
+		else if (PIM_DEBUG_IGMP_PACKETS) {
+			zlog_debug(
+				"No IGMP socket on interface: %s with connected source: %pFX",
+				ifp->name, connected_src);
+		}
+	} else if (ip_hdr->ip_p) {
+		if (PIM_DEBUG_MROUTE_DETAIL) {
+			zlog_debug(
+				"%s: no kernel upcall proto=%d src: %pI4 dst: %pI4 msg_size=%ld",
+				__func__, ip_hdr->ip_p, &ip_hdr->ip_src,
+				&ip_hdr->ip_dst, (long int)buf_size);
+		}
+
+	} else {
+		msg = (const struct igmpmsg *)buf;
+
+		ifp = pim_if_find_by_vif_index(pim, msg->im_vif);
+
+		if (!ifp)
+			return 0;
+		if (PIM_DEBUG_MROUTE) {
+			zlog_debug(
+				"%s: pim kernel upcall %s type=%d ip_p=%d from fd=%d for (S,G)=(%pI4,%pI4) on %s vifi=%d  size=%ld",
+				__func__, igmpmsgtype2str[msg->im_msgtype],
+				msg->im_msgtype, ip_hdr->ip_p,
+				pim->mroute_socket, &msg->im_src, &msg->im_dst,
+				ifp->name, msg->im_vif, (long int)buf_size);
+		}
+
+		switch (msg->im_msgtype) {
+		case IGMPMSG_WRONGVIF:
+			return pim_mroute_msg_wrongvif(pim->mroute_socket, ifp,
+						       msg);
+		case IGMPMSG_NOCACHE:
+			return pim_mroute_msg_nocache(pim->mroute_socket, ifp,
+						      msg);
+		case IGMPMSG_WHOLEPKT:
+			return pim_mroute_msg_wholepkt(pim->mroute_socket, ifp,
+						       (const char *)msg);
+		case IGMPMSG_WRVIFWHOLE:
+			return pim_mroute_msg_wrvifwhole(
+				pim->mroute_socket, ifp, (const char *)msg);
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
diff --git a/pimd/pim_msg.h b/pimd/pim_msg.h
index 456c356d9..3ad958f09 100644
--- a/pimd/pim_msg.h
+++ b/pimd/pim_msg.h
@@ -161,10 +161,18 @@ struct pim_encoded_source_ipv6 {
 typedef struct pim_encoded_ipv4_unicast pim_encoded_unicast;
 typedef struct pim_encoded_group_ipv4   pim_encoded_group;
 typedef struct pim_encoded_source_ipv4  pim_encoded_source;
+typedef struct ip 			ipv_hdr;
+#define IPV_SRC(ip_hdr)			((ip_hdr))->ip_src
+#define IPV_DST(ip_hdr)			((ip_hdr))->ip_dst
+#define IPV_LEN(ip_hdr)			((ip_hdr))->ip_len
 #else
 typedef struct pim_encoded_ipv6_unicast pim_encoded_unicast;
 typedef struct pim_encoded_group_ipv6   pim_encoded_group;
 typedef struct pim_encoded_source_ipv6  pim_encoded_source;
+typedef struct ip6_hdr 			ipv_hdr;
+#define IPV_SRC(ip_hdr)			((ip_hdr))->ip6_src
+#define IPV_DST(ip_hdr)			((ip_hdr))->ip6_dst
+#define IPV_LEN(ip_hdr)			((ip_hdr))->ip6_plen
 #endif
 /* clang-format on */
 
diff --git a/pimd/pim_oil.c b/pimd/pim_oil.c
index d5e459b44..5b5cc2c10 100644
--- a/pimd/pim_oil.c
+++ b/pimd/pim_oil.c
@@ -544,20 +544,27 @@ int pim_channel_add_oif(struct channel_oil *channel_oil, struct interface *oif,
 
 int pim_channel_oil_empty(struct channel_oil *c_oil)
 {
+#if PIM_IPV == 4
+	static struct mfcctl null_oil;
+#else
+	static struct mf6cctl null_oil;
+#endif
+
 	if (!c_oil)
 		return 1;
 
+
 	/* exclude pimreg from the OIL when checking if the inherited_oil is
 	 * non-NULL.
 	 * pimreg device (in all vrfs) uses a vifi of
 	 * 0 (PIM_OIF_PIM_REGISTER_VIF) so we simply mfcc_ttls[0] */
+	if (oil_if_has(c_oil, 0)) {
 #if PIM_IPV == 4
-	static pim_mfcctl null_oil;
-
-	return !memcmp(&c_oil->oil.mfcc_ttls[1], &null_oil.mfcc_ttls[1],
-		sizeof(null_oil.mfcc_ttls) - sizeof(null_oil.mfcc_ttls[0]));
+		null_oil.mfcc_ttls[0] = 1;
 #else
-	CPP_NOTICE("FIXME STUB");
-	return false;
+		IF_SET(0, &null_oil.mf6cc_ifset);
 #endif
+	}
+
+	return !oil_if_cmp(&c_oil->oil, &null_oil);
 }
diff --git a/pimd/pim_oil.h b/pimd/pim_oil.h
index a52e23351..68b5ef474 100644
--- a/pimd/pim_oil.h
+++ b/pimd/pim_oil.h
@@ -98,7 +98,11 @@ struct channel_oil {
 
 	struct rb_pim_oil_item oil_rb;
 
-	pim_mfcctl oil;
+#if PIM_IPV == 4
+	struct mfcctl oil;
+#else
+	struct mf6cctl oil;
+#endif
 	int installed;
 	int oil_inherited_rescan;
 	int oil_size;
@@ -135,6 +139,12 @@ static inline void oil_if_set(struct channel_oil *c_oil, vifi_t ifi, uint8_t set
 {
 	c_oil->oil.mfcc_ttls[ifi] = set;
 }
+
+static inline int oil_if_cmp(struct mfcctl *oil1, struct mfcctl *oil2)
+{
+	return memcmp(&oil1->mfcc_ttls[0], &oil2->mfcc_ttls[0],
+		      sizeof(oil1->mfcc_ttls));
+}
 #else
 static inline pim_addr *oil_origin(struct channel_oil *c_oil)
 {
@@ -163,6 +173,12 @@ static inline void oil_if_set(struct channel_oil *c_oil, mifi_t ifi, bool set)
 	else
 		IF_CLR(ifi, &c_oil->oil.mf6cc_ifset);
 }
+
+static inline int oil_if_cmp(struct mf6cctl *oil1, struct mf6cctl *oil2)
+{
+	return memcmp(&oil1->mf6cc_ifset, &oil2->mf6cc_ifset,
+		      sizeof(oil1->mf6cc_ifset));
+}
 #endif
 
 extern int pim_channel_oil_compare(const struct channel_oil *c1,
diff --git a/pimd/pim_register.h b/pimd/pim_register.h
index 0ebef40c5..79c64d995 100644
--- a/pimd/pim_register.h
+++ b/pimd/pim_register.h
@@ -35,11 +35,11 @@ int pim_register_stop_recv(struct interface *ifp, uint8_t *buf, int buf_size);
 int pim_register_recv(struct interface *ifp, pim_addr dest_addr,
 		      pim_addr src_addr, uint8_t *tlv_buf, int tlv_buf_size);
 
-void pim_register_send(const uint8_t *buf, int buf_size, struct in_addr src,
+void pim_register_send(const uint8_t *buf, int buf_size, pim_addr src,
 		       struct pim_rpf *rpg, int null_register,
 		       struct pim_upstream *up);
-void pim_register_stop_send(struct interface *ifp, pim_sgaddr *sg,
-			    struct in_addr src, struct in_addr originator);
+void pim_register_stop_send(struct interface *ifp, pim_sgaddr *sg, pim_addr src,
+			    pim_addr originator);
 void pim_register_join(struct pim_upstream *up);
 void pim_null_register_send(struct pim_upstream *up);
 void pim_reg_del_on_couldreg_fail(struct interface *ifp);
diff --git a/pimd/subdir.am b/pimd/subdir.am
index cda5acb00..9e61b03c9 100644
--- a/pimd/subdir.am
+++ b/pimd/subdir.am
@@ -77,6 +77,7 @@ pimd_pimd_SOURCES = \
 	pimd/pim_signals.c \
 	pimd/pim_zlookup.c \
 	pimd/pim_zpthread.c \
+	pimd/pim_mroute_msg.c \
 	# end
 
 nodist_pimd_pimd_SOURCES = \
@@ -90,6 +91,7 @@ pimd_pim6d_SOURCES = \
 	pimd/pim6_main.c \
 	pimd/pim6_stubs.c \
 	pimd/pim6_cmd.c \
+	pimd/pim6_mroute_msg.c \
 	# end
 
 nodist_pimd_pim6d_SOURCES = \