From cc7f68045e5f3cfc6c932996af784ab319951426 Mon Sep 17 00:00:00 2001
From: Vladimir Oltean <vladimir.oltean@nxp.com>
Date: Tue, 19 Apr 2022 13:29:20 +0300
Subject: [PATCH 3/6] mreceive: support IPv6

Extend the mreceive program with a generalization of sockets,
addresses and socket options that covers both IPv4 and IPv6.

Most of the lower-level implementation is moved to common.c and exported
through common.h such that it can be reused by msend at a later time.

The makefile rule to link object files into executables is updated to
look at all specified objects rather than just the first, by using $^
instead of $<. Otherwise, common.o would be ignored when linking
mreceive.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 Makefile   |   8 +-
 common.c   | 261 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 common.h   |  36 ++++++++
 mreceive.c | 142 ++++++++++-------------------
 4 files changed, 349 insertions(+), 98 deletions(-)
 create mode 100644 common.c
 create mode 100644 common.h

--- a/Makefile
+++ b/Makefile
@@ -20,8 +20,8 @@ mandir      = $(prefix)/share/man/man8
 # ttcp is currently not part of the distribution because its not tested
 # yet.  Please test and let me know at GitHub so I can include it! :)
 EXEC       := msend mreceive
-OBJS       := $(EXEC:=.o)
-DEPS       := $(EXEC:=.d)
+OBJS       := msend.o mreceive.o common.o
+DEPS       := msend.d mreceive.d common.d
 MANS        = $(addsuffix .8,$(EXEC))
 DISTFILES   = README.md LICENSE.md
 
@@ -33,10 +33,10 @@ all: $(EXEC)
 
 .o:
 	@printf "  LINK    $@\n"
-	@$(CC) $(CFLAGS) $(LDFLAGS) -Wl,-Map,$@.map -o $@ $< $(LDLIBS$(LDLIBS-$(@)))
+	@$(CC) $(CFLAGS) $(LDFLAGS) -Wl,-Map,$@.map -o $@ $^ $(LDLIBS$(LDLIBS-$(@)))
 
 msend:    msend.o
-mreceive: mreceive.o
+mreceive: mreceive.o common.o
 ttcp:     ttcp.o
 
 install: $(EXEC)
--- /dev/null
+++ b/common.c
@@ -0,0 +1,261 @@
+/*
+ * common.c -- Common functions for mreceive.c and msend.c
+ */
+#include <arpa/inet.h>
+#include <net/if.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "common.h"
+
+int ip_address_parse(const char *string, struct ip_address *ip)
+{
+	int ret;
+
+	ret = inet_pton(AF_INET6, string, &ip->addr6);
+	if (ret > 0) {
+		ip->family = AF_INET6;
+	} else {
+		ret = inet_pton(AF_INET, string, &ip->addr);
+		if (ret > 0) {
+			ip->family = AF_INET;
+		} else {
+			fprintf(stderr, "IP address %s not in known format\n",
+			        string);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+int socket_create(struct sock *s, int family, int port)
+{
+	struct sockaddr *serv_addr;
+	int sockopt = 1;
+	int fd, ret;
+
+	memset(s, 0, sizeof(*s));
+
+	if (family == AF_INET) {
+		serv_addr = (struct sockaddr *)&s->udp4;
+		s->udp4.sin_addr.s_addr = htonl(INADDR_ANY);
+		s->udp4.sin_port = htons(port);
+		s->udp4.sin_family = AF_INET;
+		s->addr_size = sizeof(struct sockaddr_in);
+	} else {
+		serv_addr = (struct sockaddr *)&s->udp6;
+		s->udp6.sin6_addr = in6addr_any;
+		s->udp6.sin6_port = htons(port);
+		s->udp6.sin6_family = AF_INET6;
+		s->addr_size = sizeof(struct sockaddr_in6);
+	}
+
+	fd = socket(family, SOCK_DGRAM, 0);
+	if (fd < 0) {
+		perror("socket");
+		return fd;
+	}
+
+	/* avoid EADDRINUSE error on bind() */
+	ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof(int));
+	if (ret) {
+		perror("setsockopt() SO_REUSEADDR");
+		close(fd);
+		return ret;
+	}
+
+	ret = bind(fd, serv_addr, s->addr_size);
+	if (ret) {
+		perror("bind");
+		close(fd);
+		return ret;
+	}
+
+	s->fd = fd;
+
+	return 0;
+}
+
+static int igmp_join_by_saddr(struct sock *s, const struct ip_address *mc,
+			      struct ip_address *saddr)
+{
+	struct ip_mreq mreq = {};
+	int fd = s->fd;
+	int off = 0;
+	int ret;
+
+	memcpy(&mreq.imr_multiaddr, &mc->addr, sizeof(struct in_addr));
+	memcpy(&mreq.imr_interface.s_addr, &saddr->addr,
+	       sizeof(struct in_addr));
+
+	ret = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
+	if (ret) {
+		perror("setsockopt() IP_ADD_MEMBERSHIP");
+		return -1;
+	}
+
+	ret = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &off, sizeof(int));
+	if (ret) {
+		perror("setsockopt() IP_MULTICAST_LOOP");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int igmp_join_by_if_name(struct sock *s, const struct ip_address *mc,
+				const char *if_name)
+{
+	struct ip_mreqn mreq = {};
+	int fd = s->fd;
+	int if_index;
+	int off = 0;
+	int ret;
+
+	if_index = if_nametoindex(if_name);
+	if (!if_index) {
+		perror("if_nametoindex");
+		return -1;
+	}
+
+	memcpy(&mreq.imr_multiaddr, &mc->addr, sizeof(struct in_addr));
+	mreq.imr_ifindex = if_index;
+
+	ret = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
+	if (ret) {
+		perror("setsockopt() IP_ADD_MEMBERSHIP");
+		return -1;
+	}
+
+	ret = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &off, sizeof(int));
+	if (ret) {
+		perror("setsockopt() IP_MULTICAST_LOOP");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int mld_join(struct sock *s, const struct ip_address *mc,
+		    const char *if_name)
+{
+	struct ipv6_mreq mreq = {};
+	int if_index, off = 0;
+	int fd = s->fd;
+	int ret;
+
+	if_index = if_nametoindex(if_name);
+	if (!if_index) {
+		perror("if_nametoindex");
+		return -1;
+	}
+
+	memcpy(&mreq.ipv6mr_multiaddr, &mc->addr6, sizeof(struct in6_addr));
+	mreq.ipv6mr_interface = if_index;
+	ret = setsockopt(fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq,
+			 sizeof(mreq));
+	if (ret) {
+		perror("setsockopt IPV6_ADD_MEMBERSHIP");
+		return -1;
+	}
+
+	ret = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &off,
+			 sizeof(int));
+	if (ret) {
+		perror("setsockopt IPV6_MULTICAST_LOOP");
+		return -1;
+	}
+
+	return 0;
+}
+
+int mc_join(struct sock *s, const struct ip_address *mc, const char *if_name,
+	    int num_saddrs, struct ip_address *saddrs)
+{
+	int i, ret;
+
+	if (if_name) {
+		switch (mc->family) {
+		case AF_INET:
+			return igmp_join_by_if_name(s, mc, if_name);
+		case AF_INET6:
+			return mld_join(s, mc, if_name);
+		default:
+			return -1;
+		}
+	}
+
+	if (!num_saddrs) {		/* single interface */
+		struct ip_address saddr = {
+			.family = AF_INET,
+			.addr.s_addr = INADDR_ANY,
+		};
+
+		return igmp_join_by_saddr(s, mc, &saddr);
+	}
+
+	for (i = 0; i < num_saddrs; i++) {
+		ret = igmp_join_by_saddr(s, mc, &saddrs[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int igmp_set_ttl(int fd, int ttl)
+{
+	int ret;
+
+	ret = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(int));
+	if (ret)
+		perror("setsockopt() IP_MULTICAST_TTL");
+
+	return ret;
+}
+
+static int mld_set_hop_limit(int fd, int limit)
+{
+	int ret;
+
+	ret = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &limit,
+			 sizeof(int));
+	if (ret)
+		perror("setsockopt() IPV6_MULTICAST_HOPS");
+
+	return ret;
+}
+
+int mc_set_hop_limit(struct sock *s, int limit)
+{
+	switch (s->addr_size) {
+	case sizeof(struct sockaddr_in):
+		return igmp_set_ttl(s->fd, limit);
+	case sizeof(struct sockaddr_in6):
+		return mld_set_hop_limit(s->fd, limit);
+	default:
+		return -1;
+	}
+}
+
+int mc_recv(struct sock *s, void *buf, size_t len, struct sock *from)
+{
+	from->addr_size = sizeof(struct sockaddr_in6);
+
+	return recvfrom(s->fd, buf, len, 0, (struct sockaddr *)&(from->udp6),
+			&from->addr_size);
+}
+
+int socket_get_port(const struct sock *s)
+{
+	switch (s->addr_size) {
+	case sizeof(struct sockaddr_in):
+		return ntohs(s->udp4.sin_port);
+	case sizeof(struct sockaddr_in6):
+		return ntohs(s->udp6.sin6_port);
+	default:
+		return 0;
+	}
+}
--- /dev/null
+++ b/common.h
@@ -0,0 +1,36 @@
+/*
+ * common.h -- Common header for mreceive.c and msend.c
+ */
+#ifndef _COMMON_H
+#define _COMMON_H
+
+#include <netinet/in.h>
+#include <netinet/ip.h>
+#include <netinet/ip6.h>
+
+struct ip_address {
+	int family;
+	union {
+		struct in_addr addr;
+		struct in6_addr addr6;
+	};
+};
+
+struct sock {
+	socklen_t addr_size;
+	union {
+		struct sockaddr_in udp4;
+		struct sockaddr_in6 udp6;
+	};
+	int fd;
+};
+
+int ip_address_parse(const char *string, struct ip_address *ip);
+int socket_create(struct sock *s, int family, int port);
+int mc_join(struct sock *s, const struct ip_address *mc, const char *if_name,
+	    int num_saddrs, struct ip_address *saddrs);
+int mc_set_hop_limit(struct sock *s, int limit);
+int mc_recv(struct sock *s, void *buf, size_t len, struct sock *from);
+int socket_get_port(const struct sock *s);
+
+#endif
--- a/mreceive.c
+++ b/mreceive.c
@@ -28,6 +28,8 @@
 #include <arpa/inet.h>
 #include <sys/time.h>
 
+#include "common.h"
+
 #define TRUE 1
 #define FALSE 0
 #ifndef INVALID_SOCKET
@@ -43,7 +45,7 @@
 
 char *TEST_ADDR = "224.1.1.1";
 int TEST_PORT = 4444;
-unsigned long IP[MAXIP];
+struct ip_address IP[MAXIP];
 int NUM = 0;
 
 void printHelp(void)
@@ -62,52 +64,12 @@ Usage: mreceive [-g GROUP] [-p PORT] [-i
   -h           Print the command usage.\n\n", VERSION);
 }
 
-static void igmp_join_by_saddr(int s, in_addr_t multiaddr, in_addr_t interface)
-{
-	struct ip_mreq mreq;
-	int ret;
-
-	mreq.imr_multiaddr.s_addr = multiaddr;
-	mreq.imr_interface.s_addr = interface;
-
-	ret = setsockopt(s, IPPROTO_IP, IP_ADD_MEMBERSHIP,
-			 (char *)&mreq, sizeof(mreq));
-	if (ret == SOCKET_ERROR) {
-		printf("setsockopt() IP_ADD_MEMBERSHIP failed.\n");
-		exit(1);
-	}
-}
-
-static void igmp_join_by_if_name(int s, in_addr_t multicast,
-				 const char *if_name)
-{
-	struct ip_mreqn mreq = {};
-	int if_index;
-	int ret;
-
-	if_index = if_nametoindex(if_name);
-	if (!if_index) {
-		perror("if_nametoindex");
-		exit(1);
-	}
-
-	mreq.imr_multiaddr.s_addr = multicast;
-	mreq.imr_ifindex = if_index;
-
-	ret = setsockopt(s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
-	if (ret) {
-		perror("setsockopt() IP_ADD_MEMBERSHIP");
-		exit(1);
-	}
-}
-
 int main(int argc, char *argv[])
 {
-	struct sockaddr_in stLocal, stFrom;
 	unsigned char achIn[BUFSIZE];
-	const char *if_name;
-	int s, i;
-	int iTmp, iRet;
+	const char *if_name = NULL;
+	struct ip_address mc;
+	struct sock s, from;
 	int ipnum = 0;
 	int ii;
 	unsigned int numreceived;
@@ -116,6 +78,8 @@ int main(int argc, char *argv[])
 	int starttime;
 	int curtime;
 	struct timeval tv;
+	int ret;
+	int i;
 
 /*
   if( argc < 2 ) {
@@ -152,7 +116,10 @@ int main(int argc, char *argv[])
 		} else if (strcmp(argv[ii], "-i") == 0) {
 			ii++;
 			if ((ii < argc) && !(strchr(argv[ii], '-'))) {
-				IP[ipnum] = inet_addr(argv[ii]);
+				ret = ip_address_parse(argv[ii], &IP[ipnum]);
+				if (ret)
+					exit(1);
+
 				ii++;
 				ipnum++;
 			}
@@ -177,73 +144,59 @@ int main(int argc, char *argv[])
 		}
 	}
 
-	/* get a datagram socket */
-	s = socket(AF_INET, SOCK_DGRAM, 0);
-	if (s == INVALID_SOCKET) {
-		printf("socket() failed.\n");
+	ret = ip_address_parse(TEST_ADDR, &mc);
+	if (ret)
 		exit(1);
-	}
 
-	/* avoid EADDRINUSE error on bind() */
-	iTmp = TRUE;
-	iRet = setsockopt(s, SOL_SOCKET, SO_REUSEADDR, (char *)&iTmp, sizeof(iTmp));
-	if (iRet == SOCKET_ERROR) {
-		printf("setsockopt() SO_REUSEADDR failed.\n");
+	if (mc.family == AF_INET6 && ipnum) {
+		printf("Joining IPv6 groups by source address not supported, use -I\n");
 		exit(1);
 	}
 
-	/* name the socket */
-	stLocal.sin_family = AF_INET;
-	stLocal.sin_addr.s_addr = htonl(INADDR_ANY);
-	stLocal.sin_port = htons(TEST_PORT);
-	iRet = bind(s, (struct sockaddr *)&stLocal, sizeof(stLocal));
-	if (iRet == SOCKET_ERROR) {
-		printf("bind() failed.\n");
+	if (mc.family == AF_INET6 && !if_name) {
+		printf("-I is mandatory with IPv6\n");
 		exit(1);
 	}
 
-	/* join the multicast group. */
-	if (if_name) {
-		igmp_join_by_if_name(s, inet_addr(TEST_ADDR), if_name);
-	} else {
-		if (!ipnum) {		/* single interface */
-			igmp_join_by_saddr(s, inet_addr(TEST_ADDR), INADDR_ANY);
-		} else {
-			for (i = 0; i < ipnum; i++) {
-				igmp_join_by_saddr(s, inet_addr(TEST_ADDR),
-						   IP[i]);
-			}
-		}
-	}
+	/* get a datagram socket */
+	ret = socket_create(&s, mc.family, TEST_PORT);
+	if (ret)
+		exit(1);
 
-	/* set TTL to traverse up to multiple routers */
-	iTmp = TTL_VALUE;
-	iRet = setsockopt(s, IPPROTO_IP, IP_MULTICAST_TTL, (char *)&iTmp, sizeof(iTmp));
-	if (iRet == SOCKET_ERROR) {
-		printf("setsockopt() IP_MULTICAST_TTL failed.\n");
+	/* join the multicast group. */
+	ret = mc_join(&s, &mc, if_name, ipnum, IP);
+	if (ret)
 		exit(1);
-	}
 
-	/* disable loopback */
-	/* iTmp = TRUE; */
-	iTmp = FALSE;
-	iRet = setsockopt(s, IPPROTO_IP, IP_MULTICAST_LOOP, (char *)&iTmp, sizeof(iTmp));
-	if (iRet == SOCKET_ERROR) {
-		printf("setsockopt() IP_MULTICAST_LOOP failed.\n");
+	/* set TTL to traverse up to multiple routers */
+	ret = mc_set_hop_limit(&s, TTL_VALUE);
+	if (ret)
 		exit(1);
-	}
 
 	printf("Now receiving from multicast group: %s\n", TEST_ADDR);
 
 	for (i = 0;; i++) {
-		socklen_t addr_size = sizeof(struct sockaddr_in);
+		char from_buf[INET6_ADDRSTRLEN];
 		static int iCounter = 1;
+		const char *addr_str;
 
 		/* receive from the multicast address */
 
-		iRet = recvfrom(s, achIn, BUFSIZE, 0, (struct sockaddr *)&stFrom, &addr_size);
-		if (iRet < 0) {
-			printf("recvfrom() failed.\n");
+		ret = mc_recv(&s, achIn, BUFSIZE, &from);
+		if (ret < 0) {
+			perror("recvfrom");
+			exit(1);
+		}
+
+		if (mc.family == AF_INET) {
+			addr_str = inet_ntop(AF_INET, &from.udp4.sin_addr,
+					     from_buf, INET6_ADDRSTRLEN);
+		} else {
+			addr_str = inet_ntop(AF_INET6, &from.udp6.sin6_addr,
+					     from_buf, INET6_ADDRSTRLEN);
+		}
+		if (!addr_str) {
+			perror("inet_ntop");
 			exit(1);
 		}
 
@@ -256,7 +209,8 @@ int main(int argc, char *argv[])
 			numreceived =
 			    (unsigned int)achIn[0] + ((unsigned int)(achIn[1]) << 8) + ((unsigned int)(achIn[2]) << 16) +
 			    ((unsigned int)(achIn[3]) >> 24);
-			fprintf(stdout, "%5d\t%s:%5d\t%d.%03d\t%5d\n", iCounter, inet_ntoa(stFrom.sin_addr), ntohs(stFrom.sin_port),
+			fprintf(stdout, "%5d\t%s:%5d\t%d.%03d\t%5d\n", iCounter,
+				from_buf, socket_get_port(&from),
 				curtime / 1000000, (curtime % 1000000) / 1000, numreceived);
 			fflush(stdout);
 			rcvCountNew = numreceived;
@@ -276,7 +230,7 @@ int main(int argc, char *argv[])
 			rcvCountOld = rcvCountNew;
 		} else {
 			printf("Receive msg %d from %s:%d: %s\n",
-			       iCounter, inet_ntoa(stFrom.sin_addr), ntohs(stFrom.sin_port), achIn);
+			       iCounter, from_buf, socket_get_port(&from), achIn);
 		}
 		iCounter++;
 	}
