[pim] T41 DONE ssmping support

7 files changed, 195 insertions, 13 deletions

diff --git a/pimd/TODO b/pimd/TODO
index dc2ece5f..0818d9c1 100644
--- a/pimd/TODO
+++ b/pimd/TODO
@@ -354,7 +354,16 @@ T40 Lightweight MLDv2
     http://www.ietf.org/internet-drafts/draft-ietf-mboned-lightweight-igmpv3-mldv2-05.txt
     http://www.ietf.org/html.charters/mboned-charter.html
 
-T41 ssmping
-    http://www.venaas.no/multicast/ssmping/
+T41 DONE ssmping support
+    See http://www.venaas.no/multicast/ssmping/
+
+    Example:
+
+    debug ssmpingd
+    
+    conf t
+     ip ssmpingd 1.1.1.1
+    
+    show ip ssmpingd
 
 -x-
diff --git a/pimd/pim_cmd.c b/pimd/pim_cmd.c
index a4073f50..eed74927 100644
--- a/pimd/pim_cmd.c
+++ b/pimd/pim_cmd.c
@@ -2143,7 +2143,7 @@ static void show_ssmpingd(struct vty *vty)
   struct ssmpingd_sock *ss;
   time_t                now;
 
-  vty_out(vty, "Source          Socket Uptime  Requests%s",
+  vty_out(vty, "Source          Socket Address          Port Uptime   Requests%s",
 	  VTY_NEWLINE);
 
   if (!qpim_ssmpingd_list)
@@ -2154,13 +2154,25 @@ static void show_ssmpingd(struct vty *vty)
   for (ALL_LIST_ELEMENTS_RO(qpim_ssmpingd_list, node, ss)) {
     char source_str[100];
     char ss_uptime[10];
+    struct sockaddr_in bind_addr;
+    int len = sizeof(bind_addr);
+    char bind_addr_str[100];
 
     pim_inet4_dump("<src?>", ss->source_addr, source_str, sizeof(source_str));
+
+    if (pim_socket_getsockname(ss->sock_fd, (struct sockaddr *) &bind_addr, &len)) {
+      vty_out(vty, "%% Failure reading socket name for ssmpingd source %s on fd=%d%s",
+	      source_str, ss->sock_fd, VTY_NEWLINE);
+    }
+
+    pim_inet4_dump("<addr?>", bind_addr.sin_addr, bind_addr_str, sizeof(bind_addr_str));
     pim_time_uptime(ss_uptime, sizeof(ss_uptime), now - ss->creation);
 
-    vty_out(vty, "%-15s %6d %8s %8lld%s",
+    vty_out(vty, "%-15s %6d %-15s %5d %8s %8lld%s",
 	    source_str,
 	    ss->sock_fd,
+	    bind_addr_str,
+	    ntohs(bind_addr.sin_port),
 	    ss_uptime,
 	    ss->requests,
 	    VTY_NEWLINE);
@@ -4004,6 +4016,7 @@ void pim_cmd_init()
   install_element (ENABLE_NODE, &show_ip_mroute_cmd);
   install_element (ENABLE_NODE, &show_ip_mroute_count_cmd);
   install_element (ENABLE_NODE, &show_ip_route_cmd);
+  install_element (ENABLE_NODE, &show_ip_ssmpingd_cmd);
   install_element (ENABLE_NODE, &show_debugging_cmd);
 
   install_element (ENABLE_NODE, &test_igmp_receive_report_cmd);
diff --git a/pimd/pim_sock.c b/pimd/pim_sock.c
index c908e8b8..2e786054 100644
--- a/pimd/pim_sock.c
+++ b/pimd/pim_sock.c
@@ -260,6 +260,29 @@ int pim_socket_recvfromto(int fd, char *buf, size_t len,
   char cbuf[1000];
   int err;
 
+  /*
+   * IP_PKTINFO / IP_RECVDSTADDR don't yield sin_port.
+   * Use getsockname() to get sin_port.
+   */
+  if (to) {
+    struct sockaddr_in si;
+    socklen_t si_len = sizeof(si);
+    
+    ((struct sockaddr_in *) to)->sin_family = AF_INET;
+
+    if (pim_socket_getsockname(fd, (struct sockaddr *) &si, &si_len)) {
+      ((struct sockaddr_in *) to)->sin_port        = ntohs(0);
+      ((struct sockaddr_in *) to)->sin_addr.s_addr = ntohl(0);
+    }
+    else {
+      ((struct sockaddr_in *) to)->sin_port = si.sin_port;
+      ((struct sockaddr_in *) to)->sin_addr = si.sin_addr;
+    }
+
+    if (tolen) 
+      *tolen = sizeof(si);
+  }
+
   memset(&msgh, 0, sizeof(struct msghdr));
   iov.iov_base = buf;
   iov.iov_len  = len;
@@ -291,6 +314,15 @@ int pim_socket_recvfromto(int fd, char *buf, size_t len,
 	*tolen = sizeof(struct sockaddr_in);
       if (ifindex)
 	*ifindex = i->ipi_ifindex;
+
+      if (to && PIM_DEBUG_PACKETS) {
+	char to_str[100];
+	pim_inet4_dump("<to?>", to->sin_addr, to_str, sizeof(to_str));
+	zlog_debug("%s: HAVE_IP_PKTINFO to=%s,%d",
+		   __PRETTY_FUNCTION__,
+		   to_str, ntohs(to->sin_port));
+      }
+
       break;
     }
 #endif
@@ -302,6 +334,15 @@ int pim_socket_recvfromto(int fd, char *buf, size_t len,
 	((struct sockaddr_in *) to)->sin_addr = *i;
       if (tolen)
 	*tolen = sizeof(struct sockaddr_in);
+
+      if (to && PIM_DEBUG_PACKETS) {
+	char to_str[100];
+	pim_inet4_dump("<to?>", to->sin_addr, to_str, sizeof(to_str));
+	zlog_debug("%s: HAVE_IP_RECVDSTADDR to=%s,%d",
+		   __PRETTY_FUNCTION__,
+		   to_str, ntohs(to->sin_port));
+      }
+
       break;
     }
 #endif
@@ -333,3 +374,16 @@ int pim_socket_mcastloop_get(int fd)
   
   return loop;
 }
+
+int pim_socket_getsockname(int fd, struct sockaddr *name, int *namelen)
+{
+  if (getsockname(fd, name, namelen)) {
+    int e = errno;
+    zlog_warn("Could not get Socket Name for socket fd=%d: errno=%d: %s",
+	      fd, errno, safe_strerror(errno));
+    errno = e;
+    return PIM_SOCK_ERR_NAME;
+  }
+
+  return PIM_SOCK_ERR_NONE;
+}
diff --git a/pimd/pim_sock.h b/pimd/pim_sock.h
index d3557809..3f026dcd 100644
--- a/pimd/pim_sock.h
+++ b/pimd/pim_sock.h
@@ -35,6 +35,7 @@
 #define PIM_SOCK_ERR_DSTADDR (-7) /* Outgoing interface option */
 #define PIM_SOCK_ERR_NONBLOCK_GETFL (-8) /* Get O_NONBLOCK */
 #define PIM_SOCK_ERR_NONBLOCK_SETFL (-9) /* Set O_NONBLOCK */
+#define PIM_SOCK_ERR_NAME    (-10) /* Socket name (getsockname) */
 
 int pim_socket_raw(int protocol);
 int pim_socket_mcast(int protocol, struct in_addr ifaddr, int loop);
@@ -51,4 +52,6 @@ int pim_socket_recvfromto(int fd, char *buf, size_t len,
 
 int pim_socket_mcastloop_get(int fd);
 
+int pim_socket_getsockname(int fd, struct sockaddr *name, int *namelen);
+
 #endif /* PIM_SOCK_H */
diff --git a/pimd/pim_ssmpingd.c b/pimd/pim_ssmpingd.c
index 4eb5c8dc..6422cf62 100644
--- a/pimd/pim_ssmpingd.c
+++ b/pimd/pim_ssmpingd.c
@@ -32,11 +32,24 @@
 #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()
@@ -67,8 +80,9 @@ static void ssmpingd_free(struct ssmpingd_sock *ss)
   XFREE(MTYPE_PIM_SSMPINGD, ss);
 }
 
-static int ssmpingd_socket(struct in_addr addr, int mttl)
+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);
@@ -78,21 +92,36 @@ static int ssmpingd_socket(struct in_addr addr, int mttl)
     return -1;
   }
 
+  sockaddr.sin_family = AF_INET;
+  sockaddr.sin_addr   = addr;
+  sockaddr.sin_port   = htons(port);
+
+  if (bind(fd, &sockaddr, sizeof(sockaddr))) {
+    char addr_str[100];
+    pim_inet4_dump("<addr?>", addr, addr_str, sizeof(addr_str));
+    zlog_warn("%s: bind(fd=%d,addr=%s,port=%d,len=%d) 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 IP_PKTINFO */
     int opt = 1;
     if (setsockopt(fd, SOL_IP, IP_PKTINFO, &opt, sizeof(opt))) {
-      zlog_warn("Could not set IP_PKTINFO on socket fd=%d: errno=%d: %s",
-		fd, errno, safe_strerror(errno));
+      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("Could not set IP_RECVDSTADDR on socket fd=%d: errno=%d: %s",
-		fd, errno, safe_strerror(errno));
+      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()",
@@ -121,6 +150,19 @@ static int ssmpingd_socket(struct in_addr addr, int mttl)
     return -1;
   }
 
+  {
+    int loop = 0;
+    if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP,
+		   (void *) &loop, sizeof(loop))) {
+      zlog_warn("%s: could not %s Multicast Loopback Option on socket fd=%d: errno=%d: %s",
+		__PRETTY_FUNCTION__,
+		loop ? "enable" : "disable",
+		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",
@@ -172,6 +214,34 @@ static void ssmpingd_delete(struct ssmpingd_sock *ss)
   ssmpingd_free(ss);
 }
 
+static void ssmpingd_sendto(struct ssmpingd_sock *ss,
+			    const char *buf,
+			    int len,
+			    struct sockaddr_in to)
+{
+  socklen_t tolen = sizeof(to);
+  int sent;
+
+  sent = sendto(ss->sock_fd, buf, len, MSG_DONTWAIT, &to, tolen);
+  if (sent != len) {
+    int e = errno;
+    char to_str[100];
+    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,
+		e, safe_strerror(e));
+    }
+    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;
@@ -183,6 +253,8 @@ static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
   char buf[1000];
   int len;
 
+  ++ss->requests;
+
   len = pim_socket_recvfromto(ss->sock_fd, buf, sizeof(buf),
 			      &from, &fromlen,
 			      &to, &tolen,
@@ -197,6 +269,24 @@ static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
 
   ifp = if_lookup_by_index(ifindex);
 
+  if (buf[0] != PIM_SSMPINGD_REQUEST) {
+    char source_str[100];
+    char from_str[100];
+    char to_str[100];
+    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[100];
     char from_str[100];
@@ -204,13 +294,24 @@ static int ssmpingd_read_msg(struct ssmpingd_sock *ss)
     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: ssmpingd on source %s: interface %s ifindex=%d received ssmping from %s to %s on fd=%d",
+    zlog_debug("%s: recv ssmping from %s,%d to %s,%d on interface %s ifindex=%d fd=%d src=%s",
 	       __PRETTY_FUNCTION__,
-	       source_str,
+	       from_str, ntohs(from.sin_port),
+	       to_str, ntohs(to.sin_port),
 	       ifp ? ifp->name : "<iface?>",
-	       ifindex, from_str, to_str, ss->sock_fd);
+	       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;
 }
 
@@ -259,7 +360,7 @@ static struct ssmpingd_sock *ssmpingd_new(struct in_addr source_addr)
     qpim_ssmpingd_list->del = (void (*)(void *)) ssmpingd_free;
   }
 
-  sock_fd = ssmpingd_socket(source_addr, 64 /* ttl */);
+  sock_fd = ssmpingd_socket(source_addr, /* port: */ 4321, /* mTTL: */ 64);
   if (sock_fd < 0) {
     char source_str[100];
     pim_inet4_dump("<src?>", source_addr, source_str, sizeof(source_str));
diff --git a/pimd/pimd.c b/pimd/pimd.c
index 220604d9..71b74e97 100644
--- a/pimd/pimd.c
+++ b/pimd/pimd.c
@@ -60,6 +60,7 @@ int64_t                   qpim_rpf_cache_refresh_events = 0;
 int64_t                   qpim_rpf_cache_refresh_last =  0;
 struct in_addr            qpim_inaddr_any;
 struct list              *qpim_ssmpingd_list = 0;
+struct in_addr            qpim_ssmpingd_group_addr;
 
 static void pim_free()
 {
diff --git a/pimd/pimd.h b/pimd/pimd.h
index a2dc6430..b42c54cf 100644
--- a/pimd/pimd.h
+++ b/pimd/pimd.h
@@ -86,6 +86,7 @@ int64_t                   qpim_rpf_cache_refresh_events;
 int64_t                   qpim_rpf_cache_refresh_last;
 struct in_addr            qpim_inaddr_any;
 struct list              *qpim_ssmpingd_list; /* list of struct ssmpingd_sock */
+struct in_addr            qpim_ssmpingd_group_addr;
 
 #define PIM_JP_HOLDTIME (qpim_t_periodic * 7 / 2)