[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 "lib_errors.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;

	assert(!pim->ssmpingd_list);

	result = inet_pton(AF_INET, 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,
					   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) {
		flog_err_sys(EC_LIB_SOCKET,
			     "%s: could not create socket: errno=%d: %s",
			     __func__, 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",
			__func__, 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",
				__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
	}

	{
		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",
				__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;
	}

	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;
	}

	{
		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",
				__func__, 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",
				__func__, fd, errno, safe_strerror(errno));
			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)) {
		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",
			__func__, 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",
				__func__, 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",
				__func__, 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",
			__func__, source_str, 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) {
		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",
			__func__, 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",
			__func__, 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(router->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();
		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",
			  __func__, source_str);
		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, 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", __func__,
			  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", __func__,
			  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", __func__,
			  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", __func__,
			  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"
3613d898	27	#include "lib_errors.h"
12e41d03	28
8bfb8b67	29	#include "pimd.h"
12e41d03 DL	30	#include "pim_ssmpingd.h"
	31	#include "pim_time.h"
	32	#include "pim_sock.h"
12e41d03	33
d62a17ae	34	static const char *const PIM_SSMPINGD_REPLY_GROUP = "232.43.211.234";
12e41d03	35
d62a17ae	36	enum { PIM_SSMPINGD_REQUEST = 'Q', PIM_SSMPINGD_REPLY = 'A' };
12e41d03 DL	37
	38	static void ssmpingd_read_on(struct ssmpingd_sock *ss);
	39
71ad9915	40	void pim_ssmpingd_init(struct pim_instance *pim)
12e41d03	41	{
d62a17ae	42	int result;
12e41d03	43
df5dfb77	44	assert(!pim->ssmpingd_list);
12e41d03	45
d62a17ae	46	result = inet_pton(AF_INET, PIM_SSMPINGD_REPLY_GROUP,
71ad9915	47	&pim->ssmpingd_group_addr);
d62a17ae	48
df5dfb77	49	assert(result > 0);
12e41d03 DL	50	}
12e41d03 DL	51
71ad9915	52	void pim_ssmpingd_destroy(struct pim_instance *pim)
12e41d03	53	{
affe9e99	54	if (pim->ssmpingd_list)
6a154c88	55	list_delete(&pim->ssmpingd_list);
12e41d03 DL	56	}
12e41d03 DL	57
71ad9915 DS	58	static struct ssmpingd_sock ssmpingd_find(struct pim_instance pim,
71ad9915 DS	59	struct in_addr source_addr)
12e41d03	60	{
d62a17ae	61	struct listnode *node;
d62a17ae	62	struct ssmpingd_sock *ss;
12e41d03	63
71ad9915	64	if (!pim->ssmpingd_list)
d62a17ae	65	return 0;
12e41d03	66
71ad9915	67	for (ALL_LIST_ELEMENTS_RO(pim->ssmpingd_list, node, ss))
d62a17ae	68	if (source_addr.s_addr == ss->source_addr.s_addr)
d62a17ae	69	return ss;
12e41d03	70
d62a17ae	71	return 0;
12e41d03 DL	72	}
	73
	74	static void ssmpingd_free(struct ssmpingd_sock *ss)
	75	{
d62a17ae	76	XFREE(MTYPE_PIM_SSMPINGD, ss);
12e41d03 DL	77	}
	78
	79	static int ssmpingd_socket(struct in_addr addr, int port, int mttl)
	80	{
d62a17ae	81	struct sockaddr_in sockaddr;
	82	int fd;
	83
	84	fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	85	if (fd < 0) {
450971aa	86	flog_err_sys(EC_LIB_SOCKET,
09c866e3	87	"%s: could not create socket: errno=%d: %s",
15569c58	88	__func__, errno, safe_strerror(errno));
d62a17ae	89	return -1;
	90	}
	91
	92	sockaddr.sin_family = AF_INET;
	93	sockaddr.sin_addr = addr;
	94	sockaddr.sin_port = htons(port);
	95
	96	if (bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr))) {
	97	char addr_str[INET_ADDRSTRLEN];
	98	pim_inet4_dump("<addr?>", addr, addr_str, sizeof(addr_str));
	99	zlog_warn(
	100	"%s: bind(fd=%d,addr=%s,port=%d,len=%zu) failure: errno=%d: %s",
15569c58 DA	101	__func__, fd, addr_str, port, sizeof(sockaddr), errno,
15569c58 DA	102	safe_strerror(errno));
d62a17ae	103	close(fd);
	104	return -1;
	105	}
	106
	107	/* Needed to obtain destination address from recvmsg() */
	108	{
12e41d03	109	#if defined(HAVE_IP_PKTINFO)
d62a17ae	110	/* Linux and Solaris IP_PKTINFO */
	111	int opt = 1;
	112	if (setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &opt, sizeof(opt))) {
	113	zlog_warn(
	114	"%s: could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
15569c58	115	__func__, fd, errno, safe_strerror(errno));
d62a17ae	116	}
12e41d03	117	#elif defined(HAVE_IP_RECVDSTADDR)
d62a17ae	118	/* BSD IP_RECVDSTADDR */
	119	int opt = 1;
	120	if (setsockopt(fd, IPPROTO_IP, IP_RECVDSTADDR, &opt,
	121	sizeof(opt))) {
	122	zlog_warn(
	123	"%s: could not set IP_RECVDSTADDR on socket fd=%d: errno=%d: %s",
15569c58	124	__func__, fd, errno, safe_strerror(errno));
d62a17ae	125	}
12e41d03	126	#else
af4c2728	127	flog_err(
450971aa	128	EC_LIB_DEVELOPMENT,
d62a17ae	129	"%s %s: missing IP_PKTINFO and IP_RECVDSTADDR: unable to get dst addr from recvmsg()",
15569c58	130	__FILE__, __func__);
d62a17ae	131	close(fd);
d62a17ae	132	return -1;
12e41d03	133	#endif
d62a17ae	134	}
	135
	136	{
	137	int reuse = 1;
	138	if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (void *)&reuse,
	139	sizeof(reuse))) {
	140	zlog_warn(
	141	"%s: could not set Reuse Address Option on socket fd=%d: errno=%d: %s",
15569c58	142	__func__, fd, errno, safe_strerror(errno));
d62a17ae	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",
15569c58	152	__func__, mttl, fd, errno, safe_strerror(errno));
d62a17ae	153	close(fd);
	154	return -1;
	155	}
	156
	157	if (setsockopt_ipv4_multicast_loop(fd, 0)) {
	158	zlog_warn(
	159	"%s: could not disable Multicast Loopback Option on socket fd=%d: errno=%d: %s",
15569c58	160	__func__, fd, errno, safe_strerror(errno));
d62a17ae	161	close(fd);
	162	return PIM_SOCK_ERR_LOOP;
	163	}
	164
	165	if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, (void *)&addr,
	166	sizeof(addr))) {
	167	zlog_warn(
	168	"%s: could not set Outgoing Interface Option on socket fd=%d: errno=%d: %s",
15569c58	169	__func__, fd, errno, safe_strerror(errno));
d62a17ae	170	close(fd);
	171	return -1;
	172	}
	173
	174	{
	175	long flags;
	176
	177	flags = fcntl(fd, F_GETFL, 0);
	178	if (flags < 0) {
	179	zlog_warn(
	180	"%s: could not get fcntl(F_GETFL,O_NONBLOCK) on socket fd=%d: errno=%d: %s",
15569c58	181	__func__, fd, errno, safe_strerror(errno));
d62a17ae	182	close(fd);
	183	return -1;
	184	}
	185
	186	if (fcntl(fd, F_SETFL, flags \| O_NONBLOCK)) {
	187	zlog_warn(
	188	"%s: could not set fcntl(F_SETFL,O_NONBLOCK) on socket fd=%d: errno=%d: %s",
15569c58	189	__func__, fd, errno, safe_strerror(errno));
d62a17ae	190	close(fd);
	191	return -1;
	192	}
	193	}
	194
	195	return fd;
12e41d03 DL	196	}
	197
	198	static void ssmpingd_delete(struct ssmpingd_sock *ss)
	199	{
df5dfb77	200	assert(ss);
d62a17ae	201
	202	THREAD_OFF(ss->t_sock_read);
	203
	204	if (close(ss->sock_fd)) {
	205	char source_str[INET_ADDRSTRLEN];
	206	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	207	sizeof(source_str));
	208	zlog_warn(
	209	"%s: failure closing ssmpingd sock_fd=%d for source %s: errno=%d: %s",
15569c58	210	__func__, ss->sock_fd, source_str, errno,
d62a17ae	211	safe_strerror(errno));
	212	/* warning only */
	213	}
	214
71ad9915	215	listnode_delete(ss->pim->ssmpingd_list, ss);
d62a17ae	216	ssmpingd_free(ss);
12e41d03 DL	217	}
12e41d03 DL	218
d62a17ae	219	static void ssmpingd_sendto(struct ssmpingd_sock ss, const uint8_t buf,
d62a17ae	220	int len, struct sockaddr_in to)
12e41d03	221	{
d62a17ae	222	socklen_t tolen = sizeof(to);
	223	int sent;
	224
	225	sent = sendto(ss->sock_fd, buf, len, MSG_DONTWAIT,
	226	(struct sockaddr *)&to, tolen);
	227	if (sent != len) {
	228	char to_str[INET_ADDRSTRLEN];
	229	pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
	230	if (sent < 0) {
	231	zlog_warn(
	232	"%s: sendto() failure to %s,%d: fd=%d len=%d: errno=%d: %s",
15569c58	233	__func__, to_str, ntohs(to.sin_port),
d62a17ae	234	ss->sock_fd, len, errno, safe_strerror(errno));
	235	} else {
	236	zlog_warn(
	237	"%s: sendto() partial to %s,%d: fd=%d len=%d: sent=%d",
15569c58	238	__func__, to_str, ntohs(to.sin_port),
d62a17ae	239	ss->sock_fd, len, sent);
	240	}
	241	}
12e41d03 DL	242	}
	243
	244	static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
	245	{
d62a17ae	246	struct interface *ifp;
	247	struct sockaddr_in from;
	248	struct sockaddr_in to;
	249	socklen_t fromlen = sizeof(from);
	250	socklen_t tolen = sizeof(to);
	251	ifindex_t ifindex = -1;
	252	uint8_t buf[1000];
	253	int len;
	254
	255	++ss->requests;
	256
	257	len = pim_socket_recvfromto(ss->sock_fd, buf, sizeof(buf), &from,
	258	&fromlen, &to, &tolen, &ifindex);
	259	if (len < 0) {
	260	char source_str[INET_ADDRSTRLEN];
	261	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	262	sizeof(source_str));
	263	zlog_warn(
	264	"%s: failure receiving ssmping for source %s on fd=%d: errno=%d: %s",
15569c58	265	__func__, source_str, ss->sock_fd, errno,
d62a17ae	266	safe_strerror(errno));
	267	return -1;
	268	}
	269
d3cc1e45	270	ifp = if_lookup_by_index(ifindex, ss->pim->vrf->vrf_id);
d62a17ae	271
	272	if (buf[0] != PIM_SSMPINGD_REQUEST) {
	273	char source_str[INET_ADDRSTRLEN];
	274	char from_str[INET_ADDRSTRLEN];
	275	char to_str[INET_ADDRSTRLEN];
	276	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	277	sizeof(source_str));
	278	pim_inet4_dump("<from?>", from.sin_addr, from_str,
	279	sizeof(from_str));
	280	pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
	281	zlog_warn(
	282	"%s: bad ssmping type=%d from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
15569c58 DA	283	__func__, buf[0], from_str, ntohs(from.sin_port),
15569c58 DA	284	to_str, ntohs(to.sin_port),
d62a17ae	285	ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
	286	source_str);
	287	return 0;
	288	}
	289
	290	if (PIM_DEBUG_SSMPINGD) {
	291	char source_str[INET_ADDRSTRLEN];
	292	char from_str[INET_ADDRSTRLEN];
	293	char to_str[INET_ADDRSTRLEN];
	294	pim_inet4_dump("<src?>", ss->source_addr, source_str,
	295	sizeof(source_str));
	296	pim_inet4_dump("<from?>", from.sin_addr, from_str,
	297	sizeof(from_str));
	298	pim_inet4_dump("<to?>", to.sin_addr, to_str, sizeof(to_str));
	299	zlog_debug(
	300	"%s: recv ssmping from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
15569c58 DA	301	__func__, from_str, ntohs(from.sin_port), to_str,
	302	ntohs(to.sin_port), ifp ? ifp->name : "<iface?>",
	303	ifindex, ss->sock_fd, source_str);
d62a17ae	304	}
	305
	306	buf[0] = PIM_SSMPINGD_REPLY;
	307
	308	/* unicast reply */
	309	ssmpingd_sendto(ss, buf, len, from);
	310
	311	/* multicast reply */
71ad9915	312	from.sin_addr = ss->pim->ssmpingd_group_addr;
d62a17ae	313	ssmpingd_sendto(ss, buf, len, from);
	314
	315	return 0;
12e41d03 DL	316	}
	317
	318	static int ssmpingd_sock_read(struct thread *t)
	319	{
d62a17ae	320	struct ssmpingd_sock *ss;
d62a17ae	321	int result;
12e41d03	322
d62a17ae	323	ss = THREAD_ARG(t);
12e41d03	324
d62a17ae	325	result = ssmpingd_read_msg(ss);
12e41d03	326
d62a17ae	327	/* Keep reading */
d62a17ae	328	ssmpingd_read_on(ss);
12e41d03	329
d62a17ae	330	return result;
12e41d03 DL	331	}
	332
	333	static void ssmpingd_read_on(struct ssmpingd_sock *ss)
	334	{
36417fcc	335	thread_add_read(router->master, ssmpingd_sock_read, ss, ss->sock_fd,
d62a17ae	336	&ss->t_sock_read);
12e41d03 DL	337	}
12e41d03 DL	338
71ad9915 DS	339	static struct ssmpingd_sock ssmpingd_new(struct pim_instance pim,
71ad9915 DS	340	struct in_addr source_addr)
12e41d03	341	{
d62a17ae	342	struct ssmpingd_sock *ss;
	343	int sock_fd;
	344
71ad9915 DS	345	if (!pim->ssmpingd_list) {
71ad9915 DS	346	pim->ssmpingd_list = list_new();
71ad9915	347	pim->ssmpingd_list->del = (void ()(void ))ssmpingd_free;
d62a17ae	348	}
	349
	350	sock_fd =
	351	ssmpingd_socket(source_addr, /* port: / 4321, / mTTL: */ 64);
	352	if (sock_fd < 0) {
	353	char source_str[INET_ADDRSTRLEN];
	354	pim_inet4_dump("<src?>", source_addr, source_str,
	355	sizeof(source_str));
	356	zlog_warn("%s: ssmpingd_socket() failure for source %s",
15569c58	357	__func__, source_str);
d62a17ae	358	return 0;
	359	}
	360
	361	ss = XCALLOC(MTYPE_PIM_SSMPINGD, sizeof(*ss));
d62a17ae	362
71ad9915	363	ss->pim = pim;
d62a17ae	364	ss->sock_fd = sock_fd;
	365	ss->t_sock_read = NULL;
	366	ss->source_addr = source_addr;
	367	ss->creation = pim_time_monotonic_sec();
	368	ss->requests = 0;
	369
71ad9915	370	listnode_add(pim->ssmpingd_list, ss);
d62a17ae	371
	372	ssmpingd_read_on(ss);
	373
	374	return ss;
12e41d03 DL	375	}
12e41d03 DL	376
71ad9915	377	int pim_ssmpingd_start(struct pim_instance *pim, struct in_addr source_addr)
12e41d03	378	{
d62a17ae	379	struct ssmpingd_sock *ss;
d62a17ae	380
71ad9915	381	ss = ssmpingd_find(pim, source_addr);
d62a17ae	382	if (ss) {
	383	/* silently ignore request to recreate entry */
	384	return 0;
	385	}
	386
	387	{
	388	char source_str[INET_ADDRSTRLEN];
	389	pim_inet4_dump("<src?>", source_addr, source_str,
	390	sizeof(source_str));
15569c58 DA	391	zlog_info("%s: starting ssmpingd for source %s", __func__,
15569c58 DA	392	source_str);
d62a17ae	393	}
d62a17ae	394
71ad9915	395	ss = ssmpingd_new(pim, source_addr);
d62a17ae	396	if (!ss) {
	397	char source_str[INET_ADDRSTRLEN];
	398	pim_inet4_dump("<src?>", source_addr, source_str,
	399	sizeof(source_str));
15569c58 DA	400	zlog_warn("%s: ssmpingd_new() failure for source %s", __func__,
15569c58 DA	401	source_str);
d62a17ae	402	return -1;
	403	}
	404
	405	return 0;
12e41d03 DL	406	}
12e41d03 DL	407
71ad9915	408	int pim_ssmpingd_stop(struct pim_instance *pim, struct in_addr source_addr)
12e41d03	409	{
d62a17ae	410	struct ssmpingd_sock *ss;
d62a17ae	411
71ad9915	412	ss = ssmpingd_find(pim, source_addr);
d62a17ae	413	if (!ss) {
	414	char source_str[INET_ADDRSTRLEN];
	415	pim_inet4_dump("<src?>", source_addr, source_str,
	416	sizeof(source_str));
15569c58 DA	417	zlog_warn("%s: could not find ssmpingd for source %s", __func__,
15569c58 DA	418	source_str);
d62a17ae	419	return -1;
	420	}
	421
	422	{
	423	char source_str[INET_ADDRSTRLEN];
	424	pim_inet4_dump("<src?>", source_addr, source_str,
	425	sizeof(source_str));
15569c58 DA	426	zlog_info("%s: stopping ssmpingd for source %s", __func__,
15569c58 DA	427	source_str);
d62a17ae	428	}
	429
	430	ssmpingd_delete(ss);
	431
	432	return 0;
12e41d03	433	}