[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()
{
  int result;

  zassert(!qpim_ssmpingd_list);

  result = inet_pton(AF_INET, PIM_SSMPINGD_REPLY_GROUP, &qpim_ssmpingd_group_addr);
  
  zassert(result > 0);
}

void pim_ssmpingd_destroy()
{
  if (qpim_ssmpingd_list) {
    list_free(qpim_ssmpingd_list);
    qpim_ssmpingd_list = 0;
  }
}

static struct ssmpingd_sock *ssmpingd_find(struct in_addr source_addr)
{
  struct listnode      *node;
  struct ssmpingd_sock *ss;

  if (!qpim_ssmpingd_list)
    return 0;

  for (ALL_LIST_ELEMENTS_RO(qpim_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);
  zassert(qpim_ssmpingd_list);

  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(qpim_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, VRF_DEFAULT);

  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 = qpim_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 in_addr source_addr)
{
  struct ssmpingd_sock *ss;
  int sock_fd;

  if (!qpim_ssmpingd_list) {
    qpim_ssmpingd_list = list_new();
    if (!qpim_ssmpingd_list) {
      zlog_err("%s %s: failure: qpim_ssmpingd_list=list_new()",
	       __FILE__, __PRETTY_FUNCTION__);
      return 0;
    }
    qpim_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->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(qpim_ssmpingd_list, ss);

  ssmpingd_read_on(ss);

  return ss;
}

int pim_ssmpingd_start(struct in_addr source_addr)
{
  struct ssmpingd_sock *ss;

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

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