[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_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 %pI4 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=%pI4",
			__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=%pI4",
			__func__, &from, &to, ifp ? ifp->name : "<iface?>",
			ifindex, ss->sock_fd, &ss->source_addr);
	}

	buf[0] = PIM_SSMPINGD_REPLY;

	struct sockaddr_in *from_addr = (struct sockaddr_in *)&from;

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

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

	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,
					  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;
023d3e4a BG	247	struct sockaddr_storage from;
023d3e4a BG	248	struct sockaddr_storage to;
d62a17ae	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);
023d3e4a	259
d62a17ae	260	if (len < 0) {
d62a17ae	261	zlog_warn(
023d3e4a BG	262	"%s: failure receiving ssmping for source %pI4 on fd=%d: errno=%d: %s",
023d3e4a BG	263	__func__, &ss->source_addr, ss->sock_fd, errno,
d62a17ae	264	safe_strerror(errno));
	265	return -1;
	266	}
	267
d3cc1e45	268	ifp = if_lookup_by_index(ifindex, ss->pim->vrf->vrf_id);
d62a17ae	269
d62a17ae	270	if (buf[0] != PIM_SSMPINGD_REQUEST) {
d62a17ae	271	zlog_warn(
023d3e4a BG	272	"%s: bad ssmping type=%d from %pSUp to %pSUp on interface %s ifindex=%d fd=%d src=%pI4",
023d3e4a BG	273	__func__, buf[0], &from, &to,
d62a17ae	274	ifp ? ifp->name : "<iface?>", ifindex, ss->sock_fd,
023d3e4a	275	&ss->source_addr);
d62a17ae	276	return 0;
	277	}
	278
	279	if (PIM_DEBUG_SSMPINGD) {
d62a17ae	280	zlog_debug(
023d3e4a BG	281	"%s: recv ssmping from %pSUp, to %pSUp, on interface %s ifindex=%d fd=%d src=%pI4",
	282	__func__, &from, &to, ifp ? ifp->name : "<iface?>",
	283	ifindex, ss->sock_fd, &ss->source_addr);
d62a17ae	284	}
	285
	286	buf[0] = PIM_SSMPINGD_REPLY;
	287
023d3e4a BG	288	struct sockaddr_in from_addr = (struct sockaddr_in )&from;
023d3e4a BG	289
d62a17ae	290	/* unicast reply */
023d3e4a	291	ssmpingd_sendto(ss, buf, len, *from_addr);
d62a17ae	292
d62a17ae	293	/* multicast reply */
023d3e4a BG	294	from_addr->sin_addr = ss->pim->ssmpingd_group_addr;
023d3e4a BG	295	ssmpingd_sendto(ss, buf, len, *from_addr);
d62a17ae	296
d62a17ae	297	return 0;
12e41d03 DL	298	}
12e41d03 DL	299
cc9f21da	300	static void ssmpingd_sock_read(struct thread *t)
12e41d03	301	{
d62a17ae	302	struct ssmpingd_sock *ss;
12e41d03	303
d62a17ae	304	ss = THREAD_ARG(t);
12e41d03	305
cc9f21da	306	ssmpingd_read_msg(ss);
12e41d03	307
d62a17ae	308	/* Keep reading */
d62a17ae	309	ssmpingd_read_on(ss);
12e41d03 DL	310	}
	311
	312	static void ssmpingd_read_on(struct ssmpingd_sock *ss)
	313	{
36417fcc	314	thread_add_read(router->master, ssmpingd_sock_read, ss, ss->sock_fd,
d62a17ae	315	&ss->t_sock_read);
12e41d03 DL	316	}
12e41d03 DL	317
71ad9915 DS	318	static struct ssmpingd_sock ssmpingd_new(struct pim_instance pim,
71ad9915 DS	319	struct in_addr source_addr)
12e41d03	320	{
d62a17ae	321	struct ssmpingd_sock *ss;
	322	int sock_fd;
	323
71ad9915 DS	324	if (!pim->ssmpingd_list) {
71ad9915 DS	325	pim->ssmpingd_list = list_new();
71ad9915	326	pim->ssmpingd_list->del = (void ()(void ))ssmpingd_free;
d62a17ae	327	}
	328
	329	sock_fd =
	330	ssmpingd_socket(source_addr, /* port: / 4321, / mTTL: */ 64);
	331	if (sock_fd < 0) {
	332	char source_str[INET_ADDRSTRLEN];
	333	pim_inet4_dump("<src?>", source_addr, source_str,
	334	sizeof(source_str));
	335	zlog_warn("%s: ssmpingd_socket() failure for source %s",
15569c58	336	__func__, source_str);
d62a17ae	337	return 0;
	338	}
	339
	340	ss = XCALLOC(MTYPE_PIM_SSMPINGD, sizeof(*ss));
d62a17ae	341
71ad9915	342	ss->pim = pim;
d62a17ae	343	ss->sock_fd = sock_fd;
	344	ss->t_sock_read = NULL;
	345	ss->source_addr = source_addr;
	346	ss->creation = pim_time_monotonic_sec();
	347	ss->requests = 0;
	348
71ad9915	349	listnode_add(pim->ssmpingd_list, ss);
d62a17ae	350
	351	ssmpingd_read_on(ss);
	352
	353	return ss;
12e41d03 DL	354	}
12e41d03 DL	355
71ad9915	356	int pim_ssmpingd_start(struct pim_instance *pim, struct in_addr source_addr)
12e41d03	357	{
d62a17ae	358	struct ssmpingd_sock *ss;
d62a17ae	359
71ad9915	360	ss = ssmpingd_find(pim, source_addr);
d62a17ae	361	if (ss) {
	362	/* silently ignore request to recreate entry */
	363	return 0;
	364	}
	365
	366	{
	367	char source_str[INET_ADDRSTRLEN];
	368	pim_inet4_dump("<src?>", source_addr, source_str,
	369	sizeof(source_str));
15569c58 DA	370	zlog_info("%s: starting ssmpingd for source %s", __func__,
15569c58 DA	371	source_str);
d62a17ae	372	}
d62a17ae	373
71ad9915	374	ss = ssmpingd_new(pim, source_addr);
d62a17ae	375	if (!ss) {
	376	char source_str[INET_ADDRSTRLEN];
	377	pim_inet4_dump("<src?>", source_addr, source_str,
	378	sizeof(source_str));
15569c58 DA	379	zlog_warn("%s: ssmpingd_new() failure for source %s", __func__,
15569c58 DA	380	source_str);
d62a17ae	381	return -1;
	382	}
	383
	384	return 0;
12e41d03 DL	385	}
12e41d03 DL	386
71ad9915	387	int pim_ssmpingd_stop(struct pim_instance *pim, struct in_addr source_addr)
12e41d03	388	{
d62a17ae	389	struct ssmpingd_sock *ss;
d62a17ae	390
71ad9915	391	ss = ssmpingd_find(pim, source_addr);
d62a17ae	392	if (!ss) {
	393	char source_str[INET_ADDRSTRLEN];
	394	pim_inet4_dump("<src?>", source_addr, source_str,
	395	sizeof(source_str));
15569c58 DA	396	zlog_warn("%s: could not find ssmpingd for source %s", __func__,
15569c58 DA	397	source_str);
d62a17ae	398	return -1;
	399	}
	400
	401	{
	402	char source_str[INET_ADDRSTRLEN];
	403	pim_inet4_dump("<src?>", source_addr, source_str,
	404	sizeof(source_str));
15569c58 DA	405	zlog_info("%s: stopping ssmpingd for source %s", __func__,
15569c58 DA	406	source_str);
d62a17ae	407	}
	408
	409	ssmpingd_delete(ss);
	410
	411	return 0;
12e41d03	412	}