diff --git a/configure.ac b/configure.ac
index 6bd2d44fe8..0d6c99acfa 100755
--- a/configure.ac
+++ b/configure.ac
@@ -1886,6 +1886,7 @@ AC_CONFIG_FILES([Makefile
 	  doc/eigrpd.8
 	  doc/ripngd.8
 	  doc/pimd.8
+	  doc/mtracebis.8
 	  doc/nhrpd.8
 	  doc/vtysh.1
 	  doc/watchfrr.8
diff --git a/debianpkg/backports/ubuntu14.04/debian/frr.install b/debianpkg/backports/ubuntu14.04/debian/frr.install
index adce915e1f..8e83bdf6df 100644
--- a/debianpkg/backports/ubuntu14.04/debian/frr.install
+++ b/debianpkg/backports/ubuntu14.04/debian/frr.install
@@ -1,5 +1,6 @@
 etc/frr/
 usr/bin/vtysh
+usr/bin/mtracebis
 usr/include/frr/
 usr/lib/
 tools/frr etc/init.d/
@@ -15,6 +16,7 @@ usr/share/man/man8/ripngd.8
 usr/share/man/man8/zebra.8
 usr/share/man/man8/isisd.8
 usr/share/man/man8/watchfrr.8
+usr/share/man/man8/mtracebis.8
 usr/share/snmp/mibs/
 tools/etc/* etc/
 tools/*.service    lib/systemd/system
diff --git a/debianpkg/frr.install b/debianpkg/frr.install
index 2d86009dba..1aee4d540e 100644
--- a/debianpkg/frr.install
+++ b/debianpkg/frr.install
@@ -1,5 +1,6 @@
 etc/frr/
 usr/bin/vtysh
+usr/bin/mtracebis
 usr/include/frr/
 usr/lib/
 tools/frr usr/lib/frr
@@ -16,6 +17,7 @@ usr/share/man/man8/zebra.8
 usr/share/man/man8/isisd.8
 usr/share/man/man8/watchfrr.8
 usr/share/man/man8/frr-args.8
+usr/share/man/man8/mtracebis.8
 usr/share/snmp/mibs/
 tools/etc/* etc/
 tools/*.service    lib/systemd/system
diff --git a/doc/Makefile.am b/doc/Makefile.am
index 7aaa36556f..9c81202db9 100644
--- a/doc/Makefile.am
+++ b/doc/Makefile.am
@@ -104,6 +104,7 @@ man_MANS = frr.1 frr-args.8
 
 if PIMD
 man_MANS += pimd.8
+man_MANS += mtracebis.8
 endif
 
 if BGPD
@@ -169,6 +170,7 @@ EXTRA_DIST = BGP-TypeCode draft-zebra-00.ms draft-zebra-00.txt \
 	ripd.8.in \
 	ripngd.8.in \
 	pimd.8.in \
+	mtracebis.8.in \
 	nhrpd.8.in \
 	vtysh.1.in \
 	watchfrr.8.in \
diff --git a/doc/mtracebis.8.in b/doc/mtracebis.8.in
new file mode 100644
index 0000000000..3abc717b94
--- /dev/null
+++ b/doc/mtracebis.8.in
@@ -0,0 +1,25 @@
+.\" This file was originally generated by help2man 1.44.1.
+.TH MTRACEBIS "8" "February 2018" "mtracebis 0.1" "System Administration Utilities"
+.SH NAME
+mtracebis \- a program to initiate multicast traceroute "mtrace" queries
+.SH SYNOPSIS
+mtracebis
+<multicast source>
+.SH DESCRIPTION
+.B mtracebis
+is a program used to test multicast connectivity in a multicast and multicast
+traceroute enabled IP network.
+.PP
+Initial version of the program requires multicast source IP address and
+initiates a weak traceroute across the network. This tests whether the
+interfaces towards the source are multicast enabled. First query sent is a
+full query, capable of crossing the network all the way to the source. If this
+fails, hop-by-hop queries are initiated.
+.PP
+Hop-by-hop queries start by requesting only a response from the nearest router.
+Following that, next query is extended to the next two routers, and so on...
+until a set of routers is tested for connectivity.
+.SH SEE ALSO
+See the project homepage at <@PACKAGE_URL@>.
+.SH AUTHORS
+Copyright 2018 Mladen Sablic
diff --git a/lib/prefix.h b/lib/prefix.h
index bcc2230607..5bf7d498c1 100644
--- a/lib/prefix.h
+++ b/lib/prefix.h
@@ -239,6 +239,7 @@ static inline void ipv4_addr_copy(struct in_addr *dst,
 #define IPV4_NET127(a)  ((((u_int32_t) (a)) & 0xff000000) == 0x7f000000)
 #define IPV4_LINKLOCAL(a) ((((u_int32_t) (a)) & 0xffff0000) == 0xa9fe0000)
 #define IPV4_CLASS_DE(a)  ((((u_int32_t) (a)) & 0xe0000000) == 0xe0000000)
+#define IPV4_MC_LINKLOCAL(a)  ((((u_int32_t) (a)) & 0xffffff00) == 0xe0000000)
 
 /* Max bit/byte length of IPv6 address. */
 #define IPV6_MAX_BYTELEN    16
diff --git a/pimd/.gitignore b/pimd/.gitignore
index e23216b058..1f56cfaecd 100644
--- a/pimd/.gitignore
+++ b/pimd/.gitignore
@@ -2,6 +2,7 @@
 Makefile.in
 libpim.a
 pimd
+mtracebis
 test_igmpv3_join
 tags
 TAGS
diff --git a/pimd/COMMANDS b/pimd/COMMANDS
index c545eca56e..6f2e020bd8 100644
--- a/pimd/COMMANDS
+++ b/pimd/COMMANDS
@@ -59,6 +59,7 @@ debug commands:
        clear ip pim interfaces		Reset PIM interfaces
        clear ip pim oil			Rescan PIM OIL (output interface list)
        debug igmp			IGMP protocol activity
+       debug mtrace			Mtrace protocol activity
        debug mroute			PIM interaction with kernel MFC cache
        debug pim			PIM protocol activity
        debug pim zebra			ZEBRA protocol activity
@@ -76,4 +77,6 @@ statistics commands:
        pimd:
        show memory pim		PIM memory statistics
 
+vtysh:
+	mtrace			Multicast traceroute
 -x-
diff --git a/pimd/mtracebis.c b/pimd/mtracebis.c
new file mode 100644
index 0000000000..2032cdcfbe
--- /dev/null
+++ b/pimd/mtracebis.c
@@ -0,0 +1,371 @@
+/*
+ * Multicast Traceroute for FRRouting
+ * Copyright (C) 2018  Mladen Sablic
+ *
+ * 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
+ */
+
+#ifdef __linux__
+
+#include "pim_igmp_mtrace.h"
+
+#include "checksum.h"
+#include "mtracebis_routeget.h"
+
+#include <sys/select.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <net/if.h>
+
+#define MTRACEBIS_VERSION "0.1"
+#define MTRACE_TIMEOUT (5)
+
+#define IP_HDR_LEN (sizeof(struct ip))
+#define IP_RA_LEN (4)
+#define MTRACE_BUF_LEN (MTRACE_HDR_SIZE + (MTRACE_MAX_HOPS * MTRACE_RSP_SIZE))
+#define IP_AND_MTRACE_BUF_LEN (IP_HDR_LEN + IP_RA_LEN + MTRACE_BUF_LEN)
+
+static const char *progname;
+static void usage(void)
+{
+	fprintf(stderr, "Usage : %s <multicast source>\n", progname);
+}
+static void version(void)
+{
+	fprintf(stderr, "%s %s\n", progname, MTRACEBIS_VERSION);
+}
+
+static int send_query(int fd, struct in_addr to_addr,
+		      struct igmp_mtrace *mtrace)
+{
+	struct sockaddr_in to;
+	socklen_t tolen;
+	int sent;
+
+	memset(&to, 0, sizeof(to));
+	to.sin_family = AF_INET;
+	to.sin_addr = to_addr;
+	tolen = sizeof(to);
+
+	sent = sendto(fd, (char *)mtrace, sizeof(*mtrace), MSG_DONTWAIT,
+		      (struct sockaddr *)&to, tolen);
+
+	if (sent < 1)
+		return -1;
+	return 0;
+}
+
+static void print_query(struct igmp_mtrace *mtrace)
+{
+	char src_str[INET_ADDRSTRLEN];
+	char dst_str[INET_ADDRSTRLEN];
+	char grp_str[INET_ADDRSTRLEN];
+
+	printf("* Mtrace from %s to %s via group %s\n",
+	       inet_ntop(AF_INET, &mtrace->src_addr, src_str, sizeof(src_str)),
+	       inet_ntop(AF_INET, &mtrace->dst_addr, dst_str, sizeof(dst_str)),
+	       inet_ntop(AF_INET, &mtrace->grp_addr, grp_str, sizeof(grp_str)));
+}
+
+static int recv_response(int fd, long msec, int *hops)
+{
+	int recvd;
+	char mtrace_buf[IP_AND_MTRACE_BUF_LEN];
+	struct ip *ip;
+	struct igmp_mtrace *mtrace;
+	int mtrace_len;
+	int responses;
+	int i;
+	u_short sum;
+
+	recvd = recvfrom(fd, mtrace_buf, IP_AND_MTRACE_BUF_LEN, 0, NULL, 0);
+
+	if (recvd < 1) {
+		fprintf(stderr, "recvfrom error: %s\n", strerror(errno));
+		return -1;
+	}
+
+	if (recvd < (int)sizeof(struct ip)) {
+		fprintf(stderr, "no ip header\n");
+		return -1;
+	}
+
+	ip = (struct ip *)mtrace_buf;
+
+	if (ip->ip_v != 4) {
+		fprintf(stderr, "IP not version 4\n");
+		return -1;
+	}
+
+	sum = ip->ip_sum;
+	ip->ip_sum = 0;
+
+	if (sum != in_cksum(ip, ip->ip_hl * 4))
+		return -1;
+
+	mtrace = (struct igmp_mtrace *)(mtrace_buf + (4 * ip->ip_hl));
+
+	mtrace_len = ntohs(ip->ip_len) - ip->ip_hl * 4;
+
+	if (mtrace_len < (int)MTRACE_HDR_SIZE)
+		return -1;
+
+	sum = mtrace->checksum;
+	mtrace->checksum = 0;
+	if (sum != in_cksum(mtrace, mtrace_len)) {
+		fprintf(stderr, "mtrace checksum wrong\n");
+		return -1;
+	}
+
+	if (mtrace->type != PIM_IGMP_MTRACE_RESPONSE)
+		return -1;
+
+
+	responses = mtrace_len - sizeof(struct igmp_mtrace);
+	responses /= sizeof(struct igmp_mtrace_rsp);
+
+	printf("%ld ms received responses from %d hops.\n", msec, responses);
+
+	if (hops)
+		*hops = responses;
+
+	for (i = 0; i < responses; i++) {
+		struct igmp_mtrace_rsp *rsp = &mtrace->rsp[i];
+
+		if (rsp->fwd_code != 0)
+			printf("-%d fwd. code 0x%2x.\n", i, rsp->fwd_code);
+	}
+
+	return 0;
+}
+
+static int wait_for_response(int fd, int *hops)
+{
+	fd_set readfds;
+	struct timeval timeout;
+	int ret = -1;
+	long msec, rmsec, tmsec;
+
+	FD_ZERO(&readfds);
+	FD_SET(fd, &readfds);
+
+	memset(&timeout, 0, sizeof(timeout));
+
+	timeout.tv_sec = MTRACE_TIMEOUT;
+
+	tmsec = timeout.tv_sec * 1000 + timeout.tv_usec / 1000;
+	do {
+		ret = select(fd + 1, &readfds, NULL, NULL, &timeout);
+		if (ret <= 0)
+			return ret;
+		rmsec = timeout.tv_sec * 1000 + timeout.tv_usec / 1000;
+		msec = tmsec - rmsec;
+	} while (recv_response(fd, msec, hops) != 0);
+
+	return ret;
+}
+
+int main(int argc, char *const argv[])
+{
+	struct in_addr mc_source;
+	struct in_addr iface_addr;
+	struct in_addr gw_addr;
+	struct in_addr mtrace_addr;
+	struct igmp_mtrace mtrace;
+	int hops = 255;
+	int rhops;
+	int maxhops = 255;
+	int perhop = 3;
+	int ifindex;
+	int unicast = 1;
+	int ttl = 64;
+	int fd = -1;
+	int ret = -1;
+	int c;
+	int i, j;
+	char ifname[IF_NAMESIZE];
+	char ip_str[INET_ADDRSTRLEN];
+
+	mtrace_addr.s_addr = inet_addr("224.0.1.32");
+
+	uid_t uid = getuid();
+
+	if (uid != 0) {
+		printf("must run as root\n");
+		exit(EXIT_FAILURE);
+	}
+
+	if (argc <= 0)
+		progname = "mtracebis";
+	else
+		progname = argv[0];
+
+	if (argc != 2) {
+		usage();
+		exit(EXIT_FAILURE);
+	}
+
+	while (1) {
+		static struct option long_options[] = {
+			{"help", no_argument, 0, 'h'},
+			{"version", no_argument, 0, 'v'},
+			{0, 0, 0, 0} };
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "vh", long_options, &option_index);
+
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'h':
+			usage();
+			exit(0);
+		case 'v':
+			version();
+			exit(0);
+		default:
+			usage();
+			exit(EXIT_FAILURE);
+		}
+	}
+	if (inet_pton(AF_INET, argv[1], &mc_source) != 1) {
+		usage();
+		fprintf(stderr, "%s: %s not a valid IPv4 address\n", argv[0],
+			argv[1]);
+		exit(EXIT_FAILURE);
+	}
+
+	ifindex = routeget(mc_source, &iface_addr, &gw_addr);
+	if (ifindex < 0) {
+		fprintf(stderr, "%s: failed to get route to source %s\n",
+			argv[0], argv[1]);
+		exit(EXIT_FAILURE);
+	}
+
+	if (if_indextoname(ifindex, ifname) == NULL) {
+		fprintf(stderr, "%s: if_indextoname error: %s\n", argv[0],
+			strerror(errno));
+		exit(EXIT_FAILURE);
+	}
+
+	/* zero mtrace struct */
+	memset((char *)&mtrace, 0, sizeof(mtrace));
+
+	/* set up query */
+	mtrace.type = PIM_IGMP_MTRACE_QUERY_REQUEST;
+	mtrace.hops = hops;
+	mtrace.checksum = 0;
+	mtrace.grp_addr.s_addr = 0;
+	mtrace.src_addr = mc_source;
+	mtrace.dst_addr = iface_addr;
+	mtrace.rsp_addr = unicast ? iface_addr : mtrace_addr;
+	mtrace.rsp_ttl = ttl;
+	mtrace.qry_id = 0xffffff & time(NULL);
+
+	mtrace.checksum = in_cksum(&mtrace, sizeof(mtrace));
+
+	fd = socket(AF_INET, SOCK_RAW, IPPROTO_IGMP);
+
+	if (fd < 1) {
+		fprintf(stderr, "%s: socket error: %s\n", argv[0],
+			strerror(errno));
+		exit(EXIT_FAILURE);
+	}
+
+	ret = setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, ifname,
+			 strlen(ifname));
+
+	if (ret < 0) {
+		fprintf(stderr, "%s: setsockopt error: %s\n", argv[0],
+			strerror(errno));
+		ret = EXIT_FAILURE;
+		goto close_fd;
+	}
+
+	print_query(&mtrace);
+	if (send_query(fd, gw_addr, &mtrace) < 0) {
+		fprintf(stderr, "%s: sendto error: %s\n", argv[0],
+			strerror(errno));
+		ret = EXIT_FAILURE;
+		goto close_fd;
+	}
+	printf("Querying full reverse path...\n");
+	ret = wait_for_response(fd, NULL);
+	if (ret > 0) {
+		ret = 0;
+		goto close_fd;
+	}
+	if (ret < 0) {
+		fprintf(stderr, "%s: select error: %s\n", argv[0],
+			strerror(errno));
+		ret = EXIT_FAILURE;
+		goto close_fd;
+	}
+	printf(" * ");
+	printf("switching to hop-by-hop:\n");
+	printf("%3d  ? (%s)\n", 0,
+	       inet_ntop(AF_INET, &mtrace.dst_addr, ip_str, sizeof(ip_str)));
+	for (i = 1; i < maxhops; i++) {
+		printf("%3d ", -i);
+		mtrace.hops = i;
+		for (j = 0; j < perhop; j++) {
+			mtrace.qry_id++;
+			mtrace.checksum = 0;
+			mtrace.checksum = in_cksum(&mtrace, sizeof(mtrace));
+			if (send_query(fd, gw_addr, &mtrace) < 0) {
+				fprintf(stderr, "%s: sendto error: %s\n",
+					argv[0], strerror(errno));
+				ret = EXIT_FAILURE;
+				goto close_fd;
+			}
+			ret = wait_for_response(fd, &rhops);
+			if (ret > 0) {
+				if (i > rhops) {
+					ret = 0;
+					goto close_fd;
+				}
+				break;
+			}
+			printf(" *");
+		}
+		if (ret <= 0)
+			printf("\n");
+	}
+	ret = 0;
+close_fd:
+	close(fd);
+	exit(ret);
+}
+
+#else /* __linux__ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+int main(int argc, char *argv[])
+{
+	printf("%s implemented only for GNU/Linux\n", argv[0]);
+	exit(0);
+}
+
+#endif /* __linux__ */
diff --git a/pimd/mtracebis_netlink.c b/pimd/mtracebis_netlink.c
new file mode 100644
index 0000000000..42b80b218b
--- /dev/null
+++ b/pimd/mtracebis_netlink.c
@@ -0,0 +1,700 @@
+/*
+ * libnetlink.c	RTnetlink service routines.
+ *
+ *		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.
+ *
+ * Authors:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
+ *
+ */
+
+#ifdef __linux__
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <syslog.h>
+#include <fcntl.h>
+#include <net/if_arp.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <string.h>
+#include <errno.h>
+#include <time.h>
+#include <sys/uio.h>
+#include <assert.h>
+
+#include "mtracebis_netlink.h"
+
+int rcvbuf = 1024 * 1024;
+
+void rtnl_close(struct rtnl_handle *rth)
+{
+	if (rth->fd >= 0) {
+		close(rth->fd);
+		rth->fd = -1;
+	}
+}
+
+int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions,
+		      int protocol)
+{
+	socklen_t addr_len;
+	int sndbuf = 32768;
+
+	memset(rth, 0, sizeof(*rth));
+
+	rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
+	if (rth->fd < 0) {
+		perror("Cannot open netlink socket");
+		return -1;
+	}
+
+	if (setsockopt(rth->fd,SOL_SOCKET,SO_SNDBUF,&sndbuf,sizeof(sndbuf)) < 0) {
+		perror("SO_SNDBUF");
+		return -1;
+	}
+
+	if (setsockopt(rth->fd,SOL_SOCKET,SO_RCVBUF,&rcvbuf,sizeof(rcvbuf)) < 0) {
+		perror("SO_RCVBUF");
+		return -1;
+	}
+
+	memset(&rth->local, 0, sizeof(rth->local));
+	rth->local.nl_family = AF_NETLINK;
+	rth->local.nl_groups = subscriptions;
+
+	if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0) {
+		perror("Cannot bind netlink socket");
+		return -1;
+	}
+	addr_len = sizeof(rth->local);
+	if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0) {
+		perror("Cannot getsockname");
+		return -1;
+	}
+	if (addr_len != sizeof(rth->local)) {
+		fprintf(stderr, "Wrong address length %d\n", addr_len);
+		return -1;
+	}
+	if (rth->local.nl_family != AF_NETLINK) {
+		fprintf(stderr, "Wrong address family %d\n", rth->local.nl_family);
+		return -1;
+	}
+	rth->seq = time(NULL);
+	return 0;
+}
+
+int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
+{
+	return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);
+}
+
+int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
+{
+	struct {
+		struct nlmsghdr nlh;
+		struct rtgenmsg g;
+	} req;
+
+	memset(&req, 0, sizeof(req));
+	req.nlh.nlmsg_len = sizeof(req);
+	req.nlh.nlmsg_type = type;
+	req.nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
+	req.nlh.nlmsg_pid = 0;
+	req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
+	req.g.rtgen_family = family;
+
+	return send(rth->fd, (void*)&req, sizeof(req), 0);
+}
+
+int rtnl_send(struct rtnl_handle *rth, const char *buf, int len)
+{
+	return send(rth->fd, buf, len, 0);
+}
+
+int rtnl_send_check(struct rtnl_handle *rth, const char *buf, int len)
+{
+	struct nlmsghdr *h;
+	int status;
+	char resp[1024];
+
+	status = send(rth->fd, buf, len, 0);
+	if (status < 0)
+		return status;
+
+	/* Check for immediate errors */
+	status = recv(rth->fd, resp, sizeof(resp), MSG_DONTWAIT|MSG_PEEK);
+	if (status < 0) {
+		if (errno == EAGAIN)
+			return 0;
+		return -1;
+	}
+
+	for (h = (struct nlmsghdr *)resp; NLMSG_OK(h, (uint32_t)status);
+	     h = NLMSG_NEXT(h, status)) {
+		if (h->nlmsg_type == NLMSG_ERROR) {
+			struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
+			if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr)))
+				fprintf(stderr, "ERROR truncated\n");
+			else
+				errno = -err->error;
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
+{
+	struct nlmsghdr nlh;
+	struct sockaddr_nl nladdr;
+	struct iovec iov[2] = {
+		{ .iov_base = &nlh, .iov_len = sizeof(nlh) },
+		{ .iov_base = req, .iov_len = len }
+	};
+	struct msghdr msg = {
+		.msg_name = &nladdr,
+		.msg_namelen = 	sizeof(nladdr),
+		.msg_iov = iov,
+		.msg_iovlen = 2,
+	};
+
+	memset(&nladdr, 0, sizeof(nladdr));
+	nladdr.nl_family = AF_NETLINK;
+
+	nlh.nlmsg_len = NLMSG_LENGTH(len);
+	nlh.nlmsg_type = type;
+	nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
+	nlh.nlmsg_pid = 0;
+	nlh.nlmsg_seq = rth->dump = ++rth->seq;
+
+	return sendmsg(rth->fd, &msg, 0);
+}
+
+int rtnl_dump_filter_l(struct rtnl_handle *rth,
+		       const struct rtnl_dump_filter_arg *arg)
+{
+	struct sockaddr_nl nladdr;
+	struct iovec iov;
+	struct msghdr msg = {
+		.msg_name = &nladdr,
+		.msg_namelen = sizeof(nladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	char buf[16384];
+
+	iov.iov_base = buf;
+	while (1) {
+		int status;
+		const struct rtnl_dump_filter_arg *a;
+		int found_done = 0;
+		int msglen = 0;
+
+		iov.iov_len = sizeof(buf);
+		status = recvmsg(rth->fd, &msg, 0);
+
+		if (status < 0) {
+			if (errno == EINTR || errno == EAGAIN)
+				continue;
+			fprintf(stderr, "netlink receive error %s (%d)\n",
+				strerror(errno), errno);
+			return -1;
+		}
+
+		if (status == 0) {
+			fprintf(stderr, "EOF on netlink\n");
+			return -1;
+		}
+
+		for (a = arg; a->filter; a++) {
+			struct nlmsghdr *h = (struct nlmsghdr*)buf;
+			msglen = status;
+
+			while (NLMSG_OK(h, (uint32_t)msglen)) {
+				int err;
+
+				if (nladdr.nl_pid != 0 ||
+				    h->nlmsg_pid != rth->local.nl_pid ||
+				    h->nlmsg_seq != rth->dump) {
+					if (a->junk) {
+						err = a->junk(&nladdr, h,
+							      a->arg2);
+						if (err < 0)
+							return err;
+					}
+					goto skip_it;
+				}
+
+				if (h->nlmsg_type == NLMSG_DONE) {
+					found_done = 1;
+					break; /* process next filter */
+				}
+				if (h->nlmsg_type == NLMSG_ERROR) {
+					struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
+					if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
+						fprintf(stderr,
+							"ERROR truncated\n");
+					} else {
+						errno = -err->error;
+						perror("RTNETLINK answers");
+					}
+					return -1;
+				}
+				err = a->filter(&nladdr, h, a->arg1);
+				if (err < 0)
+					return err;
+
+skip_it:
+				h = NLMSG_NEXT(h, msglen);
+			}
+		}
+
+		if (found_done)
+			return 0;
+
+		if (msg.msg_flags & MSG_TRUNC) {
+			fprintf(stderr, "Message truncated\n");
+			continue;
+		}
+		if (msglen) {
+			fprintf(stderr, "!!!Remnant of size %d\n", msglen);
+			exit(1);
+		}
+	}
+}
+
+int rtnl_dump_filter(struct rtnl_handle *rth,
+		     rtnl_filter_t filter,
+		     void *arg1,
+		     rtnl_filter_t junk,
+		     void *arg2)
+{
+	const struct rtnl_dump_filter_arg a[2] = {
+		{ .filter = filter, .arg1 = arg1, .junk = junk, .arg2 = arg2 },
+		{ .filter = NULL,   .arg1 = NULL, .junk = NULL, .arg2 = NULL }
+	};
+
+	return rtnl_dump_filter_l(rth, a);
+}
+
+int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
+	      unsigned groups, struct nlmsghdr *answer,
+	      rtnl_filter_t junk,
+	      void *jarg)
+{
+	int status;
+	unsigned seq;
+	struct nlmsghdr *h;
+	struct sockaddr_nl nladdr;
+	struct iovec iov = {
+		.iov_base = (void*) n,
+		.iov_len = n->nlmsg_len
+	};
+	struct msghdr msg = {
+		.msg_name = &nladdr,
+		.msg_namelen = sizeof(nladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	char   buf[16384];
+
+	memset(&nladdr, 0, sizeof(nladdr));
+	nladdr.nl_family = AF_NETLINK;
+	nladdr.nl_pid = peer;
+	nladdr.nl_groups = groups;
+
+	n->nlmsg_seq = seq = ++rtnl->seq;
+
+	if (answer == NULL)
+		n->nlmsg_flags |= NLM_F_ACK;
+
+	status = sendmsg(rtnl->fd, &msg, 0);
+
+	if (status < 0) {
+		perror("Cannot talk to rtnetlink");
+		return -1;
+	}
+
+	memset(buf,0,sizeof(buf));
+
+	iov.iov_base = buf;
+
+	while (1) {
+		iov.iov_len = sizeof(buf);
+		status = recvmsg(rtnl->fd, &msg, 0);
+
+		if (status < 0) {
+			if (errno == EINTR || errno == EAGAIN)
+				continue;
+			fprintf(stderr, "netlink receive error %s (%d)\n",
+				strerror(errno), errno);
+			return -1;
+		}
+		if (status == 0) {
+			fprintf(stderr, "EOF on netlink\n");
+			return -1;
+		}
+		if (msg.msg_namelen != sizeof(nladdr)) {
+			fprintf(stderr, "sender address length == %d\n", msg.msg_namelen);
+			exit(1);
+		}
+		for (h = (struct nlmsghdr*)buf; status >= (int)sizeof(*h); ) {
+			int err;
+			int len = h->nlmsg_len;
+			int l = len - sizeof(*h);
+
+			if (l<0 || len>status) {
+				if (msg.msg_flags & MSG_TRUNC) {
+					fprintf(stderr, "Truncated message\n");
+					return -1;
+				}
+				fprintf(stderr, "!!!malformed message: len=%d\n", len);
+				exit(1);
+			}
+
+			if ((int)nladdr.nl_pid != peer ||
+			    h->nlmsg_pid != rtnl->local.nl_pid ||
+			    h->nlmsg_seq != seq) {
+				if (junk) {
+					err = junk(&nladdr, h, jarg);
+					if (err < 0)
+						return err;
+				}
+				/* Don't forget to skip that message. */
+				status -= NLMSG_ALIGN(len);
+				h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
+				continue;
+			}
+
+			if (h->nlmsg_type == NLMSG_ERROR) {
+				struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
+				if (l < (int)sizeof(struct nlmsgerr)) {
+					fprintf(stderr, "ERROR truncated\n");
+				} else {
+					errno = -err->error;
+					if (errno == 0) {
+						if (answer)
+							memcpy(answer, h, h->nlmsg_len);
+						return 0;
+					}
+					perror("RTNETLINK answers");
+				}
+				return -1;
+			}
+			if (answer) {
+				memcpy(answer, h, h->nlmsg_len);
+				return 0;
+			}
+
+			fprintf(stderr, "Unexpected reply!!!\n");
+
+			status -= NLMSG_ALIGN(len);
+			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
+		}
+		if (msg.msg_flags & MSG_TRUNC) {
+			fprintf(stderr, "Message truncated\n");
+			continue;
+		}
+		if (status) {
+			fprintf(stderr, "!!!Remnant of size %d\n", status);
+			exit(1);
+		}
+	}
+}
+
+int rtnl_listen(struct rtnl_handle *rtnl,
+		rtnl_filter_t handler,
+		void *jarg)
+{
+	int status;
+	struct nlmsghdr *h;
+	struct sockaddr_nl nladdr;
+	struct iovec iov;
+	struct msghdr msg = {
+		.msg_name = &nladdr,
+		.msg_namelen = sizeof(nladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	char   buf[8192];
+
+	memset(&nladdr, 0, sizeof(nladdr));
+	nladdr.nl_family = AF_NETLINK;
+	nladdr.nl_pid = 0;
+	nladdr.nl_groups = 0;
+
+	iov.iov_base = buf;
+	while (1) {
+		iov.iov_len = sizeof(buf);
+		status = recvmsg(rtnl->fd, &msg, 0);
+
+		if (status < 0) {
+			if (errno == EINTR || errno == EAGAIN)
+				continue;
+			fprintf(stderr, "netlink receive error %s (%d)\n",
+				strerror(errno), errno);
+			if (errno == ENOBUFS)
+				continue;
+			return -1;
+		}
+		if (status == 0) {
+			fprintf(stderr, "EOF on netlink\n");
+			return -1;
+		}
+		if (msg.msg_namelen != sizeof(nladdr)) {
+			fprintf(stderr, "Sender address length == %d\n", msg.msg_namelen);
+			exit(1);
+		}
+		for (h =(struct nlmsghdr*)buf; status >= (int)sizeof(*h); ) {
+			int err;
+			int len = h->nlmsg_len;
+			int l = len - sizeof(*h);
+
+			if (l<0 || len>status) {
+				if (msg.msg_flags & MSG_TRUNC) {
+					fprintf(stderr, "Truncated message\n");
+					return -1;
+				}
+				fprintf(stderr, "!!!malformed message: len=%d\n", len);
+				exit(1);
+			}
+
+			err = handler(&nladdr, h, jarg);
+			if (err < 0)
+				return err;
+
+			status -= NLMSG_ALIGN(len);
+			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
+		}
+		if (msg.msg_flags & MSG_TRUNC) {
+			fprintf(stderr, "Message truncated\n");
+			continue;
+		}
+		if (status) {
+			fprintf(stderr, "!!!Remnant of size %d\n", status);
+			exit(1);
+		}
+	}
+}
+
+int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler,
+		   void *jarg)
+{
+	int status;
+	struct sockaddr_nl nladdr;
+	char   buf[8192];
+	struct nlmsghdr *h = (void*)buf;
+
+	memset(&nladdr, 0, sizeof(nladdr));
+	nladdr.nl_family = AF_NETLINK;
+	nladdr.nl_pid = 0;
+	nladdr.nl_groups = 0;
+
+	while (1) {
+		int err, len;
+		int l;
+
+		status = fread(&buf, 1, sizeof(*h), rtnl);
+
+		if (status < 0) {
+			if (errno == EINTR)
+				continue;
+			perror("rtnl_from_file: fread");
+			return -1;
+		}
+		if (status == 0)
+			return 0;
+
+		len = h->nlmsg_len;
+		l = len - sizeof(*h);
+
+		if (l<0 || len>(int)sizeof(buf)) {
+			fprintf(stderr, "!!!malformed message: len=%d @%lu\n",
+				len, ftell(rtnl));
+			return -1;
+		}
+
+		status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl);
+
+		if (status < 0) {
+			perror("rtnl_from_file: fread");
+			return -1;
+		}
+		if (status < l) {
+			fprintf(stderr, "rtnl-from_file: truncated message\n");
+			return -1;
+		}
+
+		err = handler(&nladdr, h, jarg);
+		if (err < 0)
+			return err;
+	}
+}
+
+int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
+{
+	int len = RTA_LENGTH(4);
+	struct rtattr *rta;
+	if ((int)(NLMSG_ALIGN(n->nlmsg_len) + len) > maxlen) {
+		fprintf(stderr,"addattr32: Error! max allowed bound %d exceeded\n",maxlen);
+		return -1;
+	}
+	rta = NLMSG_TAIL(n);
+	rta->rta_type = type;
+	rta->rta_len = len;
+	memcpy(RTA_DATA(rta), &data, 4);
+	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
+	return 0;
+}
+
+int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
+	      int alen)
+{
+	int len = RTA_LENGTH(alen);
+	struct rtattr *rta;
+
+	if ((int)(NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len)) > maxlen) {
+		fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen);
+		return -1;
+	}
+	rta = NLMSG_TAIL(n);
+	rta->rta_type = type;
+	rta->rta_len = len;
+
+	if (data)
+		memcpy(RTA_DATA(rta), data, alen);
+	else
+		assert(alen == 0);
+
+	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
+	return 0;
+}
+
+int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)
+{
+	if ((int)(NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len)) > maxlen) {
+		fprintf(stderr, "addraw_l ERROR: message exceeded bound of %d\n",maxlen);
+		return -1;
+	}
+
+	memcpy(NLMSG_TAIL(n), data, len);
+	memset((uint8_t *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
+	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
+	return 0;
+}
+
+struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type)
+{
+	struct rtattr *nest = NLMSG_TAIL(n);
+
+	addattr_l(n, maxlen, type, NULL, 0);
+	return nest;
+}
+
+int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest)
+{
+	nest->rta_len = (uint8_t *)NLMSG_TAIL(n) - (uint8_t *)nest;
+	return n->nlmsg_len;
+}
+
+struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type,
+				   const void *data, int len)
+{
+	struct rtattr *start = NLMSG_TAIL(n);
+
+	addattr_l(n, maxlen, type, data, len);
+	addattr_nest(n, maxlen, type);
+	return start;
+}
+
+int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start)
+{
+	struct rtattr *nest = start + NLMSG_ALIGN(start->rta_len);
+
+	start->rta_len = (uint8_t *)NLMSG_TAIL(n) - (uint8_t *)start;
+	addattr_nest_end(n, nest);
+	return n->nlmsg_len;
+}
+
+int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
+{
+	int len = RTA_LENGTH(4);
+	struct rtattr *subrta;
+
+	if ((int)(RTA_ALIGN(rta->rta_len) + len) > maxlen) {
+		fprintf(stderr,"rta_addattr32: Error! max allowed bound %d exceeded\n",maxlen);
+		return -1;
+	}
+	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
+	subrta->rta_type = type;
+	subrta->rta_len = len;
+	memcpy(RTA_DATA(subrta), &data, 4);
+	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
+	return 0;
+}
+
+int rta_addattr_l(struct rtattr *rta, int maxlen, int type,
+		  const void *data, int alen)
+{
+	struct rtattr *subrta;
+	int len = RTA_LENGTH(alen);
+
+	if ((int)(RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len)) > maxlen) {
+		fprintf(stderr,"rta_addattr_l: Error! max allowed bound %d exceeded\n",maxlen);
+		return -1;
+	}
+	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
+	subrta->rta_type = type;
+	subrta->rta_len = len;
+	memcpy(RTA_DATA(subrta), data, alen);
+	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
+	return 0;
+}
+
+int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
+{
+	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
+	while (RTA_OK(rta, len)) {
+		if ((rta->rta_type <= max) && (!tb[rta->rta_type]))
+			tb[rta->rta_type] = rta;
+		rta = RTA_NEXT(rta,len);
+	}
+	if (len)
+		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
+	return 0;
+}
+
+int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len)
+{
+	int i = 0;
+
+	memset(tb, 0, sizeof(struct rtattr *) * max);
+	while (RTA_OK(rta, len)) {
+		if (rta->rta_type <= max && i < max)
+			tb[i++] = rta;
+		rta = RTA_NEXT(rta,len);
+	}
+	if (len)
+		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
+	return i;
+}
+
+int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta,
+			         int len)
+{
+	if ((int)RTA_PAYLOAD(rta) < len)
+		return -1;
+	if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) {
+		rta = (struct rtattr *)(uint8_t *)RTA_DATA(rta)+RTA_ALIGN(len);
+		return parse_rtattr_nested(tb, max, rta);
+	}
+	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
+	return 0;
+}
+
+#endif /* __linux__ */
diff --git a/pimd/mtracebis_netlink.h b/pimd/mtracebis_netlink.h
new file mode 100644
index 0000000000..7a60ead975
--- /dev/null
+++ b/pimd/mtracebis_netlink.h
@@ -0,0 +1,130 @@
+/*
+ * libnetlink.c	RTnetlink service routines.
+ *
+ *		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.
+ *
+ * Authors:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
+ *
+ */
+
+#ifdef __linux__
+
+#ifndef __LIBNETLINK_H__
+#define __LIBNETLINK_H__ 1
+
+#include <asm/types.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <linux/if_link.h>
+#include <linux/if_addr.h>
+#include <linux/neighbour.h>
+
+struct rtnl_handle
+{
+	int			fd;
+	struct sockaddr_nl	local;
+	struct sockaddr_nl	peer;
+	__u32			seq;
+	__u32			dump;
+};
+
+extern int rcvbuf;
+
+extern int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions);
+extern int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions, int protocol);
+extern void rtnl_close(struct rtnl_handle *rth);
+extern int rtnl_wilddump_request(struct rtnl_handle *rth, int fam, int type);
+extern int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len);
+
+typedef int (*rtnl_filter_t)(const struct sockaddr_nl *,
+			     struct nlmsghdr *n, void *);
+
+struct rtnl_dump_filter_arg
+{
+	rtnl_filter_t filter;
+	void *arg1;
+	rtnl_filter_t junk;
+	void *arg2;
+};
+
+extern int rtnl_dump_filter_l(struct rtnl_handle *rth,
+			      const struct rtnl_dump_filter_arg *arg);
+extern int rtnl_dump_filter(struct rtnl_handle *rth, rtnl_filter_t filter,
+			    void *arg1,
+			    rtnl_filter_t junk,
+			    void *arg2);
+
+extern int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
+		     unsigned groups, struct nlmsghdr *answer,
+		     rtnl_filter_t junk,
+		     void *jarg);
+extern int rtnl_send(struct rtnl_handle *rth, const char *buf, int);
+extern int rtnl_send_check(struct rtnl_handle *rth, const char *buf, int);
+
+extern int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data);
+extern int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, int alen);
+extern int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len);
+extern struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type);
+extern int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest);
+extern struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type, const void *data, int len);
+extern int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *nest);
+extern int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data);
+extern int rta_addattr_l(struct rtattr *rta, int maxlen, int type, const void *data, int alen);
+
+extern int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len);
+extern int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len);
+extern int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta, int len);
+
+#define parse_rtattr_nested(tb, max, rta) \
+	(parse_rtattr((tb), (max), RTA_DATA(rta), RTA_PAYLOAD(rta)))
+
+#define parse_rtattr_nested_compat(tb, max, rta, data, len) \
+({	data = RTA_PAYLOAD(rta) >= len ? RTA_DATA(rta) : NULL; \
+	__parse_rtattr_nested_compat(tb, max, rta, len); })
+
+extern int rtnl_listen(struct rtnl_handle *, rtnl_filter_t handler,
+		       void *jarg);
+extern int rtnl_from_file(FILE *, rtnl_filter_t handler,
+		       void *jarg);
+
+#define NLMSG_TAIL(nmsg) \
+	((struct rtattr *) (((uint8_t *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))
+
+#ifndef IFA_RTA
+#define IFA_RTA(r) \
+	((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ifaddrmsg))))
+#endif
+#ifndef IFA_PAYLOAD
+#define IFA_PAYLOAD(n)	NLMSG_PAYLOAD(n,sizeof(struct ifaddrmsg))
+#endif
+
+#ifndef IFLA_RTA
+#define IFLA_RTA(r) \
+	((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ifinfomsg))))
+#endif
+#ifndef IFLA_PAYLOAD
+#define IFLA_PAYLOAD(n)	NLMSG_PAYLOAD(n,sizeof(struct ifinfomsg))
+#endif
+
+#ifndef NDA_RTA
+#define NDA_RTA(r) \
+	((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ndmsg))))
+#endif
+#ifndef NDA_PAYLOAD
+#define NDA_PAYLOAD(n)	NLMSG_PAYLOAD(n,sizeof(struct ndmsg))
+#endif
+
+#ifndef NDTA_RTA
+#define NDTA_RTA(r) \
+	((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ndtmsg))))
+#endif
+#ifndef NDTA_PAYLOAD
+#define NDTA_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct ndtmsg))
+#endif
+
+#endif /* __LIBNETLINK_H__ */
+
+#endif /* __linux__ */
diff --git a/pimd/mtracebis_routeget.c b/pimd/mtracebis_routeget.c
new file mode 100644
index 0000000000..d75aaa3708
--- /dev/null
+++ b/pimd/mtracebis_routeget.c
@@ -0,0 +1,98 @@
+/*
+ * Multicast Traceroute for FRRouting
+ * Copyright (C) 2018  Mladen Sablic
+ *
+ * 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
+ */
+
+#ifdef __linux__
+
+#include <asm/types.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <sys/types.h>
+#include <stdio.h>
+#include <arpa/inet.h>
+#include <string.h>
+
+#include "mtracebis_netlink.h"
+#include "mtracebis_routeget.h"
+
+static int find_dst(struct nlmsghdr *n, struct in_addr *src, struct in_addr *gw)
+{
+	struct rtmsg *r = NLMSG_DATA(n);
+	int len = n->nlmsg_len;
+	struct rtattr *tb[RTA_MAX + 1];
+
+	len -= NLMSG_LENGTH(sizeof(*r));
+	if (len < 0) {
+		fprintf(stderr, "BUG: wrong nlmsg len %d\n", len);
+		return -1;
+	}
+
+	parse_rtattr(tb, RTA_MAX, RTM_RTA(r), len);
+	if (tb[RTA_PREFSRC])
+		src->s_addr = *(uint32_t *)RTA_DATA(tb[RTA_PREFSRC]);
+	if (tb[RTA_GATEWAY])
+		gw->s_addr = *(uint32_t *)RTA_DATA(tb[RTA_GATEWAY]);
+	if (tb[RTA_OIF])
+		return *(int *)RTA_DATA(tb[RTA_OIF]);
+	return 0;
+}
+
+int routeget(struct in_addr dst, struct in_addr *src, struct in_addr *gw)
+{
+	struct {
+		struct nlmsghdr n;
+		struct rtmsg r;
+		char buf[1024];
+	} req;
+	int ret;
+	struct rtnl_handle rth = {.fd = -1};
+
+	memset(&req, 0, sizeof(req));
+
+	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtmsg));
+	req.n.nlmsg_flags = NLM_F_REQUEST;
+	req.n.nlmsg_type = RTM_GETROUTE;
+	req.r.rtm_family = AF_INET;
+	req.r.rtm_table = 0;
+	req.r.rtm_protocol = 0;
+	req.r.rtm_scope = 0;
+	req.r.rtm_type = 0;
+	req.r.rtm_src_len = 0;
+	req.r.rtm_dst_len = 0;
+	req.r.rtm_tos = 0;
+
+	addattr_l(&req.n, sizeof(req), RTA_DST, &dst.s_addr, 4);
+	req.r.rtm_dst_len = 32;
+
+	ret = rtnl_open(&rth, 0);
+
+	if (ret < 0)
+		return ret;
+
+	if (rtnl_talk(&rth, &req.n, 0, 0, &req.n, NULL, NULL) < 0) {
+		ret = -1;
+		goto close_rth;
+	}
+
+	ret = find_dst(&req.n, src, gw);
+close_rth:
+	rtnl_close(&rth);
+	return ret;
+}
+
+#endif /* __linux__ */
diff --git a/pimd/mtracebis_routeget.h b/pimd/mtracebis_routeget.h
new file mode 100644
index 0000000000..18ecf6ebfa
--- /dev/null
+++ b/pimd/mtracebis_routeget.h
@@ -0,0 +1,31 @@
+/*
+ * Multicast Traceroute for FRRouting
+ * Copyright (C) 2018  Mladen Sablic
+ *
+ * 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
+ */
+
+#ifdef __linux__
+
+#ifndef ROUTEGET_H
+#define ROUTEGET_H
+
+#include <netinet/in.h>
+
+int routeget(struct in_addr dst, struct in_addr *src, struct in_addr *gw);
+
+#endif /* ROUTEGET */
+
+#endif /* __linux__ */
diff --git a/pimd/pim_cmd.c b/pimd/pim_cmd.c
index 11aeeddf93..fc07b706a9 100644
--- a/pimd/pim_cmd.c
+++ b/pimd/pim_cmd.c
@@ -7311,6 +7311,27 @@ DEFUN (no_debug_msdp_packets,
 ALIAS(no_debug_msdp_packets, undebug_msdp_packets_cmd, "undebug msdp packets",
       UNDEBUG_STR DEBUG_MSDP_STR DEBUG_MSDP_PACKETS_STR)
 
+DEFUN (debug_mtrace,
+       debug_mtrace_cmd,
+       "debug mtrace",
+       DEBUG_STR
+       DEBUG_MTRACE_STR)
+{
+	PIM_DO_DEBUG_MTRACE;
+	return CMD_SUCCESS;
+}
+
+DEFUN (no_debug_mtrace,
+       no_debug_mtrace_cmd,
+       "no debug mtrace",
+       NO_STR
+       DEBUG_STR
+       DEBUG_MTRACE_STR)
+{
+	PIM_DONT_DEBUG_MTRACE;
+	return CMD_SUCCESS;
+}
+
 DEFUN_NOSH (show_debugging_pim,
 	    show_debugging_pim_cmd,
 	    "show debugging [pim]",
@@ -8721,6 +8742,8 @@ void pim_cmd_init(void)
 	install_element(ENABLE_NODE, &debug_msdp_packets_cmd);
 	install_element(ENABLE_NODE, &no_debug_msdp_packets_cmd);
 	install_element(ENABLE_NODE, &undebug_msdp_packets_cmd);
+	install_element(ENABLE_NODE, &debug_mtrace_cmd);
+	install_element(ENABLE_NODE, &no_debug_mtrace_cmd);
 
 	install_element(CONFIG_NODE, &debug_igmp_cmd);
 	install_element(CONFIG_NODE, &no_debug_igmp_cmd);
@@ -8763,6 +8786,8 @@ void pim_cmd_init(void)
 	install_element(CONFIG_NODE, &debug_msdp_packets_cmd);
 	install_element(CONFIG_NODE, &no_debug_msdp_packets_cmd);
 	install_element(CONFIG_NODE, &undebug_msdp_packets_cmd);
+	install_element(CONFIG_NODE, &debug_mtrace_cmd);
+	install_element(CONFIG_NODE, &no_debug_mtrace_cmd);
 
 	install_element(CONFIG_NODE, &ip_msdp_mesh_group_member_cmd);
 	install_element(VRF_NODE, &ip_msdp_mesh_group_member_cmd);
diff --git a/pimd/pim_cmd.h b/pimd/pim_cmd.h
index 8867514876..b58da30bdd 100644
--- a/pimd/pim_cmd.h
+++ b/pimd/pim_cmd.h
@@ -63,6 +63,7 @@
 #define DEBUG_MSDP_EVENTS_STR                       "MSDP protocol events\n"
 #define DEBUG_MSDP_INTERNAL_STR                     "MSDP protocol internal\n"
 #define DEBUG_MSDP_PACKETS_STR                      "MSDP protocol packets\n"
+#define DEBUG_MTRACE_STR                            "Mtrace protocol activity\n"
 
 void pim_cmd_init(void);
 
diff --git a/pimd/pim_igmp.c b/pimd/pim_igmp.c
index 7524119e52..0522420364 100644
--- a/pimd/pim_igmp.c
+++ b/pimd/pim_igmp.c
@@ -29,6 +29,7 @@
 #include "pim_igmp.h"
 #include "pim_igmpv2.h"
 #include "pim_igmpv3.h"
+#include "pim_igmp_mtrace.h"
 #include "pim_iface.h"
 #include "pim_sock.h"
 #include "pim_mroute.h"
@@ -504,6 +505,16 @@ int pim_igmp_packet(struct igmp_sock *igmp, char *buf, size_t len)
 	case PIM_IGMP_V2_LEAVE_GROUP:
 		return igmp_v2_recv_leave(igmp, ip_hdr->ip_src, from_str,
 					  igmp_msg, igmp_msg_len);
+
+	case PIM_IGMP_MTRACE_RESPONSE:
+		return igmp_mtrace_recv_response(igmp, ip_hdr, ip_hdr->ip_src,
+						 from_str, igmp_msg,
+						 igmp_msg_len);
+		break;
+	case PIM_IGMP_MTRACE_QUERY_REQUEST:
+		return igmp_mtrace_recv_qry_req(igmp, ip_hdr, ip_hdr->ip_src,
+						from_str, igmp_msg,
+						igmp_msg_len);
 	}
 
 	zlog_warn("Ignoring unsupported IGMP message type: %d", msg_type);
diff --git a/pimd/pim_igmp.h b/pimd/pim_igmp.h
index 275f25f63f..962c50e76a 100644
--- a/pimd/pim_igmp.h
+++ b/pimd/pim_igmp.h
@@ -37,6 +37,8 @@
 #define PIM_IGMP_V1_MEMBERSHIP_REPORT (0x12)
 #define PIM_IGMP_V2_MEMBERSHIP_REPORT (0x16)
 #define PIM_IGMP_V2_LEAVE_GROUP       (0x17)
+#define PIM_IGMP_MTRACE_RESPONSE      (0x1E)
+#define PIM_IGMP_MTRACE_QUERY_REQUEST (0x1F)
 #define PIM_IGMP_V3_MEMBERSHIP_REPORT (0x22)
 
 #define IGMP_V3_REPORT_HEADER_SIZE    (8)
diff --git a/pimd/pim_igmp_mtrace.c b/pimd/pim_igmp_mtrace.c
new file mode 100644
index 0000000000..629e10cb0c
--- /dev/null
+++ b/pimd/pim_igmp_mtrace.c
@@ -0,0 +1,727 @@
+/*
+ * Multicast traceroute for FRRouting
+ * Copyright (C) 2017  Mladen Sablic
+ *
+ * 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 "pimd.h"
+#include "pim_util.h"
+#include "pim_sock.h"
+#include "pim_rp.h"
+#include "pim_oil.h"
+#include "pim_ifchannel.h"
+#include "pim_macro.h"
+#include "pim_igmp_mtrace.h"
+
+static void mtrace_rsp_init(struct igmp_mtrace_rsp *mtrace_rspp)
+{
+	mtrace_rspp->arrival = 0;
+	mtrace_rspp->incoming.s_addr = 0;
+	mtrace_rspp->outgoing.s_addr = 0;
+	mtrace_rspp->prev_hop.s_addr = 0;
+	mtrace_rspp->in_count = MTRACE_UNKNOWN_COUNT;
+	mtrace_rspp->out_count = MTRACE_UNKNOWN_COUNT;
+	mtrace_rspp->total = MTRACE_UNKNOWN_COUNT;
+	mtrace_rspp->rtg_proto = 0;
+	mtrace_rspp->fwd_ttl = 0;
+	mtrace_rspp->mbz = 0;
+	mtrace_rspp->s = 0;
+	mtrace_rspp->src_mask = 0;
+	mtrace_rspp->fwd_code = MTRACE_FWD_CODE_NO_ERROR;
+}
+
+static void mtrace_rsp_debug(uint32_t qry_id, int rsp,
+			     struct igmp_mtrace_rsp *mrspp)
+{
+	char inc_str[INET_ADDRSTRLEN];
+	char out_str[INET_ADDRSTRLEN];
+	char prv_str[INET_ADDRSTRLEN];
+
+	zlog_debug(
+		"Rx mt(%d) qid=%ud arr=%x in=%s out=%s prev=%s proto=%d fwd=%d",
+		rsp, ntohl(qry_id), mrspp->arrival,
+		inet_ntop(AF_INET, &(mrspp->incoming), inc_str,
+			  sizeof(inc_str)),
+		inet_ntop(AF_INET, &(mrspp->outgoing), out_str,
+			  sizeof(out_str)),
+		inet_ntop(AF_INET, &(mrspp->prev_hop), prv_str,
+			  sizeof(prv_str)),
+		mrspp->rtg_proto, mrspp->fwd_code);
+}
+
+static void mtrace_debug(struct pim_interface *pim_ifp,
+			 struct igmp_mtrace *mtracep, int mtrace_len)
+{
+	char inc_str[INET_ADDRSTRLEN];
+	char grp_str[INET_ADDRSTRLEN];
+	char src_str[INET_ADDRSTRLEN];
+	char dst_str[INET_ADDRSTRLEN];
+	char rsp_str[INET_ADDRSTRLEN];
+
+	zlog_debug(
+		"Rx mtrace packet incoming on %s: "
+		"hops=%d type=%d size=%d, grp=%s, src=%s,"
+		" dst=%s rsp=%s ttl=%d qid=%ud",
+		inet_ntop(AF_INET, &(pim_ifp->primary_address), inc_str,
+			  sizeof(inc_str)),
+		mtracep->hops, mtracep->type, mtrace_len,
+		inet_ntop(AF_INET, &(mtracep->grp_addr), grp_str,
+			  sizeof(grp_str)),
+		inet_ntop(AF_INET, &(mtracep->src_addr), src_str,
+			  sizeof(src_str)),
+		inet_ntop(AF_INET, &(mtracep->dst_addr), dst_str,
+			  sizeof(dst_str)),
+		inet_ntop(AF_INET, &(mtracep->rsp_addr), rsp_str,
+			  sizeof(rsp_str)),
+		mtracep->rsp_ttl, ntohl(mtracep->qry_id));
+	if (mtrace_len > (int)sizeof(struct igmp_mtrace)) {
+
+		int i;
+
+		int responses = mtrace_len - sizeof(struct igmp_mtrace);
+
+		if ((responses % sizeof(struct igmp_mtrace_rsp)) != 0)
+			if (PIM_DEBUG_MTRACE)
+				zlog_debug(
+					"Mtrace response block of wrong"
+					 " length");
+
+		responses = responses / sizeof(struct igmp_mtrace_rsp);
+
+		for (i = 0; i < responses; i++)
+			mtrace_rsp_debug(mtracep->qry_id, i, &mtracep->rsp[i]);
+	}
+}
+
+/* 5.1 Query Arrival Time */
+static uint32_t query_arrival_time(void)
+{
+	struct timeval tv;
+	uint32_t qat;
+
+	char m_qat[] = "Query arrival time lookup failed: errno=%d: %s";
+
+	if (gettimeofday(&tv, NULL) < 0) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(m_qat, errno, safe_strerror(errno));
+		return 0;
+	}
+	/* not sure second offset correct, as I get different value */
+	qat = ((tv.tv_sec + 32384) << 16) + ((tv.tv_usec << 10) / 15625);
+
+	return qat;
+}
+
+static int mtrace_send_packet(struct interface *ifp,
+			      struct igmp_mtrace *mtracep,
+			      size_t mtrace_buf_len, struct in_addr dst_addr,
+			      struct in_addr group_addr)
+{
+	struct sockaddr_in to;
+	struct pim_interface *pim_ifp;
+	socklen_t tolen;
+	ssize_t sent;
+	int ret;
+	int fd;
+	char pim_str[INET_ADDRSTRLEN];
+	char rsp_str[INET_ADDRSTRLEN];
+	u_char ttl;
+
+	pim_ifp = ifp->info;
+
+	memset(&to, 0, sizeof(to));
+	to.sin_family = AF_INET;
+	to.sin_addr = dst_addr;
+	tolen = sizeof(to);
+
+	if (PIM_DEBUG_MTRACE)
+		zlog_debug("Sending mtrace packet to %s on %s",
+			   inet_ntop(AF_INET, &mtracep->rsp_addr, rsp_str,
+				     sizeof(rsp_str)),
+			   inet_ntop(AF_INET, &pim_ifp->primary_address,
+				     pim_str, sizeof(pim_str)));
+
+	fd = pim_socket_raw(IPPROTO_IGMP);
+
+	if (fd < 0)
+		return -1;
+
+	ret = pim_socket_bind(fd, ifp);
+
+	if (ret < 0) {
+		ret = -1;
+		goto close_fd;
+	}
+
+	if (IPV4_CLASS_DE(ntohl(dst_addr.s_addr))) {
+		if (IPV4_MC_LINKLOCAL(ntohl(dst_addr.s_addr))) {
+			ttl = 1;
+		} else {
+			if (mtracep->type == PIM_IGMP_MTRACE_RESPONSE)
+				ttl = mtracep->rsp_ttl;
+			else
+				ttl = 64;
+		}
+		ret = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl,
+				 sizeof(ttl));
+
+		if (ret < 0) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn("Failed to set socket multicast TTL");
+			ret = -1;
+			goto close_fd;
+		}
+	}
+
+	sent = sendto(fd, (char *)mtracep, mtrace_buf_len, MSG_DONTWAIT,
+		      (struct sockaddr *)&to, tolen);
+
+	if (sent != (ssize_t)mtrace_buf_len) {
+		char dst_str[INET_ADDRSTRLEN];
+		char group_str[INET_ADDRSTRLEN];
+
+		pim_inet4_dump("<dst?>", dst_addr, dst_str, sizeof(dst_str));
+		pim_inet4_dump("<group?>", group_addr, group_str,
+			       sizeof(group_str));
+		if (sent < 0) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn(
+					"Send mtrace request failed for %s on"
+					"%s: group=%s msg_size=%zd: errno=%d: "
+					" %s",
+					dst_str, ifp->name, group_str,
+					mtrace_buf_len, errno,
+					safe_strerror(errno));
+		} else {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn(
+					"Send mtrace request failed for %s on"
+					" %s: group=%s msg_size=%zd: sent=%zd",
+					dst_str, ifp->name, group_str,
+					mtrace_buf_len, sent);
+		}
+		ret = -1;
+		goto close_fd;
+	}
+	ret = 0;
+close_fd:
+	close(fd);
+	return ret;
+}
+
+static int mtrace_un_forward_packet(struct pim_instance *pim, struct ip *ip_hdr,
+				    struct interface *interface)
+{
+	struct pim_nexthop nexthop;
+	struct sockaddr_in to;
+	struct interface *if_out;
+	socklen_t tolen;
+	int ret;
+	int fd;
+	int sent;
+	uint16_t checksum;
+
+	checksum = ip_hdr->ip_sum;
+
+	ip_hdr->ip_sum = 0;
+
+	if (checksum != in_cksum(ip_hdr, ip_hdr->ip_hl * 4))
+		return -1;
+
+	if (ip_hdr->ip_ttl-- <= 1)
+		return -1;
+
+	ip_hdr->ip_sum = in_cksum(ip_hdr, ip_hdr->ip_hl * 4);
+
+	fd = pim_socket_raw(IPPROTO_RAW);
+
+	if (fd < 0)
+		return -1;
+
+	pim_socket_ip_hdr(fd);
+
+	if (interface == NULL) {
+		ret = pim_nexthop_lookup(pim, &nexthop, ip_hdr->ip_dst, 0);
+
+		if (ret != 0) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn(
+					"Dropping mtrace packet, "
+					"no route to destination");
+			return -1;
+		}
+
+		if_out = nexthop.interface;
+	} else {
+		if_out = interface;
+	}
+
+	ret = pim_socket_bind(fd, if_out);
+
+	if (ret < 0) {
+		close(fd);
+		return -1;
+	}
+
+	memset(&to, 0, sizeof(to));
+	to.sin_family = AF_INET;
+	to.sin_addr = ip_hdr->ip_dst;
+	tolen = sizeof(to);
+
+	sent = sendto(fd, ip_hdr, ntohs(ip_hdr->ip_len), 0,
+		      (struct sockaddr *)&to, tolen);
+
+	close(fd);
+
+	if (sent < 0) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(
+				"Failed to forward mtrace packet:"
+				" sendto errno=%d, %s",
+				errno, safe_strerror(errno));
+		return -1;
+	}
+
+	if (PIM_DEBUG_MTRACE) {
+		zlog_debug("Fwd mtrace packet len=%u to %s ttl=%u",
+			   ntohs(ip_hdr->ip_len), inet_ntoa(ip_hdr->ip_dst),
+			   ip_hdr->ip_ttl);
+	}
+
+	return 0;
+}
+
+static int mtrace_mc_forward_packet(struct pim_instance *pim, struct ip *ip_hdr)
+{
+	struct prefix_sg sg;
+	struct channel_oil *c_oil;
+	struct listnode *chnode;
+	struct listnode *chnextnode;
+	struct pim_ifchannel *ch = NULL;
+	int ret = -1;
+
+	memset(&sg, 0, sizeof(struct prefix_sg));
+	sg.grp = ip_hdr->ip_dst;
+
+	c_oil = pim_find_channel_oil(pim, &sg);
+
+	if (c_oil == NULL) {
+		if (PIM_DEBUG_MTRACE) {
+			zlog_debug(
+				"Dropping mtrace multicast packet "
+				"len=%u to %s ttl=%u",
+				ntohs(ip_hdr->ip_len),
+				inet_ntoa(ip_hdr->ip_dst), ip_hdr->ip_ttl);
+		}
+		return -1;
+	}
+	if (c_oil->up == NULL)
+		return -1;
+	if (c_oil->up->ifchannels == NULL)
+		return -1;
+	for (ALL_LIST_ELEMENTS(c_oil->up->ifchannels, chnode, chnextnode, ch)) {
+		if (pim_macro_chisin_oiflist(ch)) {
+			int r;
+
+			r = mtrace_un_forward_packet(pim, ip_hdr,
+						     ch->interface);
+			if (r == 0)
+				ret = 0;
+		}
+	}
+	return ret;
+}
+
+
+static int mtrace_forward_packet(struct pim_instance *pim, struct ip *ip_hdr)
+{
+	if (IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr)))
+		return mtrace_mc_forward_packet(pim, ip_hdr);
+	else
+		return mtrace_un_forward_packet(pim, ip_hdr, NULL);
+}
+
+/* 6.5 Sending Traceroute Responses */
+static int mtrace_send_mc_response(struct pim_instance *pim,
+				   struct igmp_mtrace *mtracep,
+				   size_t mtrace_len)
+{
+	struct prefix_sg sg;
+	struct channel_oil *c_oil;
+	struct listnode *chnode;
+	struct listnode *chnextnode;
+	struct pim_ifchannel *ch = NULL;
+	int ret = -1;
+
+	memset(&sg, 0, sizeof(struct prefix_sg));
+	sg.grp = mtracep->rsp_addr;
+
+	c_oil = pim_find_channel_oil(pim, &sg);
+
+	if (c_oil == NULL) {
+		if (PIM_DEBUG_MTRACE) {
+			zlog_debug(
+				"Dropping mtrace multicast response packet "
+				"len=%u to %s",
+				(unsigned int)mtrace_len,
+				inet_ntoa(mtracep->rsp_addr));
+		}
+		return -1;
+	}
+	if (c_oil->up == NULL)
+		return -1;
+	if (c_oil->up->ifchannels == NULL)
+		return -1;
+	for (ALL_LIST_ELEMENTS(c_oil->up->ifchannels, chnode, chnextnode, ch)) {
+		if (pim_macro_chisin_oiflist(ch)) {
+			int r;
+
+			r = mtrace_send_packet(ch->interface, mtracep,
+					       mtrace_len, mtracep->rsp_addr,
+					       mtracep->grp_addr);
+			if (r == 0)
+				ret = 0;
+		}
+	}
+	return ret;
+}
+
+static int mtrace_send_response(struct pim_instance *pim,
+				struct igmp_mtrace *mtracep, size_t mtrace_len)
+{
+	struct pim_nexthop nexthop;
+	int ret;
+
+	mtracep->type = PIM_IGMP_MTRACE_RESPONSE;
+
+	mtracep->checksum = 0;
+	mtracep->checksum = in_cksum((char *)mtracep, mtrace_len);
+
+	if (IPV4_CLASS_DE(ntohl(mtracep->rsp_addr.s_addr))) {
+		struct pim_rpf *p_rpf;
+		char grp_str[INET_ADDRSTRLEN];
+
+		if (pim_rp_i_am_rp(pim, mtracep->rsp_addr))
+			return mtrace_send_mc_response(pim, mtracep,
+						       mtrace_len);
+
+		p_rpf = pim_rp_g(pim, mtracep->rsp_addr);
+
+		if (p_rpf == NULL) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn("mtrace no RP for %s",
+					  inet_ntop(AF_INET,
+						    &(mtracep->rsp_addr),
+						    grp_str, sizeof(grp_str)));
+			return -1;
+		}
+		nexthop = p_rpf->source_nexthop;
+		if (PIM_DEBUG_MTRACE)
+			zlog_debug("mtrace response to RP");
+	} else {
+		/* TODO: should use unicast rib lookup */
+		ret = pim_nexthop_lookup(pim, &nexthop, mtracep->rsp_addr, 1);
+
+		if (ret != 0) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn(
+					"Dropped response qid=%ud, no route to "
+					"response address",
+					mtracep->qry_id);
+			return -1;
+		}
+	}
+
+	return mtrace_send_packet(nexthop.interface, mtracep, mtrace_len,
+				  mtracep->rsp_addr, mtracep->grp_addr);
+}
+
+int igmp_mtrace_recv_qry_req(struct igmp_sock *igmp, struct ip *ip_hdr,
+			     struct in_addr from, const char *from_str,
+			     char *igmp_msg, int igmp_msg_len)
+{
+	static uint32_t qry_id, qry_src;
+	char mtrace_buf[MTRACE_HDR_SIZE + MTRACE_MAX_HOPS * MTRACE_RSP_SIZE];
+	struct pim_nexthop nexthop;
+	struct interface *ifp;
+	struct interface *out_ifp;
+	struct pim_interface *pim_ifp;
+	struct pim_interface *pim_out_ifp;
+	struct pim_instance *pim;
+	struct igmp_mtrace *mtracep;
+	struct igmp_mtrace_rsp *rspp;
+	struct in_addr nh_addr;
+	enum mtrace_fwd_code fwd_code = MTRACE_FWD_CODE_NO_ERROR;
+	int ret;
+	size_t r_len;
+	int last_rsp_ind = 0;
+	size_t mtrace_len;
+	uint16_t recv_checksum;
+	uint16_t checksum;
+
+	ifp = igmp->interface;
+	pim_ifp = ifp->info;
+	pim = pim_ifp->pim;
+
+	/*
+	 * 6. Router Behaviour
+	 * Check if mtrace packet is addressed elsewhere and forward,
+	 * if applicable
+	 */
+	if (!IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr)))
+		if (!if_lookup_exact_address(&ip_hdr->ip_dst, AF_INET,
+					     pim->vrf_id))
+			return mtrace_forward_packet(pim, ip_hdr);
+
+	if (igmp_msg_len < (int)sizeof(struct igmp_mtrace)) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(
+				"Recv mtrace packet from %s on %s: too short,"
+				" len=%d, min=%lu",
+				from_str, ifp->name, igmp_msg_len,
+				sizeof(struct igmp_mtrace));
+		return -1;
+	}
+
+	mtracep = (struct igmp_mtrace *)igmp_msg;
+
+	recv_checksum = mtracep->checksum;
+
+	mtracep->checksum = 0;
+
+	checksum = in_cksum(igmp_msg, igmp_msg_len);
+
+	if (recv_checksum != checksum) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(
+				"Recv mtrace packet from %s on %s: checksum"
+				" mismatch: received=%x computed=%x",
+				from_str, ifp->name, recv_checksum, checksum);
+		return -1;
+	}
+
+	if (PIM_DEBUG_MTRACE)
+		mtrace_debug(pim_ifp, mtracep, igmp_msg_len);
+
+	/* subtract header from message length */
+	r_len = igmp_msg_len - sizeof(struct igmp_mtrace);
+
+	/* Classify mtrace packet, check if it is a query */
+	if (!r_len) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_debug("Received IGMP multicast traceroute query");
+
+		/* 6.1.1  Packet verification */
+		if (!pim_if_connected_to_source(ifp, mtracep->dst_addr)) {
+			if (IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr))) {
+				if (PIM_DEBUG_MTRACE)
+					zlog_debug(
+						"Dropping multicast query "
+						"on wrong interface");
+				return -1;
+			}
+			/* Unicast query on wrong interface */
+			fwd_code = MTRACE_FWD_CODE_WRONG_IF;
+		}
+		if (qry_id == mtracep->qry_id && qry_src == from.s_addr) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_debug(
+					"Dropping multicast query with "
+					"duplicate source and id");
+			return -1;
+		}
+		qry_id = mtracep->qry_id;
+		qry_src = from.s_addr;
+	}
+	/* if response fields length is equal to a whole number of responses */
+	else if ((r_len % sizeof(struct igmp_mtrace_rsp)) == 0) {
+		r_len = igmp_msg_len - sizeof(struct igmp_mtrace);
+
+		if (r_len != 0)
+			last_rsp_ind = r_len / sizeof(struct igmp_mtrace_rsp);
+		if (last_rsp_ind > MTRACE_MAX_HOPS) {
+			if (PIM_DEBUG_MTRACE)
+				zlog_warn("Mtrace request of excessive size");
+			return -1;
+		}
+	} else {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(
+				"Recv mtrace packet from %s on %s: "
+				"invalid length %d",
+				from_str, ifp->name, igmp_msg_len);
+		return -1;
+	}
+
+	/* 6.2.1 Packet Verification - drop not link-local multicast */
+	if (IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr))
+	    && !IPV4_MC_LINKLOCAL(ntohl(ip_hdr->ip_dst.s_addr))) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(
+				"Recv mtrace packet from %s on %s:"
+				" not link-local multicast %s",
+				from_str, ifp->name, inet_ntoa(ip_hdr->ip_dst));
+		return -1;
+	}
+
+	/* 6.2.2. Normal Processing */
+
+	/* 6.2.2. 1. */
+
+	if (last_rsp_ind == MTRACE_MAX_HOPS) {
+		mtracep->rsp[MTRACE_MAX_HOPS - 1].fwd_code =
+			MTRACE_FWD_CODE_NO_SPACE;
+		return mtrace_send_response(pim_ifp->pim, mtracep,
+					    igmp_msg_len);
+	}
+
+	/* calculate new mtrace mtrace lenght with extra response */
+	mtrace_len = igmp_msg_len + sizeof(struct igmp_mtrace_rsp);
+
+	/* copy received query/request */
+	memcpy(mtrace_buf, igmp_msg, igmp_msg_len);
+
+	/* repoint mtracep pointer to copy */
+	mtracep = (struct igmp_mtrace *)mtrace_buf;
+
+	/* pointer for extra response field to be filled in */
+	rspp = &mtracep->rsp[last_rsp_ind];
+
+	/* initialize extra response field */
+	mtrace_rsp_init(rspp);
+
+	rspp->arrival = htonl(query_arrival_time());
+	rspp->outgoing = pim_ifp->primary_address;
+	rspp->out_count = htonl(MTRACE_UNKNOWN_COUNT);
+
+	/* 6.2.2. 2. Attempt to determine forwarding information */
+
+	nh_addr.s_addr = 0;
+
+	ret = pim_nexthop_lookup(pim, &nexthop, mtracep->src_addr, 1);
+
+	if (ret == 0) {
+		char nexthop_str[INET_ADDRSTRLEN];
+
+		if (PIM_DEBUG_MTRACE)
+			zlog_debug("mtrace pim_nexthop_lookup OK");
+
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn("mtrace next_hop=%s",
+				  inet_ntop(nexthop.mrib_nexthop_addr.family,
+					    &nexthop.mrib_nexthop_addr.u.prefix,
+					    nexthop_str, sizeof(nexthop_str)));
+
+		if (nexthop.mrib_nexthop_addr.family == AF_INET)
+			nh_addr = nexthop.mrib_nexthop_addr.u.prefix4;
+	}
+	/* 6.4 Forwarding Traceroute Requests: ... Otherwise, ... */
+	else {
+		if (PIM_DEBUG_MTRACE)
+			zlog_debug("mtrace not found neighbor");
+		if (!fwd_code)
+			rspp->fwd_code = MTRACE_FWD_CODE_NO_ROUTE;
+		else
+			rspp->fwd_code = fwd_code;
+		/* 6.5 Sending Traceroute Responses */
+		return mtrace_send_response(pim, mtracep, mtrace_len);
+	}
+
+	out_ifp = nexthop.interface;
+	pim_out_ifp = out_ifp->info;
+
+	rspp->incoming = pim_out_ifp->primary_address;
+	rspp->prev_hop = nh_addr;
+	rspp->in_count = htonl(MTRACE_UNKNOWN_COUNT);
+	rspp->total = htonl(MTRACE_UNKNOWN_COUNT);
+	rspp->rtg_proto = MTRACE_RTG_PROTO_PIM;
+	rspp->s = 1;
+	rspp->src_mask = 32;
+
+	if (nh_addr.s_addr == 0) {
+		/* reached source? */
+		if (pim_if_connected_to_source(out_ifp, mtracep->src_addr))
+			return mtrace_send_response(pim, mtracep, mtrace_len);
+		/*
+		 * 6.4 Forwarding Traceroute Requests:
+		 * Previous-hop router not known
+		 */
+		inet_aton(MCAST_ALL_ROUTERS, &nh_addr);
+	}
+
+	if (mtracep->hops <= (last_rsp_ind + 1))
+		return mtrace_send_response(pim, mtracep, mtrace_len);
+
+	mtracep->checksum = 0;
+
+	mtracep->checksum = in_cksum(mtrace_buf, mtrace_len);
+
+	return mtrace_send_packet(out_ifp, mtracep, mtrace_len, nh_addr,
+				  mtracep->grp_addr);
+}
+
+int igmp_mtrace_recv_response(struct igmp_sock *igmp, struct ip *ip_hdr,
+			      struct in_addr from, const char *from_str,
+			      char *igmp_msg, int igmp_msg_len)
+{
+	static uint32_t qry_id, rsp_dst;
+	struct interface *ifp;
+	struct pim_interface *pim_ifp;
+	struct pim_instance *pim;
+	struct igmp_mtrace *mtracep;
+	uint16_t recv_checksum;
+	uint16_t checksum;
+
+	ifp = igmp->interface;
+	pim_ifp = ifp->info;
+	pim = pim_ifp->pim;
+
+	mtracep = (struct igmp_mtrace *)igmp_msg;
+
+	recv_checksum = mtracep->checksum;
+
+	mtracep->checksum = 0;
+
+	checksum = in_cksum(igmp_msg, igmp_msg_len);
+
+	if (recv_checksum != checksum) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_warn(
+				"Recv mtrace response from %s on %s: checksum"
+				" mismatch: received=%x computed=%x",
+				from_str, ifp->name, recv_checksum, checksum);
+		return -1;
+	}
+
+	mtracep->checksum = checksum;
+
+	if (PIM_DEBUG_MTRACE)
+		mtrace_debug(pim_ifp, mtracep, igmp_msg_len);
+
+	/* Drop duplicate packets */
+	if (qry_id == mtracep->qry_id && rsp_dst == ip_hdr->ip_dst.s_addr) {
+		if (PIM_DEBUG_MTRACE)
+			zlog_debug("duplicate mtrace response packet dropped");
+		return -1;
+	}
+
+	qry_id = mtracep->qry_id;
+	rsp_dst = ip_hdr->ip_dst.s_addr;
+
+	return mtrace_forward_packet(pim, ip_hdr);
+}
diff --git a/pimd/pim_igmp_mtrace.h b/pimd/pim_igmp_mtrace.h
new file mode 100644
index 0000000000..b5c1008444
--- /dev/null
+++ b/pimd/pim_igmp_mtrace.h
@@ -0,0 +1,103 @@
+/*
+ * Multicast traceroute for FRRouting
+ * Copyright (C) 2017  Mladen Sablic
+ *
+ * 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
+ */
+
+#ifndef PIM_IGMP_MTRACE_H
+#define PIM_IGMP_MTRACE_H
+
+#include <zebra.h>
+
+#include "pim_igmp.h"
+
+#define MTRACE_MAX_HOPS (255)
+#define MTRACE_UNKNOWN_COUNT (0xffffffff)
+
+enum mtrace_fwd_code {
+	MTRACE_FWD_CODE_NO_ERROR        = 0x00,
+	MTRACE_FWD_CODE_WRONG_IF        = 0x01,
+	MTRACE_FWD_CODE_PRUNE_SENT      = 0x02,
+	MTRACE_FWD_CODE_PRUNE_RCVD      = 0x03,
+	MTRACE_FWD_CODE_SCOPED          = 0x04,
+	MTRACE_FWD_CODE_NO_ROUTE        = 0x05,
+	MTRACE_FWD_CODE_WRONG_LAST_HOP  = 0x06,
+	MTRACE_FWD_CODE_NOT_FORWARDING  = 0x07,
+	MTRACE_FWD_CODE_REACHED_RP      = 0x08,
+	MTRACE_FWD_CODE_RPF_IF          = 0x09,
+	MTRACE_FWD_CODE_NO_MULTICAST    = 0x0A,
+	MTRACE_FWD_CODE_INFO_HIDDEN     = 0x0B,
+	MTRACE_FWD_CODE_NO_SPACE        = 0x81,
+	MTRACE_FWD_CODE_OLD_ROUTER      = 0x82,
+	MTRACE_FWD_CODE_ADMIN_PROHIB    = 0x83
+};
+
+enum mtrace_rtg_proto {
+	MTRACE_RTG_PROTO_DVMRP          = 1,
+	MTRACE_RTG_PROTO_MOSPF          = 2,
+	MTRACE_RTG_PROTO_PIM            = 3,
+	MTRACE_RTG_PROTO_CBT            = 4,
+	MTRACE_RTG_PROTO_PIM_SPECIAL    = 5,
+	MTRACE_RTG_PROTO_PIM_STATIC     = 6,
+	MTRACE_RTG_PROTO_DVMRP_STATIC   = 7,
+	MTRACE_RTG_PROTO_PIM_MBGP       = 8,
+	MTRACE_RTG_PROTO_CBT_SPECIAL    = 9,
+	MTRACE_RTG_PROTO_CBT_STATIC     = 10,
+	MTRACE_RTG_PROTO_PIM_ASSERT     = 11,
+};
+
+struct igmp_mtrace_rsp {
+	uint32_t arrival;
+	struct in_addr incoming;
+	struct in_addr outgoing;
+	struct in_addr prev_hop;
+	uint32_t in_count;
+	uint32_t out_count;
+	uint32_t total;
+	uint32_t rtg_proto : 8;
+	uint32_t fwd_ttl : 8;
+	/* little endian order for next three fields */
+	uint32_t src_mask : 6;
+	uint32_t s : 1;
+	uint32_t mbz : 1;
+	uint32_t fwd_code : 8;
+} __attribute__((packed));
+
+struct igmp_mtrace {
+	uint8_t type;
+	uint8_t hops;
+	uint16_t checksum;
+	struct in_addr grp_addr;
+	struct in_addr src_addr;
+	struct in_addr dst_addr;
+	struct in_addr rsp_addr;
+	uint32_t rsp_ttl : 8;
+	uint32_t qry_id : 24;
+	struct igmp_mtrace_rsp rsp[0];
+} __attribute__((packed));
+
+#define MTRACE_HDR_SIZE (sizeof(struct igmp_mtrace))
+#define MTRACE_RSP_SIZE (sizeof(struct igmp_mtrace_rsp))
+
+int igmp_mtrace_recv_qry_req(struct igmp_sock *igmp, struct ip *ip_hdr,
+			     struct in_addr from, const char *from_str,
+			     char *igmp_msg, int igmp_msg_len);
+
+int igmp_mtrace_recv_response(struct igmp_sock *igmp, struct ip *ip_hdr,
+			      struct in_addr from, const char *from_str,
+			      char *igmp_msg, int igmp_msg_len);
+
+#endif /* PIM_IGMP_MTRACE_H */
diff --git a/pimd/pim_oil.c b/pimd/pim_oil.c
index c45b0ce14c..53bbf54f3e 100644
--- a/pimd/pim_oil.c
+++ b/pimd/pim_oil.c
@@ -135,8 +135,8 @@ void pim_channel_oil_free(struct channel_oil *c_oil)
 	XFREE(MTYPE_PIM_CHANNEL_OIL, c_oil);
 }
 
-static struct channel_oil *pim_find_channel_oil(struct pim_instance *pim,
-						struct prefix_sg *sg)
+struct channel_oil *pim_find_channel_oil(struct pim_instance *pim,
+					 struct prefix_sg *sg)
 {
 	struct channel_oil *c_oil = NULL;
 	struct channel_oil lookup;
diff --git a/pimd/pim_oil.h b/pimd/pim_oil.h
index 1168ba0a8f..94d3840e98 100644
--- a/pimd/pim_oil.h
+++ b/pimd/pim_oil.h
@@ -86,6 +86,8 @@ void pim_oil_init(struct pim_instance *pim);
 void pim_oil_terminate(struct pim_instance *pim);
 
 void pim_channel_oil_free(struct channel_oil *c_oil);
+struct channel_oil *pim_find_channel_oil(struct pim_instance *pim,
+					 struct prefix_sg *sg);
 struct channel_oil *pim_channel_oil_add(struct pim_instance *pim,
 					struct prefix_sg *sg,
 					int input_vif_index);
diff --git a/pimd/pim_vty.c b/pimd/pim_vty.c
index 791680a911..688bc42c3d 100644
--- a/pimd/pim_vty.c
+++ b/pimd/pim_vty.c
@@ -78,6 +78,11 @@ int pim_debug_config_write(struct vty *vty)
 		++writes;
 	}
 
+	if (PIM_DEBUG_MTRACE) {
+		vty_out(vty, "debug mtrace\n");
+		++writes;
+	}
+
 	if (PIM_DEBUG_MROUTE_DETAIL) {
 		vty_out(vty, "debug mroute detail\n");
 		++writes;
diff --git a/pimd/pimd.h b/pimd/pimd.h
index cd00a2df46..de7f259319 100644
--- a/pimd/pimd.h
+++ b/pimd/pimd.h
@@ -112,6 +112,7 @@
 #define PIM_MASK_PIM_NHT             (1 << 22)
 #define PIM_MASK_PIM_NHT_DETAIL      (1 << 23)
 #define PIM_MASK_PIM_NHT_RP          (1 << 24)
+#define PIM_MASK_MTRACE              (1 << 25)
 /* Remember 32 bits!!! */
 
 /* PIM error codes */
@@ -188,6 +189,7 @@ extern int32_t qpim_register_probe_time;
 #define PIM_DEBUG_PIM_NHT             (qpim_debugs & PIM_MASK_PIM_NHT)
 #define PIM_DEBUG_PIM_NHT_DETAIL      (qpim_debugs & PIM_MASK_PIM_NHT_DETAIL)
 #define PIM_DEBUG_PIM_NHT_RP          (qpim_debugs & PIM_MASK_PIM_NHT_RP)
+#define PIM_DEBUG_MTRACE              (qpim_debugs & PIM_MASK_MTRACE)
 
 #define PIM_DEBUG_EVENTS       (qpim_debugs & (PIM_MASK_PIM_EVENTS | PIM_MASK_IGMP_EVENTS | PIM_MASK_MSDP_EVENTS))
 #define PIM_DEBUG_PACKETS      (qpim_debugs & (PIM_MASK_PIM_PACKETS | PIM_MASK_IGMP_PACKETS | PIM_MASK_MSDP_PACKETS))
@@ -216,6 +218,7 @@ extern int32_t qpim_register_probe_time;
 #define PIM_DO_DEBUG_MSDP_INTERNAL       (qpim_debugs |= PIM_MASK_MSDP_INTERNAL)
 #define PIM_DO_DEBUG_PIM_NHT             (qpim_debugs |= PIM_MASK_PIM_NHT)
 #define PIM_DO_DEBUG_PIM_NHT_RP          (qpim_debugs |= PIM_MASK_PIM_NHT_RP)
+#define PIM_DO_DEBUG_MTRACE              (qpim_debugs |= PIM_MASK_MTRACE)
 
 #define PIM_DONT_DEBUG_PIM_EVENTS          (qpim_debugs &= ~PIM_MASK_PIM_EVENTS)
 #define PIM_DONT_DEBUG_PIM_PACKETS         (qpim_debugs &= ~PIM_MASK_PIM_PACKETS)
@@ -240,6 +243,7 @@ extern int32_t qpim_register_probe_time;
 #define PIM_DONT_DEBUG_MSDP_INTERNAL       (qpim_debugs &= ~PIM_MASK_MSDP_INTERNAL)
 #define PIM_DONT_DEBUG_PIM_NHT             (qpim_debugs &= ~PIM_MASK_PIM_NHT)
 #define PIM_DONT_DEBUG_PIM_NHT_RP          (qpim_debugs &= ~PIM_MASK_PIM_NHT_RP)
+#define PIM_DONT_DEBUG_MTRACE              (qpim_debugs &= ~PIM_MASK_MTRACE)
 
 void pim_init(void);
 void pim_terminate(void);
diff --git a/pimd/subdir.am b/pimd/subdir.am
index 2fcb061576..2254362221 100644
--- a/pimd/subdir.am
+++ b/pimd/subdir.am
@@ -5,6 +5,7 @@
 if PIMD
 noinst_LIBRARIES += pimd/libpim.a
 sbin_PROGRAMS += pimd/pimd
+bin_PROGRAMS += pimd/mtracebis
 noinst_PROGRAMS += pimd/test_igmpv3_join
 dist_examples_DATA += pimd/pimd.conf.sample
 endif
@@ -18,6 +19,7 @@ pimd_libpim_a_SOURCES = \
 	pimd/pim_iface.c \
 	pimd/pim_ifchannel.c \
 	pimd/pim_igmp.c \
+	pimd/pim_igmp_mtrace.c \
 	pimd/pim_igmpv2.c \
 	pimd/pim_igmpv3.c \
 	pimd/pim_instance.c \
@@ -66,6 +68,7 @@ noinst_HEADERS += \
 	pimd/pim_ifchannel.h \
 	pimd/pim_igmp.h \
 	pimd/pim_igmp_join.h \
+	pimd/pim_igmp_mtrace.h \
 	pimd/pim_igmpv2.h \
 	pimd/pim_igmpv3.h \
 	pimd/pim_instance.h \
@@ -101,6 +104,8 @@ noinst_HEADERS += \
 	pimd/pim_zebra.h \
 	pimd/pim_zlookup.h \
 	pimd/pimd.h \
+	pimd/mtracebis_netlink.h \
+	pimd/mtracebis_routeget.h \
 	# end
 
 pimd_pimd_LDADD = pimd/libpim.a lib/libfrr.la @LIBCAP@
@@ -108,3 +113,9 @@ pimd_pimd_SOURCES = pimd/pim_main.c
 
 pimd_test_igmpv3_join_LDADD = lib/libfrr.la
 pimd_test_igmpv3_join_SOURCES = pimd/test_igmpv3_join.c
+
+pimd_mtracebis_LDADD =  lib/libfrr.la
+pimd_mtracebis_SOURCES = pimd/mtracebis.c \
+			 pimd/mtracebis_netlink.c \
+			 pimd/mtracebis_routeget.c \
+			 # end
diff --git a/vtysh/vtysh.c b/vtysh/vtysh.c
index 94c4ba4330..65e9c9f8c5 100644
--- a/vtysh/vtysh.c
+++ b/vtysh/vtysh.c
@@ -2628,6 +2628,19 @@ ALIAS(vtysh_traceroute, vtysh_traceroute_ip_cmd, "traceroute ip WORD",
       "IP trace\n"
       "Trace route to destination address or hostname\n")
 
+DEFUN (vtysh_mtrace,
+       vtysh_mtrace_cmd,
+       "mtrace WORD",
+       "Multicast trace route to multicast source\n"
+       "Multicast trace route to multicast source address\n")
+{
+	int idx = 1;
+
+	argv_find(argv, argc, "WORD", &idx);
+	execute_command("mtracebis", 1, argv[idx]->arg, NULL);
+	return CMD_SUCCESS;
+}
+
 DEFUN (vtysh_ping6,
        vtysh_ping6_cmd,
        "ping ipv6 WORD",
@@ -3327,6 +3340,7 @@ void vtysh_init_vty(void)
 	install_element(VIEW_NODE, &vtysh_ping_ip_cmd);
 	install_element(VIEW_NODE, &vtysh_traceroute_cmd);
 	install_element(VIEW_NODE, &vtysh_traceroute_ip_cmd);
+	install_element(VIEW_NODE, &vtysh_mtrace_cmd);
 	install_element(VIEW_NODE, &vtysh_ping6_cmd);
 	install_element(VIEW_NODE, &vtysh_traceroute6_cmd);
 #if defined(HAVE_SHELL_ACCESS)