[mirror_frr.git] / pimd / pim_ssmpingd.c

/*
 * 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 "pimd.h"
#include "pim_ssmpingd.h"
#include "pim_time.h"
#include "pim_sock.h"

static const char *const PIM_SSMPINGD_REPLY_GROUP = "232.43.211.234";

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;

	zassert(!pim->ssmpingd_list);

	result = inet_pton(AF_INET, PIM_SSMPINGD_REPLY_GROUP,
			   &pim->ssmpingd_group_addr);

	zassert(result > 0);
}

void pim_ssmpingd_destroy(struct pim_instance *pim)
{
	if (pim->ssmpingd_list)
		list_delete_and_null(&pim->ssmpingd_list);
}

static struct ssmpingd_sock *ssmpingd_find(struct pim_instance *pim,
					   struct in_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 (source_addr.s_addr == ss->source_addr.s_addr)
			return ss;

	return 0;
}

static void ssmpingd_free(struct ssmpingd_sock *ss)
{
	XFREE(MTYPE_PIM_SSMPINGD, ss);
}

static int ssmpingd_socket(struct in_addr addr, int port, int mttl)
{
	struct sockaddr_in sockaddr;
	int fd;

	fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if (fd < 0) {
		zlog_err("%s: could not create socket: errno=%d: %s",
			 __PRETTY_FUNCTION__, errno, safe_strerror(errno));
		return -1;
	}

	sockaddr.sin_family = AF_INET;
	sockaddr.sin_addr = addr;
	sockaddr.sin_port = htons(port);

	if (bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr))) {
		char addr_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<addr?>", addr, addr_str, sizeof(addr_str));
		zlog_warn(
			"%s: bind(fd=%d,addr=%s,port=%d,len=%zu) failure: errno=%d: %s",
			__PRETTY_FUNCTION__, fd, addr_str, port,
			sizeof(sockaddr), errno, safe_strerror(errno));
		close(fd);
		return -1;
	}

	/* 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",
				__PRETTY_FUNCTION__, 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",
				__PRETTY_FUNCTION__, fd, errno,
				safe_strerror(errno));
		}
#else
		zlog_err(
			"%s %s: missing IP_PKTINFO and IP_RECVDSTADDR: unable to get dst addr from recvmsg()",
			__FILE__, __PRETTY_FUNCTION__);
		close(fd);
		return -1;
#endif
	}

	{
		int reuse = 1;
		if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (void *)&reuse,
			       sizeof(reuse))) {
			zlog_warn(
				"%s: could not set Reuse Address Option on socket fd=%d: errno=%d: %s",
				__PRETTY_FUNCTION__, 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",
			__PRETTY_FUNCTION__, mttl, fd, errno,
			safe_strerror(errno));
		close(fd);
		return -1;
	}

	if (setsockopt_ipv4_multicast_loop(fd, 0)) {
		zlog_warn(
			"%s: could not disable Multicast Loopback Option on socket fd=%d: errno=%d: %s",
			__PRETTY_FUNCTION__, 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",
			__PRETTY_FUNCTION__, fd, errno, safe_strerror(errno));
		close(fd);
		return -1;
	}

	{
		long flags;

		flags = fcntl(fd, F_GETFL, 0);
		if (flags < 0) {
			zlog_warn(
				"%s: could not get fcntl(F_GETFL,O_NONBLOCK) on socket fd=%d: errno=%d: %s",
				__PRETTY_FUNCTION__, fd, errno,
				safe_strerror(errno));
			close(fd);
			return -1;
		}

		if (fcntl(fd, F_SETFL, flags | O_NONBLOCK)) {
			zlog_warn(
				"%s: could not set fcntl(F_SETFL,O_NONBLOCK) on socket fd=%d: errno=%d: %s",
				__PRETTY_FUNCTION__, fd, errno,
				safe_strerror(errno));
			close(fd);
			return -1;
		}
	}

	return fd;
}

static void ssmpingd_delete(struct ssmpingd_sock *ss)
{
	zassert(ss);

	THREAD_OFF(ss->t_sock_read);

	if (close(ss->sock_fd)) {
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", ss->source_addr, source_str,
			       sizeof(source_str));
		zlog_warn(
			"%s: failure closing ssmpingd sock_fd=%d for source %s: errno=%d: %s",
			__PRETTY_FUNCTION__, ss->sock_fd, source_str, 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_in to)
{
	socklen_t tolen = sizeof(to);
	int sent;

	sent = sendto(ss->sock_fd, buf, len, MSG_DONTWAIT,
		      (struct sockaddr *)&to, tolen);
	if (sent != len) {
		char to_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
		if (sent < 0) {
			zlog_warn(
				"%s: sendto() failure to %s,%d: fd=%d len=%d: errno=%d: %s",
				__PRETTY_FUNCTION__, to_str, ntohs(to.sin_port),
				ss->sock_fd, len, errno, safe_strerror(errno));
		} else {
			zlog_warn(
				"%s: sendto() partial to %s,%d: fd=%d len=%d: sent=%d",
				__PRETTY_FUNCTION__, to_str, ntohs(to.sin_port),
				ss->sock_fd, len, sent);
		}
	}
}

static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
{
	struct interface *ifp;
	struct sockaddr_in from;
	struct sockaddr_in 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) {
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", ss->source_addr, source_str,
			       sizeof(source_str));
		zlog_warn(
			"%s: failure receiving ssmping for source %s on fd=%d: errno=%d: %s",
			__PRETTY_FUNCTION__, source_str, ss->sock_fd, errno,
			safe_strerror(errno));
		return -1;
	}

	ifp = if_lookup_by_index(ifindex, ss->pim->vrf_id);

	if (buf[0] != PIM_SSMPINGD_REQUEST) {
		char source_str[INET_ADDRSTRLEN];
		char from_str[INET_ADDRSTRLEN];
		char to_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", ss->source_addr, source_str,
			       sizeof(source_str));
		pim_inet4_dump("<from?>", from.sin_addr, from_str,
			       sizeof(from_str));
		pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
		zlog_warn(
			"%s: bad ssmping type=%d from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
			__PRETTY_FUNCTION__, buf[0], from_str,
			ntohs(from.sin_port), to_str, ntohs(to.sin_port),
			ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
			source_str);
		return 0;
	}

	if (PIM_DEBUG_SSMPINGD) {
		char source_str[INET_ADDRSTRLEN];
		char from_str[INET_ADDRSTRLEN];
		char to_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", ss->source_addr, source_str,
			       sizeof(source_str));
		pim_inet4_dump("<from?>", from.sin_addr, from_str,
			       sizeof(from_str));
		pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
		zlog_debug(
			"%s: recv ssmping from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
			__PRETTY_FUNCTION__, from_str, ntohs(from.sin_port),
			to_str, ntohs(to.sin_port),
			ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
			source_str);
	}

	buf[0] = PIM_SSMPINGD_REPLY;

	/* unicast reply */
	ssmpingd_sendto(ss, buf, len, from);

	/* multicast reply */
	from.sin_addr = ss->pim->ssmpingd_group_addr;
	ssmpingd_sendto(ss, buf, len, from);

	return 0;
}

static int ssmpingd_sock_read(struct thread *t)
{
	struct ssmpingd_sock *ss;
	int result;

	ss = THREAD_ARG(t);

	result = ssmpingd_read_msg(ss);

	/* Keep reading */
	ssmpingd_read_on(ss);

	return result;
}

static void ssmpingd_read_on(struct ssmpingd_sock *ss)
{
	thread_add_read(master, ssmpingd_sock_read, ss, ss->sock_fd,
			&ss->t_sock_read);
}

static struct ssmpingd_sock *ssmpingd_new(struct pim_instance *pim,
					  struct in_addr source_addr)
{
	struct ssmpingd_sock *ss;
	int sock_fd;

	if (!pim->ssmpingd_list) {
		pim->ssmpingd_list = list_new();
		if (!pim->ssmpingd_list) {
			zlog_err(
				"%s %s: failure: qpim_ssmpingd_list=list_new()",
				__FILE__, __PRETTY_FUNCTION__);
			return 0;
		}
		pim->ssmpingd_list->del = (void (*)(void *))ssmpingd_free;
	}

	sock_fd =
		ssmpingd_socket(source_addr, /* port: */ 4321, /* mTTL: */ 64);
	if (sock_fd < 0) {
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", source_addr, source_str,
			       sizeof(source_str));
		zlog_warn("%s: ssmpingd_socket() failure for source %s",
			  __PRETTY_FUNCTION__, source_str);
		return 0;
	}

	ss = XCALLOC(MTYPE_PIM_SSMPINGD, sizeof(*ss));
	if (!ss) {
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", source_addr, source_str,
			       sizeof(source_str));
		zlog_err("%s: XCALLOC(%zu) failure for ssmpingd source %s",
			 __PRETTY_FUNCTION__, sizeof(*ss), source_str);
		close(sock_fd);
		return 0;
	}

	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, struct in_addr source_addr)
{
	struct ssmpingd_sock *ss;

	ss = ssmpingd_find(pim, source_addr);
	if (ss) {
		/* silently ignore request to recreate entry */
		return 0;
	}

	{
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", source_addr, source_str,
			       sizeof(source_str));
		zlog_info("%s: starting ssmpingd for source %s",
			  __PRETTY_FUNCTION__, source_str);
	}

	ss = ssmpingd_new(pim, source_addr);
	if (!ss) {
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", source_addr, source_str,
			       sizeof(source_str));
		zlog_warn("%s: ssmpingd_new() failure for source %s",
			  __PRETTY_FUNCTION__, source_str);
		return -1;
	}

	return 0;
}

int pim_ssmpingd_stop(struct pim_instance *pim, struct in_addr source_addr)
{
	struct ssmpingd_sock *ss;

	ss = ssmpingd_find(pim, source_addr);
	if (!ss) {
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", source_addr, source_str,
			       sizeof(source_str));
		zlog_warn("%s: could not find ssmpingd for source %s",
			  __PRETTY_FUNCTION__, source_str);
		return -1;
	}

	{
		char source_str[INET_ADDRSTRLEN];
		pim_inet4_dump("<src?>", source_addr, source_str,
			       sizeof(source_str));
		zlog_info("%s: stopping ssmpingd for source %s",
			  __PRETTY_FUNCTION__, source_str);
	}

	ssmpingd_delete(ss);

	return 0;
}
Commit	Line	Data
12e41d03	1	/*
896014f4 DL	2	* PIM for Quagga
	3	* Copyright (C) 2008 Everton da Silva Marques
	4	*
	5	* This program is free software; you can redistribute it and/or modify
	6	* it under the terms of the GNU General Public License as published by
	7	* the Free Software Foundation; either version 2 of the License, or
	8	* (at your option) any later version.
	9	*
	10	* This program is distributed in the hope that it will be useful, but
	11	* WITHOUT ANY WARRANTY; without even the implied warranty of
	12	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	13	* General Public License for more details.
	14	*
	15	* You should have received a copy of the GNU General Public License along
	16	* with this program; see the file COPYING; if not, write to the Free Software
	17	* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
	18	*/
12e41d03 DL	19
	20	#include <zebra.h>
	21
	22	#include "if.h"
	23	#include "log.h"
	24	#include "memory.h"
c5bdb09f	25	#include "sockopt.h"
7e2b7603	26	#include "vrf.h"
12e41d03	27
8bfb8b67	28	#include "pimd.h"
12e41d03 DL	29	#include "pim_ssmpingd.h"
	30	#include "pim_time.h"
	31	#include "pim_sock.h"
12e41d03	32
d62a17ae	33	static const char *const PIM_SSMPINGD_REPLY_GROUP = "232.43.211.234";
12e41d03	34
d62a17ae	35	enum { PIM_SSMPINGD_REQUEST = 'Q', PIM_SSMPINGD_REPLY = 'A' };
12e41d03 DL	36
	37	static void ssmpingd_read_on(struct ssmpingd_sock *ss);
	38
71ad9915	39	void pim_ssmpingd_init(struct pim_instance *pim)
12e41d03	40	{
d62a17ae	41	int result;
12e41d03	42
71ad9915	43	zassert(!pim->ssmpingd_list);
12e41d03	44
d62a17ae	45	result = inet_pton(AF_INET, PIM_SSMPINGD_REPLY_GROUP,
71ad9915	46	&pim->ssmpingd_group_addr);
d62a17ae	47
d62a17ae	48	zassert(result > 0);
12e41d03 DL	49	}
12e41d03 DL	50
71ad9915	51	void pim_ssmpingd_destroy(struct pim_instance *pim)
12e41d03	52	{
affe9e99 DS	53	if (pim->ssmpingd_list)
affe9e99 DS	54	list_delete_and_null(&pim->ssmpingd_list);
12e41d03 DL	55	}
12e41d03 DL	56
71ad9915 DS	57	static struct ssmpingd_sock ssmpingd_find(struct pim_instance pim,
71ad9915 DS	58	struct in_addr source_addr)
12e41d03	59	{
d62a17ae	60	struct listnode *node;
d62a17ae	61	struct ssmpingd_sock *ss;
12e41d03	62
71ad9915	63	if (!pim->ssmpingd_list)
d62a17ae	64	return 0;
12e41d03	65
71ad9915	66	for (ALL_LIST_ELEMENTS_RO(pim->ssmpingd_list, node, ss))
d62a17ae	67	if (source_addr.s_addr == ss->source_addr.s_addr)
d62a17ae	68	return ss;
12e41d03	69
d62a17ae	70	return 0;
12e41d03 DL	71	}
	72
	73	static void ssmpingd_free(struct ssmpingd_sock *ss)
	74	{
d62a17ae	75	XFREE(MTYPE_PIM_SSMPINGD, ss);
12e41d03 DL	76	}
	77
	78	static int ssmpingd_socket(struct in_addr addr, int port, int mttl)
	79	{
d62a17ae	80	struct sockaddr_in sockaddr;
	81	int fd;
	82
	83	fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	84	if (fd < 0) {
	85	zlog_err("%s: could not create socket: errno=%d: %s",
	86	__PRETTY_FUNCTION__, errno, safe_strerror(errno));
	87	return -1;
	88	}
	89
	90	sockaddr.sin_family = AF_INET;
	91	sockaddr.sin_addr = addr;
	92	sockaddr.sin_port = htons(port);
	93
	94	if (bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr))) {
	95	char addr_str[INET_ADDRSTRLEN];
	96	pim_inet4_dump("<addr?>", addr, addr_str, sizeof(addr_str));
	97	zlog_warn(
	98	"%s: bind(fd=%d,addr=%s,port=%d,len=%zu) failure: errno=%d: %s",
	99	__PRETTY_FUNCTION__, fd, addr_str, port,
	100	sizeof(sockaddr), errno, safe_strerror(errno));
	101	close(fd);
	102	return -1;
	103	}
	104
	105	/* Needed to obtain destination address from recvmsg() */
	106	{
12e41d03	107	#if defined(HAVE_IP_PKTINFO)
d62a17ae	108	/* Linux and Solaris IP_PKTINFO */
	109	int opt = 1;
	110	if (setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &opt, sizeof(opt))) {
	111	zlog_warn(
	112	"%s: could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
	113	__PRETTY_FUNCTION__, fd, errno,
	114	safe_strerror(errno));
	115	}
12e41d03	116	#elif defined(HAVE_IP_RECVDSTADDR)
d62a17ae	117	/* BSD IP_RECVDSTADDR */
	118	int opt = 1;
	119	if (setsockopt(fd, IPPROTO_IP, IP_RECVDSTADDR, &opt,
	120	sizeof(opt))) {
	121	zlog_warn(
	122	"%s: could not set IP_RECVDSTADDR on socket fd=%d: errno=%d: %s",
	123	__PRETTY_FUNCTION__, fd, errno,
	124	safe_strerror(errno));
	125	}
12e41d03	126	#else
d62a17ae	127	zlog_err(
	128	"%s %s: missing IP_PKTINFO and IP_RECVDSTADDR: unable to get dst addr from recvmsg()",
	129	__FILE__, __PRETTY_FUNCTION__);
	130	close(fd);
	131	return -1;
12e41d03	132	#endif
d62a17ae	133	}
	134
	135	{
	136	int reuse = 1;
	137	if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (void *)&reuse,
	138	sizeof(reuse))) {
	139	zlog_warn(
	140	"%s: could not set Reuse Address Option on socket fd=%d: errno=%d: %s",
	141	__PRETTY_FUNCTION__, fd, errno,
	142	safe_strerror(errno));
	143	close(fd);
	144	return -1;
	145	}
	146	}
	147
	148	if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, (void *)&mttl,
	149	sizeof(mttl))) {
	150	zlog_warn(
	151	"%s: could not set multicast TTL=%d on socket fd=%d: errno=%d: %s",
	152	__PRETTY_FUNCTION__, mttl, fd, errno,
	153	safe_strerror(errno));
	154	close(fd);
	155	return -1;
	156	}
	157
	158	if (setsockopt_ipv4_multicast_loop(fd, 0)) {
	159	zlog_warn(
	160	"%s: could not disable Multicast Loopback Option on socket fd=%d: errno=%d: %s",
	161	__PRETTY_FUNCTION__, fd, errno, safe_strerror(errno));
	162	close(fd);
	163	return PIM_SOCK_ERR_LOOP;
	164	}
	165
	166	if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, (void *)&addr,
	167	sizeof(addr))) {
	168	zlog_warn(
	169	"%s: could not set Outgoing Interface Option on socket fd=%d: errno=%d: %s",
	170	__PRETTY_FUNCTION__, fd, errno, safe_strerror(errno));
	171	close(fd);
	172	return -1;
	173	}
	174
	175	{
	176	long flags;
	177
	178	flags = fcntl(fd, F_GETFL, 0);
	179	if (flags < 0) {
	180	zlog_warn(
	181	"%s: could not get fcntl(F_GETFL,O_NONBLOCK) on socket fd=%d: errno=%d: %s",
	182	__PRETTY_FUNCTION__, fd, errno,
	183	safe_strerror(errno));
	184	close(fd);
	185	return -1;
	186	}
	187
	188	if (fcntl(fd, F_SETFL, flags \| O_NONBLOCK)) {
	189	zlog_warn(
	190	"%s: could not set fcntl(F_SETFL,O_NONBLOCK) on socket fd=%d: errno=%d: %s",
	191	__PRETTY_FUNCTION__, fd, errno,
	192	safe_strerror(errno));
	193	close(fd);
	194	return -1;
	195	}
	196	}
197
198	return fd;
12e41d03 DL	199	}
	200
	201	static void ssmpingd_delete(struct ssmpingd_sock *ss)
	202	{
d62a17ae	203	zassert(ss);
d62a17ae	204
	205	THREAD_OFF(ss->t_sock_read);
	206
	207	if (close(ss->sock_fd)) {
	208	char source_str[INET_ADDRSTRLEN];
	209	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	210	sizeof(source_str));
	211	zlog_warn(
	212	"%s: failure closing ssmpingd sock_fd=%d for source %s: errno=%d: %s",
	213	__PRETTY_FUNCTION__, ss->sock_fd, source_str, errno,
	214	safe_strerror(errno));
	215	/* warning only */
	216	}
	217
71ad9915	218	listnode_delete(ss->pim->ssmpingd_list, ss);
d62a17ae	219	ssmpingd_free(ss);
12e41d03 DL	220	}
12e41d03 DL	221
d62a17ae	222	static void ssmpingd_sendto(struct ssmpingd_sock ss, const uint8_t buf,
d62a17ae	223	int len, struct sockaddr_in to)
12e41d03	224	{
d62a17ae	225	socklen_t tolen = sizeof(to);
	226	int sent;
	227
	228	sent = sendto(ss->sock_fd, buf, len, MSG_DONTWAIT,
	229	(struct sockaddr *)&to, tolen);
	230	if (sent != len) {
	231	char to_str[INET_ADDRSTRLEN];
	232	pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
	233	if (sent < 0) {
	234	zlog_warn(
	235	"%s: sendto() failure to %s,%d: fd=%d len=%d: errno=%d: %s",
	236	__PRETTY_FUNCTION__, to_str, ntohs(to.sin_port),
	237	ss->sock_fd, len, errno, safe_strerror(errno));
	238	} else {
	239	zlog_warn(
	240	"%s: sendto() partial to %s,%d: fd=%d len=%d: sent=%d",
	241	__PRETTY_FUNCTION__, to_str, ntohs(to.sin_port),
	242	ss->sock_fd, len, sent);
	243	}
	244	}
12e41d03 DL	245	}
	246
	247	static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
	248	{
d62a17ae	249	struct interface *ifp;
	250	struct sockaddr_in from;
	251	struct sockaddr_in to;
	252	socklen_t fromlen = sizeof(from);
	253	socklen_t tolen = sizeof(to);
	254	ifindex_t ifindex = -1;
	255	uint8_t buf[1000];
	256	int len;
	257
	258	++ss->requests;
	259
	260	len = pim_socket_recvfromto(ss->sock_fd, buf, sizeof(buf), &from,
	261	&fromlen, &to, &tolen, &ifindex);
	262	if (len < 0) {
	263	char source_str[INET_ADDRSTRLEN];
	264	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	265	sizeof(source_str));
	266	zlog_warn(
	267	"%s: failure receiving ssmping for source %s on fd=%d: errno=%d: %s",
	268	__PRETTY_FUNCTION__, source_str, ss->sock_fd, errno,
	269	safe_strerror(errno));
	270	return -1;
	271	}
	272
71ad9915	273	ifp = if_lookup_by_index(ifindex, ss->pim->vrf_id);
d62a17ae	274
	275	if (buf[0] != PIM_SSMPINGD_REQUEST) {
	276	char source_str[INET_ADDRSTRLEN];
	277	char from_str[INET_ADDRSTRLEN];
	278	char to_str[INET_ADDRSTRLEN];
	279	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	280	sizeof(source_str));
	281	pim_inet4_dump("<from?>", from.sin_addr, from_str,
	282	sizeof(from_str));
	283	pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
	284	zlog_warn(
	285	"%s: bad ssmping type=%d from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
	286	__PRETTY_FUNCTION__, buf[0], from_str,
	287	ntohs(from.sin_port), to_str, ntohs(to.sin_port),
	288	ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
	289	source_str);
	290	return 0;
	291	}
	292
	293	if (PIM_DEBUG_SSMPINGD) {
	294	char source_str[INET_ADDRSTRLEN];
	295	char from_str[INET_ADDRSTRLEN];
	296	char to_str[INET_ADDRSTRLEN];
	297	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	298	sizeof(source_str));
	299	pim_inet4_dump("<from?>", from.sin_addr, from_str,
	300	sizeof(from_str));
	301	pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
	302	zlog_debug(
	303	"%s: recv ssmping from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
	304	__PRETTY_FUNCTION__, from_str, ntohs(from.sin_port),
	305	to_str, ntohs(to.sin_port),
	306	ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
	307	source_str);
	308	}
	309
	310	buf[0] = PIM_SSMPINGD_REPLY;
	311
	312	/* unicast reply */
	313	ssmpingd_sendto(ss, buf, len, from);
	314
	315	/* multicast reply */
71ad9915	316	from.sin_addr = ss->pim->ssmpingd_group_addr;
d62a17ae	317	ssmpingd_sendto(ss, buf, len, from);
	318
	319	return 0;
12e41d03 DL	320	}
	321
	322	static int ssmpingd_sock_read(struct thread *t)
	323	{
d62a17ae	324	struct ssmpingd_sock *ss;
d62a17ae	325	int result;
12e41d03	326
d62a17ae	327	ss = THREAD_ARG(t);
12e41d03	328
d62a17ae	329	result = ssmpingd_read_msg(ss);
12e41d03	330
d62a17ae	331	/* Keep reading */
d62a17ae	332	ssmpingd_read_on(ss);
12e41d03	333
d62a17ae	334	return result;
12e41d03 DL	335	}
	336
	337	static void ssmpingd_read_on(struct ssmpingd_sock *ss)
	338	{
d62a17ae	339	thread_add_read(master, ssmpingd_sock_read, ss, ss->sock_fd,
d62a17ae	340	&ss->t_sock_read);
12e41d03 DL	341	}
12e41d03 DL	342
71ad9915 DS	343	static struct ssmpingd_sock ssmpingd_new(struct pim_instance pim,
71ad9915 DS	344	struct in_addr source_addr)
12e41d03	345	{
d62a17ae	346	struct ssmpingd_sock *ss;
	347	int sock_fd;
	348
71ad9915 DS	349	if (!pim->ssmpingd_list) {
	350	pim->ssmpingd_list = list_new();
	351	if (!pim->ssmpingd_list) {
d62a17ae	352	zlog_err(
	353	"%s %s: failure: qpim_ssmpingd_list=list_new()",
	354	__FILE__, __PRETTY_FUNCTION__);
	355	return 0;
	356	}
71ad9915	357	pim->ssmpingd_list->del = (void ()(void ))ssmpingd_free;
d62a17ae	358	}
	359
	360	sock_fd =
	361	ssmpingd_socket(source_addr, /* port: / 4321, / mTTL: */ 64);
	362	if (sock_fd < 0) {
	363	char source_str[INET_ADDRSTRLEN];
	364	pim_inet4_dump("<src?>", source_addr, source_str,
	365	sizeof(source_str));
	366	zlog_warn("%s: ssmpingd_socket() failure for source %s",
	367	__PRETTY_FUNCTION__, source_str);
	368	return 0;
	369	}
	370
	371	ss = XCALLOC(MTYPE_PIM_SSMPINGD, sizeof(*ss));
	372	if (!ss) {
	373	char source_str[INET_ADDRSTRLEN];
	374	pim_inet4_dump("<src?>", source_addr, source_str,
	375	sizeof(source_str));
	376	zlog_err("%s: XCALLOC(%zu) failure for ssmpingd source %s",
	377	__PRETTY_FUNCTION__, sizeof(*ss), source_str);
	378	close(sock_fd);
	379	return 0;
	380	}
	381
71ad9915	382	ss->pim = pim;
d62a17ae	383	ss->sock_fd = sock_fd;
	384	ss->t_sock_read = NULL;
	385	ss->source_addr = source_addr;
	386	ss->creation = pim_time_monotonic_sec();
	387	ss->requests = 0;
	388
71ad9915	389	listnode_add(pim->ssmpingd_list, ss);
d62a17ae	390
	391	ssmpingd_read_on(ss);
	392
	393	return ss;
12e41d03 DL	394	}
12e41d03 DL	395
71ad9915	396	int pim_ssmpingd_start(struct pim_instance *pim, struct in_addr source_addr)
12e41d03	397	{
d62a17ae	398	struct ssmpingd_sock *ss;
d62a17ae	399
71ad9915	400	ss = ssmpingd_find(pim, source_addr);
d62a17ae	401	if (ss) {
	402	/* silently ignore request to recreate entry */
	403	return 0;
	404	}
	405
	406	{
	407	char source_str[INET_ADDRSTRLEN];
	408	pim_inet4_dump("<src?>", source_addr, source_str,
	409	sizeof(source_str));
	410	zlog_info("%s: starting ssmpingd for source %s",
	411	__PRETTY_FUNCTION__, source_str);
	412	}
	413
71ad9915	414	ss = ssmpingd_new(pim, source_addr);
d62a17ae	415	if (!ss) {
	416	char source_str[INET_ADDRSTRLEN];
	417	pim_inet4_dump("<src?>", source_addr, source_str,
	418	sizeof(source_str));
	419	zlog_warn("%s: ssmpingd_new() failure for source %s",
	420	__PRETTY_FUNCTION__, source_str);
	421	return -1;
	422	}
	423
	424	return 0;
12e41d03 DL	425	}
12e41d03 DL	426
71ad9915	427	int pim_ssmpingd_stop(struct pim_instance *pim, struct in_addr source_addr)
12e41d03	428	{
d62a17ae	429	struct ssmpingd_sock *ss;
d62a17ae	430
71ad9915	431	ss = ssmpingd_find(pim, source_addr);
d62a17ae	432	if (!ss) {
	433	char source_str[INET_ADDRSTRLEN];
	434	pim_inet4_dump("<src?>", source_addr, source_str,
	435	sizeof(source_str));
	436	zlog_warn("%s: could not find ssmpingd for source %s",
	437	__PRETTY_FUNCTION__, source_str);
	438	return -1;
	439	}
	440
	441	{
	442	char source_str[INET_ADDRSTRLEN];
	443	pim_inet4_dump("<src?>", source_addr, source_str,
	444	sizeof(source_str));
	445	zlog_info("%s: stopping ssmpingd for source %s",
	446	__PRETTY_FUNCTION__, source_str);
	447	}
	448
	449	ssmpingd_delete(ss);
	450
	451	return 0;
12e41d03	452	}