[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 "pim_ssmpingd.h"
#include "pim_time.h"
#include "pim_sock.h"
#include "pim_str.h"
#include "pimd.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);

  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 sock_fd;
  int result;

  ss = THREAD_ARG(t);

  sock_fd = THREAD_FD(t);
  zassert(sock_fd == ss->sock_fd);

  result = ssmpingd_read_msg(ss);

  /* Keep reading */
  ss->t_sock_read = 0;
  ssmpingd_read_on(ss);

  return result;
}

static void ssmpingd_read_on(struct ssmpingd_sock *ss)
{
  zassert(!ss->t_sock_read);
  THREAD_READ_ON(master, ss->t_sock_read,
		 ssmpingd_sock_read, ss, ss->sock_fd);
}

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