Merge branch 'socket2' into new

5 files changed, 330 insertions, 30 deletions

diff --git a/sysdep/bsd/sysio.h b/sysdep/bsd/sysio.h
index 80f8f94..9ee4ade 100644
--- a/sysdep/bsd/sysio.h
+++ b/sysdep/bsd/sysio.h
@@ -11,21 +11,39 @@
 static inline void
 set_inaddr(struct in6_addr * ia, ip_addr a)
 {
-	ipa_hton(a);
-	memcpy(ia, &a, sizeof(a));
+  ipa_hton(a);
+  memcpy(ia, &a, sizeof(a));
+}
+
+static inline void
+get_inaddr(ip_addr *a, struct in6_addr *ia)
+{
+  memcpy(a, ia, sizeof(*a));
+  ipa_ntoh(*a);
 }
 
 #else
 
 #include <net/if.h>
+#include <net/if_dl.h>
 
 static inline void
 set_inaddr(struct in_addr * ia, ip_addr a)
 {
-	ipa_hton(a);
-	memcpy(&ia->s_addr, &a, sizeof(a));
+  ipa_hton(a);
+  memcpy(&ia->s_addr, &a, sizeof(a));
+}
+
+static inline void
+get_inaddr(ip_addr *a, struct in_addr *ia)
+{
+  memcpy(a, &ia->s_addr, sizeof(*a));
+  ipa_ntoh(*a);
 }
 
+
+/* BSD Multicast handling for IPv4 */
+
 static inline char *
 sysio_setup_multicast(sock *s)
 {
@@ -80,6 +98,90 @@ sysio_leave_group(sock *s, ip_addr maddr)
 	return NULL;
 }
 
+
+/* BSD RX/TX packet info handling for IPv4 */
+/* it uses IP_RECVDSTADDR / IP_RECVIF socket options instead of IP_PKTINFO */
+
+#define CMSG_RX_SPACE (CMSG_SPACE(sizeof(struct in_addr)) + CMSG_SPACE(sizeof(struct sockaddr_dl)))
+#define CMSG_TX_SPACE CMSG_SPACE(sizeof(struct in_addr))
+
+static char *
+sysio_register_cmsgs(sock *s)
+{
+  int ok = 1;
+  if (s->flags & SKF_LADDR_RX)
+    {
+      if (setsockopt(s->fd, IPPROTO_IP, IP_RECVDSTADDR, &ok, sizeof(ok)) < 0)
+	return "IP_RECVDSTADDR";
+
+      if (setsockopt(s->fd, IPPROTO_IP, IP_RECVIF, &ok, sizeof(ok)) < 0)
+	return "IP_RECVIF";
+    }
+
+  return NULL;
+}
+
+static void
+sysio_process_rx_cmsgs(sock *s, struct msghdr *msg)
+{
+  struct cmsghdr *cm;
+
+  if (!(s->flags & SKF_LADDR_RX))
+    return;
+
+  s->laddr = IPA_NONE;
+  s->lifindex = 0;
+
+  for (cm = CMSG_FIRSTHDR(msg); cm != NULL; cm = CMSG_NXTHDR(msg, cm))
+    {
+      if (cm->cmsg_level == IPPROTO_IP && cm->cmsg_type == IP_RECVDSTADDR)
+	{
+	  struct in_addr *ra = (struct in_addr *) CMSG_DATA(cm);
+	  get_inaddr(&s->laddr, ra);
+	}
+
+      if (cm->cmsg_level == IPPROTO_IP && cm->cmsg_type == IP_RECVIF)
+	{
+	  struct sockaddr_dl *ri = (struct sockaddr_dl *) CMSG_DATA(cm);
+	  s->lifindex = ri->sdl_index;
+	}
+    }
+
+  // log(L_WARN "RX %I %d", s->laddr, s->lifindex);
+}
+
+
+void
+sysio_prepare_tx_cmsgs(sock *s, struct msghdr *msg)
+{
+  struct cmsghdr *cm;
+  struct in_addr *sa;
+
+  if (!(s->flags & SKF_LADDR_TX))
+    {
+      msg->msg_controllen = 0;
+      return;
+    }
+
+  if (s->iface)
+    {
+      struct in_addr m;
+//      set_inaddr(&m, s->iface->addr->ip);
+      set_inaddr(&m, s->saddr);
+      setsockopt(s->fd, IPPROTO_IP, IP_MULTICAST_IF, &m, sizeof(m));
+    }
+
+  cm = CMSG_FIRSTHDR(msg);
+  cm->cmsg_level = IPPROTO_IP;
+  cm->cmsg_type = IP_SENDSRCADDR;
+  cm->cmsg_len = CMSG_LEN(sizeof(*sa));
+
+  sa = (struct in_addr *) CMSG_DATA(cm);
+  set_inaddr(sa, s->saddr);
+
+  msg->msg_controllen = cm->cmsg_len;
+}
+
 #endif
 
 
diff --git a/sysdep/cf/README b/sysdep/cf/README
index deed866..15a45a6 100644
--- a/sysdep/cf/README
+++ b/sysdep/cf/README
@@ -5,6 +5,7 @@ CONFIG_AUTO_ROUTES	Device routes are added automagically by the kernel
 CONFIG_SELF_CONSCIOUS	We're able to recognize whether route was installed by us
 CONFIG_MULTIPLE_TABLES	The kernel supports multiple routing tables
 CONFIG_ALL_TABLES_AT_ONCE	Kernel scanner wants to process all tables at once
+CONFIG_MC_PROPER_SRC	Multicast packets have source address according to socket saddr field
 
 CONFIG_UNIX_IFACE	Use Unix interface scanner
 CONFIG_UNIX_SET		Use Unix route setting
diff --git a/sysdep/cf/linux-22.h b/sysdep/cf/linux-22.h
index 92ffb4c..9ccab64 100644
--- a/sysdep/cf/linux-22.h
+++ b/sysdep/cf/linux-22.h
@@ -10,6 +10,7 @@
 #define CONFIG_SELF_CONSCIOUS
 #define CONFIG_MULTIPLE_TABLES
 #define CONFIG_ALL_TABLES_AT_ONCE
+#define CONFIG_MC_PROPER_SRC
 
 #undef CONFIG_SKIP_MC_BIND
 
diff --git a/sysdep/linux/sysio.h b/sysdep/linux/sysio.h
index 7fe3566..91cbdcd 100644
--- a/sysdep/linux/sysio.h
+++ b/sysdep/linux/sysio.h
@@ -21,6 +21,13 @@ set_inaddr(struct in6_addr *ia, ip_addr a)
   memcpy(ia, &a, sizeof(a));
 }
 
+static inline void
+get_inaddr(ip_addr *a, struct in6_addr *ia)
+{
+  memcpy(a, ia, sizeof(*a));
+  ipa_ntoh(*a);
+}
+
 #else
 
 #include <net/if.h>
@@ -32,6 +39,13 @@ set_inaddr(struct in_addr *ia, ip_addr a)
   memcpy(&ia->s_addr, &a, sizeof(a));
 }
 
+static inline void
+get_inaddr(ip_addr *a, struct in_addr *ia)
+{
+  memcpy(a, &ia->s_addr, sizeof(*a));
+  ipa_ntoh(*a);
+}
+
 /*
  *  Multicasting in Linux systems is a real mess. Not only different kernels
  *  have different interfaces, but also different libc's export it in different
@@ -48,18 +62,18 @@ set_inaddr(struct in_addr *ia, ip_addr a)
 
 #define MREQ_IFA struct in_addr
 #define MREQ_GRP struct ip_mreq
-static inline void fill_mreq_ifa(struct in_addr *m, struct iface *ifa, UNUSED ip_addr maddr)
+static inline void fill_mreq_ifa(struct in_addr *m, struct iface *ifa UNUSED, ip_addr saddr, ip_addr maddr UNUSED)
 {
-  set_inaddr(m, ifa->addr->ip);
+  set_inaddr(m, saddr);
 }
 
-static inline void fill_mreq_grp(struct ip_mreq *m, struct iface *ifa, ip_addr maddr)
+static inline void fill_mreq_grp(struct ip_mreq *m, struct iface *ifa, ip_addr saddr, ip_addr maddr)
 {
   bzero(m, sizeof(*m));
 #ifdef CONFIG_LINUX_MC_MREQ_BIND
   m->imr_interface.s_addr = INADDR_ANY;
 #else
-  set_inaddr(&m->imr_interface, ifa->addr->ip);
+  set_inaddr(&m->imr_interface, saddr);
 #endif
   set_inaddr(&m->imr_multiaddr, maddr);
 }
@@ -87,11 +101,11 @@ struct ip_mreqn
 #define fill_mreq_ifa fill_mreq
 #define fill_mreq_grp fill_mreq
 
-static inline void fill_mreq(struct ip_mreqn *m, struct iface *ifa, ip_addr maddr)
+static inline void fill_mreq(struct ip_mreqn *m, struct iface *ifa, ip_addr saddr, ip_addr maddr)
 {
   bzero(m, sizeof(*m));
   m->imr_ifindex = ifa->index;
-  set_inaddr(&m->imr_address, ifa->addr->ip);
+  set_inaddr(&m->imr_address, saddr);
   set_inaddr(&m->imr_multiaddr, maddr);
 }
 #endif
@@ -109,7 +123,7 @@ sysio_setup_multicast(sock *s)
     return "IP_MULTICAST_TTL";
 
   /* This defines where should we send _outgoing_ multicasts */
-  fill_mreq_ifa(&m, s->iface, IPA_NONE);
+  fill_mreq_ifa(&m, s->iface, s->saddr, IPA_NONE);
   if (setsockopt(s->fd, SOL_IP, IP_MULTICAST_IF, &m, sizeof(m)) < 0)
     return "IP_MULTICAST_IF";
 
@@ -131,7 +145,7 @@ sysio_join_group(sock *s, ip_addr maddr)
   MREQ_GRP m;
 
   /* And this one sets interface for _receiving_ multicasts from */
-  fill_mreq_grp(&m, s->iface, maddr);
+  fill_mreq_grp(&m, s->iface, s->saddr, maddr);
   if (setsockopt(s->fd, SOL_IP, IP_ADD_MEMBERSHIP, &m, sizeof(m)) < 0)
     return "IP_ADD_MEMBERSHIP";
 
@@ -144,7 +158,7 @@ sysio_leave_group(sock *s, ip_addr maddr)
   MREQ_GRP m;
 
   /* And this one sets interface for _receiving_ multicasts from */
-  fill_mreq_grp(&m, s->iface, maddr);
+  fill_mreq_grp(&m, s->iface, s->saddr, maddr);
   if (setsockopt(s->fd, SOL_IP, IP_DROP_MEMBERSHIP, &m, sizeof(m)) < 0)
     return "IP_DROP_MEMBERSHIP";
 
@@ -209,3 +223,76 @@ sk_set_md5_auth_int(sock *s, sockaddr *sa, char *passwd)
 
   return rv;
 }
+
+
+#ifndef IPV6
+
+/* RX/TX packet info handling for IPv4 */
+/* Mostly similar to standardized IPv6 code */
+
+#define CMSG_RX_SPACE CMSG_SPACE(sizeof(struct in_pktinfo))
+#define CMSG_TX_SPACE CMSG_SPACE(sizeof(struct in_pktinfo))
+
+static char *
+sysio_register_cmsgs(sock *s)
+{
+  int ok = 1;
+  if ((s->flags & SKF_LADDR_RX) &&
+      setsockopt(s->fd, IPPROTO_IP, IP_PKTINFO, &ok, sizeof(ok)) < 0)
+    return "IP_PKTINFO";
+
+  return NULL;
+}
+
+static void
+sysio_process_rx_cmsgs(sock *s, struct msghdr *msg)
+{
+  struct cmsghdr *cm;
+  struct in_pktinfo *pi = NULL;
+
+  if (!(s->flags & SKF_LADDR_RX))
+    return;
+
+  for (cm = CMSG_FIRSTHDR(msg); cm != NULL; cm = CMSG_NXTHDR(msg, cm))
+    {
+      if (cm->cmsg_level == IPPROTO_IP && cm->cmsg_type == IP_PKTINFO)
+	pi = (struct in_pktinfo *) CMSG_DATA(cm);
+    }
+
+  if (!pi)
+    {
+      s->laddr = IPA_NONE;
+      s->lifindex = 0;
+      return;
+    }
+
+  get_inaddr(&s->laddr, &pi->ipi_addr);
+  s->lifindex = pi->ipi_ifindex;
+  return;
+}
+
+
+void
+sysio_prepare_tx_cmsgs(sock *s, struct msghdr *msg)
+{
+  struct cmsghdr *cm;
+  struct in_pktinfo *pi;
+
+  if (!(s->flags & SKF_LADDR_TX))
+    {
+      msg->msg_controllen = 0;
+      return;
+    }
+
+  cm = CMSG_FIRSTHDR(msg);
+  cm->cmsg_level = IPPROTO_IP;
+  cm->cmsg_type = IP_PKTINFO;
+  cm->cmsg_len = CMSG_LEN(sizeof(*pi));
+
+  pi = (struct in_pktinfo *) CMSG_DATA(cm);
+  set_inaddr(&pi->ipi_spec_dst, s->saddr);
+  pi->ipi_ifindex = s->iface ? s->iface->index : 0;
+
+  msg->msg_controllen = cm->cmsg_len;
+}
+#endif
diff --git a/sysdep/unix/io.c b/sysdep/unix/io.c
index d86c8cb..e9d78b6 100644
--- a/sysdep/unix/io.c
+++ b/sysdep/unix/io.c
@@ -685,7 +685,7 @@ static char *
 sk_setup(sock *s)
 {
   int fd = s->fd;
-  char *err;
+  char *err = NULL;
 
   if (fcntl(fd, F_SETFL, O_NONBLOCK) < 0)
     ERR("fcntl(O_NONBLOCK)");
@@ -704,9 +704,8 @@ sk_setup(sock *s)
 
   if (s->ttl >= 0)
     err = sk_set_ttl_int(s);
-  else
-    err = NULL;
 
+  sysio_register_cmsgs(s);
 bad:
   return err;
 }
@@ -857,6 +856,72 @@ sk_leave_group(sock *s, ip_addr maddr)
   return 0;
 }
 
+/* PKTINFO handling is also standardized in IPv6 */
+#define CMSG_RX_SPACE CMSG_SPACE(sizeof(struct in6_pktinfo))
+
+static char *
+sysio_register_cmsgs(sock *s)
+{
+  int ok = 1;
+  if ((s->flags & SKF_LADDR_RX) &&
+      setsockopt(s->fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &ok, sizeof(ok)) < 0)
+    return "IPV6_RECVPKTINFO";
+
+  return NULL;
+}
+
+void
+sysio_process_rx_cmsgs(sock *s, struct msghdr *msg)
+{
+  struct cmsghdr *cm;
+  struct in6_pktinfo *pi = NULL;
+
+  if (!(s->flags & SKF_LADDR_RX))
+    return;
+
+  for (cm = CMSG_FIRSTHDR(msg); cm != NULL; cm = CMSG_NXTHDR(msg, cm))
+    {
+      if (cm->cmsg_level == IPPROTO_IPV6 && cm->cmsg_type == IPV6_PKTINFO)
+	pi = (struct in6_pktinfo *) CMSG_DATA(cm);
+    }
+
+  if (!pi)
+    {
+      s->laddr = IPA_NONE;
+      s->lifindex = 0;
+      return;
+    }
+
+  get_inaddr(&s->laddr, &pi->ipi6_addr);
+  s->lifindex = pi->ipi6_ifindex;
+  return;
+}
+
+void
+sysio_prepare_tx_cmsgs(sock *s, struct msghdr *msg)
+{
+  struct cmsghdr *cm;
+  struct in6_pktinfo *pi;
+
+  if (!(s->flags & SKF_LADDR_TX))
+    {
+      msg->msg_controllen = 0;
+      return;
+    }
+
+  cm = CMSG_FIRSTHDR(msg);
+  cm->cmsg_level = IPPROTO_IPV6;
+  cm->cmsg_type = PIV6_PKTINFO;
+  cm->cmsg_len = CMSG_LEN(sizeof(*pi));
+
+  pi = (struct in6_pktinfo *) CMSG_DATA(cm);
+  set_inaddr(&pi->ipi6_addr, s->saddr);
+  pi->ipi6_ifindex = s->iface ? s->iface->index : 0;
+
+  msg->msg_controllen = cmsg->cmsg_len;
+  return;
+}
+
 #else /* IPV4 */
 
 int
@@ -1105,6 +1170,8 @@ sk_open_unix(sock *s, char *name)
   die("Unable to create control socket %s", name);
 }
 
+static inline void reset_tx_buffer(sock *s) { s->ttx = s->tpos = s->tbuf; }
+
 static int
 sk_maybe_write(sock *s)
 {
@@ -1122,7 +1189,7 @@ sk_maybe_write(sock *s)
 	    {
 	      if (errno != EINTR && errno != EAGAIN)
 		{
-                  s->ttx = s->tpos;	/* empty tx buffer */
+		  reset_tx_buffer(s);
 		  s->err_hook(s, errno);
 		  return -1;
 		}
@@ -1130,30 +1197,44 @@ sk_maybe_write(sock *s)
 	    }
 	  s->ttx += e;
 	}
-      s->ttx = s->tpos = s->tbuf;
+      reset_tx_buffer(s);
       return 1;
     case SK_UDP:
     case SK_IP:
       {
-	sockaddr sa;
-
 	if (s->tbuf == s->tpos)
 	  return 1;
 
-	fill_in_sockaddr(&sa, s->faddr, s->fport);
+	sockaddr sa;
+	fill_in_sockaddr(&sa, s->daddr, s->dport);
 	fill_in_sockifa(&sa, s->iface);
-	e = sendto(s->fd, s->tbuf, s->tpos - s->tbuf, 0, (struct sockaddr *) &sa, sizeof(sa));
+
+	struct iovec iov = {s->tbuf, s->tpos - s->tbuf};
+	byte cmsg_buf[CMSG_TX_SPACE];
+
+	struct msghdr msg = {
+	  .msg_name = &sa,
+	  .msg_namelen = sizeof(sa),
+	  .msg_iov = &iov,
+	  .msg_iovlen = 1,
+	  .msg_control = cmsg_buf,
+	  .msg_controllen = sizeof(cmsg_buf),
+	  .msg_flags = 0};
+
+	sysio_prepare_tx_cmsgs(s, &msg);
+	e = sendmsg(s->fd, &msg, 0);
+
 	if (e < 0)
 	  {
 	    if (errno != EINTR && errno != EAGAIN)
 	      {
-                s->ttx = s->tpos;	/* empty tx buffer */
+		reset_tx_buffer(s);
 		s->err_hook(s, errno);
 		return -1;
 	      }
 	    return 0;
 	  }
-	s->tpos = s->tbuf;
+	reset_tx_buffer(s);
 	return 1;
       }
     default:
@@ -1199,8 +1280,6 @@ sk_rx_ready(sock *s)
 int
 sk_send(sock *s, unsigned len)
 {
-  s->faddr = s->daddr;
-  s->fport = s->dport;
   s->ttx = s->tbuf;
   s->tpos = s->tbuf + len;
   return sk_maybe_write(s);
@@ -1219,13 +1298,28 @@ sk_send(sock *s, unsigned len)
 int
 sk_send_to(sock *s, unsigned len, ip_addr addr, unsigned port)
 {
-  s->faddr = addr;
-  s->fport = port;
+  s->daddr = addr;
+  s->dport = port;
   s->ttx = s->tbuf;
   s->tpos = s->tbuf + len;
   return sk_maybe_write(s);
 }
 
+/*
+int
+sk_send_full(sock *s, unsigned len, struct iface *ifa,
+	     ip_addr saddr, ip_addr daddr, unsigned dport)
+{
+  s->iface = ifa;
+  s->saddr = saddr;
+  s->daddr = daddr;
+  s->dport = dport;
+  s->ttx = s->tbuf;
+  s->tpos = s->tbuf + len;
+  return sk_maybe_write(s);
+}
+*/
+
 static int
 sk_read(sock *s)
 {
@@ -1271,8 +1365,21 @@ sk_read(sock *s)
     default:
       {
 	sockaddr sa;
-	int al = sizeof(sa);
-	int e = recvfrom(s->fd, s->rbuf, s->rbsize, 0, (struct sockaddr *) &sa, &al);
+	int e;
+
+	struct iovec iov = {s->rbuf, s->rbsize};
+	byte cmsg_buf[CMSG_RX_SPACE];
+
+	struct msghdr msg = {
+	  .msg_name = &sa,
+	  .msg_namelen = sizeof(sa),
+	  .msg_iov = &iov,
+	  .msg_iovlen = 1,
+	  .msg_control = cmsg_buf,
+	  .msg_controllen = sizeof(cmsg_buf),
+	  .msg_flags = 0};
+
+	e = recvmsg(s->fd, &msg, 0);
 
 	if (e < 0)
 	  {
@@ -1282,6 +1389,8 @@ sk_read(sock *s)
 	  }
 	s->rpos = s->rbuf + e;
 	get_sockaddr(&sa, &s->faddr, &s->fport, 1);
+	sysio_process_rx_cmsgs(s, &msg);
+
 	s->rx_hook(s, e);
 	return 1;
       }