1 files changed, 395 insertions, 0 deletions
diff --git a/pimd/pim_ssmpingd.c b/pimd/pim_ssmpingd.c
new file mode 100644
index 0000000..c8d4037
--- /dev/null
+++ b/pimd/pim_ssmpingd.c
@@ -0,0 +1,395 @@
+/*
+ * PIM for Quagga
+ * Copyright (C) 2008  Everton da Silva Marques
+ *
+ * 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 "if.h"
+#include "log.h"
+#include "memory.h"
+#include "sockopt.h"
+#include "vrf.h"
+#include "lib_errors.h"
+
+#include "pimd.h"
+#include "pim_instance.h"
+#include "pim_ssmpingd.h"
+#include "pim_time.h"
+#include "pim_sock.h"
+#include "network.h"
+
+#if PIM_IPV == 4
+static const char *const PIM_SSMPINGD_REPLY_GROUP = "232.43.211.234";
+#else
+static const char *const PIM_SSMPINGD_REPLY_GROUP = "ff3e::4321:1234";
+#endif
+
+enum { PIM_SSMPINGD_REQUEST = 'Q', PIM_SSMPINGD_REPLY = 'A' };
+
+static void ssmpingd_read_on(struct ssmpingd_sock *ss);
+
+void pim_ssmpingd_init(struct pim_instance *pim)
+{
+	int result;
+
+	assert(!pim->ssmpingd_list);
+
+	result = inet_pton(PIM_AF, PIM_SSMPINGD_REPLY_GROUP,
+			   &pim->ssmpingd_group_addr);
+
+	assert(result > 0);
+}
+
+void pim_ssmpingd_destroy(struct pim_instance *pim)
+{
+	if (pim->ssmpingd_list)
+		list_delete(&pim->ssmpingd_list);
+}
+
+static struct ssmpingd_sock *ssmpingd_find(struct pim_instance *pim,
+					   pim_addr source_addr)
+{
+	struct listnode *node;
+	struct ssmpingd_sock *ss;
+
+	if (!pim->ssmpingd_list)
+		return 0;
+
+	for (ALL_LIST_ELEMENTS_RO(pim->ssmpingd_list, node, ss))
+		if (!pim_addr_cmp(source_addr, ss->source_addr))
+			return ss;
+
+	return 0;
+}
+
+static void ssmpingd_free(struct ssmpingd_sock *ss)
+{
+	XFREE(MTYPE_PIM_SSMPINGD, ss);
+}
+
+#if PIM_IPV == 4
+static inline int ssmpingd_setsockopt(int fd, pim_addr addr, int mttl)
+{
+	/* Needed to obtain destination address from recvmsg() */
+#if defined(HAVE_IP_PKTINFO)
+	/* Linux and Solaris IP_PKTINFO */
+	int opt = 1;
+	if (setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &opt, sizeof(opt))) {
+		zlog_warn(
+			"%s: could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
+			__func__, fd, errno, safe_strerror(errno));
+	}
+#elif defined(HAVE_IP_RECVDSTADDR)
+	/* BSD IP_RECVDSTADDR */
+	int opt = 1;
+	if (setsockopt(fd, IPPROTO_IP, IP_RECVDSTADDR, &opt, sizeof(opt))) {
+		zlog_warn(
+			"%s: could not set IP_RECVDSTADDR on socket fd=%d: errno=%d: %s",
+			__func__, fd, errno, safe_strerror(errno));
+	}
+#else
+	flog_err(
+		EC_LIB_DEVELOPMENT,
+		"%s %s: missing IP_PKTINFO and IP_RECVDSTADDR: unable to get dst addr from recvmsg()",
+		__FILE__, __func__);
+	close(fd);
+	return -1;
+#endif
+
+	if (setsockopt_ipv4_multicast_loop(fd, 0)) {
+		zlog_warn(
+			"%s: could not disable Multicast Loopback Option on socket fd=%d: errno=%d: %s",
+			__func__, fd, errno, safe_strerror(errno));
+		close(fd);
+		return PIM_SOCK_ERR_LOOP;
+	}
+
+	if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, (void *)&addr,
+		       sizeof(addr))) {
+		zlog_warn(
+			"%s: could not set Outgoing Interface Option on socket fd=%d: errno=%d: %s",
+			__func__, fd, errno, safe_strerror(errno));
+		close(fd);
+		return -1;
+	}
+
+	if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, (void *)&mttl,
+		       sizeof(mttl))) {
+		zlog_warn(
+			"%s: could not set multicast TTL=%d on socket fd=%d: errno=%d: %s",
+			__func__, mttl, fd, errno, safe_strerror(errno));
+		close(fd);
+		return -1;
+	}
+
+	return 0;
+}
+#else
+static inline int ssmpingd_setsockopt(int fd, pim_addr addr, int mttl)
+{
+	setsockopt_ipv6_pktinfo(fd, 1);
+	setsockopt_ipv6_multicast_hops(fd, mttl);
+
+	if (setsockopt_ipv6_multicast_loop(fd, 0)) {
+		zlog_warn(
+			"%s: could not disable Multicast Loopback Option on socket fd=%d: errno=%d: %s",
+			__func__, fd, errno, safe_strerror(errno));
+		close(fd);
+		return PIM_SOCK_ERR_LOOP;
+	}
+
+	if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_IF, (void *)&addr,
+		       sizeof(addr))) {
+		zlog_warn(
+			"%s: could not set Outgoing Interface Option on socket fd=%d: errno=%d: %s",
+			__func__, fd, errno, safe_strerror(errno));
+		close(fd);
+		return -1;
+	}
+	return 0;
+}
+#endif
+
+
+static int ssmpingd_socket(pim_addr addr, int port, int mttl)
+{
+	struct sockaddr_storage sockaddr;
+	int fd;
+	int ret;
+	socklen_t len = sizeof(sockaddr);
+
+	fd = socket(PIM_AF, SOCK_DGRAM, IPPROTO_UDP);
+	if (fd < 0) {
+		flog_err_sys(EC_LIB_SOCKET,
+			     "%s: could not create socket: errno=%d: %s",
+			     __func__, errno, safe_strerror(errno));
+		return -1;
+	}
+
+	pim_socket_getsockname(fd, (struct sockaddr *)&sockaddr, &len);
+
+	if (bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr))) {
+		zlog_warn(
+			"%s: bind(fd=%d,addr=%pSUp,port=%d,len=%zu) failure: errno=%d: %s",
+			__func__, fd, &sockaddr, port, sizeof(sockaddr), errno,
+			safe_strerror(errno));
+		close(fd);
+		return -1;
+	}
+
+	set_nonblocking(fd);
+	sockopt_reuseaddr(fd);
+
+	ret = ssmpingd_setsockopt(fd, addr, mttl);
+	if (ret) {
+		zlog_warn("ssmpingd_setsockopt failed");
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+static void ssmpingd_delete(struct ssmpingd_sock *ss)
+{
+	assert(ss);
+
+	THREAD_OFF(ss->t_sock_read);
+
+	if (close(ss->sock_fd)) {
+		zlog_warn(
+			"%s: failure closing ssmpingd sock_fd=%d for source %pPA: errno=%d: %s",
+			__func__, ss->sock_fd, &ss->source_addr, errno,
+			safe_strerror(errno));
+		/* warning only */
+	}
+
+	listnode_delete(ss->pim->ssmpingd_list, ss);
+	ssmpingd_free(ss);
+}
+
+static void ssmpingd_sendto(struct ssmpingd_sock *ss, const uint8_t *buf,
+			    int len, struct sockaddr_storage to)
+{
+	socklen_t tolen = sizeof(to);
+	int sent;
+
+	sent = sendto(ss->sock_fd, buf, len, MSG_DONTWAIT,
+		      (struct sockaddr *)&to, tolen);
+	if (sent != len) {
+		if (sent < 0) {
+			zlog_warn(
+				"%s: sendto() failure to %pSUp,fd=%d len=%d: errno=%d: %s",
+				__func__, &to, ss->sock_fd, len, errno,
+				safe_strerror(errno));
+		} else {
+			zlog_warn(
+				"%s: sendto() partial to %pSUp, fd=%d len=%d: sent=%d",
+				__func__, &to, ss->sock_fd, len, sent);
+		}
+	}
+}
+
+static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
+{
+	struct interface *ifp;
+	struct sockaddr_storage from;
+	struct sockaddr_storage to;
+	socklen_t fromlen = sizeof(from);
+	socklen_t tolen = sizeof(to);
+	ifindex_t ifindex = -1;
+	uint8_t buf[1000];
+	int len;
+
+	++ss->requests;
+
+	len = pim_socket_recvfromto(ss->sock_fd, buf, sizeof(buf), &from,
+				    &fromlen, &to, &tolen, &ifindex);
+
+	if (len < 0) {
+		zlog_warn(
+			"%s: failure receiving ssmping for source %pPA on fd=%d: errno=%d: %s",
+			__func__, &ss->source_addr, ss->sock_fd, errno,
+			safe_strerror(errno));
+		return -1;
+	}
+
+	ifp = if_lookup_by_index(ifindex, ss->pim->vrf->vrf_id);
+
+	if (buf[0] != PIM_SSMPINGD_REQUEST) {
+		zlog_warn(
+			"%s: bad ssmping type=%d from %pSUp to %pSUp on interface %s ifindex=%d fd=%d src=%pPA",
+			__func__, buf[0], &from, &to,
+			ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
+			&ss->source_addr);
+		return 0;
+	}
+
+	if (PIM_DEBUG_SSMPINGD) {
+		zlog_debug(
+			"%s: recv ssmping from %pSUp, to %pSUp, on interface %s ifindex=%d fd=%d src=%pPA",
+			__func__, &from, &to, ifp ? ifp->name : "<iface?>",
+			ifindex, ss->sock_fd, &ss->source_addr);
+	}
+
+	buf[0] = PIM_SSMPINGD_REPLY;
+
+	/* unicast reply */
+	ssmpingd_sendto(ss, buf, len, from);
+
+	/* multicast reply */
+	memcpy(&from, &ss->pim->ssmpingd_group_addr, sizeof(pim_addr));
+	ssmpingd_sendto(ss, buf, len, from);
+
+	return 0;
+}
+
+static void ssmpingd_sock_read(struct thread *t)
+{
+	struct ssmpingd_sock *ss;
+
+	ss = THREAD_ARG(t);
+
+	ssmpingd_read_msg(ss);
+
+	/* Keep reading */
+	ssmpingd_read_on(ss);
+}
+
+static void ssmpingd_read_on(struct ssmpingd_sock *ss)
+{
+	thread_add_read(router->master, ssmpingd_sock_read, ss, ss->sock_fd,
+			&ss->t_sock_read);
+}
+
+static struct ssmpingd_sock *ssmpingd_new(struct pim_instance *pim,
+					  pim_addr source_addr)
+{
+	struct ssmpingd_sock *ss;
+	int sock_fd;
+
+	if (!pim->ssmpingd_list) {
+		pim->ssmpingd_list = list_new();
+		pim->ssmpingd_list->del = (void (*)(void *))ssmpingd_free;
+	}
+
+	sock_fd =
+		ssmpingd_socket(source_addr, /* port: */ 4321, /* mTTL: */ 64);
+	if (sock_fd < 0) {
+		zlog_warn("%s: ssmpingd_socket() failure for source %pPA",
+			  __func__, &source_addr);
+		return 0;
+	}
+
+	ss = XCALLOC(MTYPE_PIM_SSMPINGD, sizeof(*ss));
+
+	ss->pim = pim;
+	ss->sock_fd = sock_fd;
+	ss->t_sock_read = NULL;
+	ss->source_addr = source_addr;
+	ss->creation = pim_time_monotonic_sec();
+	ss->requests = 0;
+
+	listnode_add(pim->ssmpingd_list, ss);
+
+	ssmpingd_read_on(ss);
+
+	return ss;
+}
+
+int pim_ssmpingd_start(struct pim_instance *pim, pim_addr source_addr)
+{
+	struct ssmpingd_sock *ss;
+
+	ss = ssmpingd_find(pim, source_addr);
+	if (ss) {
+		/* silently ignore request to recreate entry */
+		return 0;
+	}
+
+	zlog_info("%s: starting ssmpingd for source %pPAs", __func__,
+		  &source_addr);
+
+	ss = ssmpingd_new(pim, source_addr);
+	if (!ss) {
+		zlog_warn("%s: ssmpingd_new() failure for source %pPAs",
+			  __func__, &source_addr);
+		return -1;
+	}
+
+	return 0;
+}
+
+int pim_ssmpingd_stop(struct pim_instance *pim, pim_addr source_addr)
+{
+	struct ssmpingd_sock *ss;
+
+	ss = ssmpingd_find(pim, source_addr);
+	if (!ss) {
+		zlog_warn("%s: could not find ssmpingd for source %pPAs",
+			  __func__, &source_addr);
+		return -1;
+	}
+
+	zlog_info("%s: stopping ssmpingd for source %pPAs", __func__,
+		  &source_addr);
+
+	ssmpingd_delete(ss);
+
+	return 0;
+}