From 20db645d14508e9732ab61f856b6d31263e7e600 Mon Sep 17 00:00:00 2001
From: Richard Fuchs <rfuchs@sipwise.com>
Date: Tue, 1 Aug 2023 14:12:12 -0400
Subject: [PATCH] MT#55283 add cmsg_pktinfo family

These fill in the cmsg structs for sendmsg() to set the local source
address to use.

Change-Id: I58a67c6e93cf4ef57eb1afd76f55c76f5b3612e1
---
 lib/socket.c | 20 ++++++++++++++++++++
 lib/socket.h |  1 +
 2 files changed, 21 insertions(+)

diff --git a/lib/socket.c b/lib/socket.c
index 395f2ab93..b486f4cf2 100644
--- a/lib/socket.c
+++ b/lib/socket.c
@@ -53,6 +53,8 @@ static unsigned int __ip4_packet_header(unsigned char *, const endpoint_t *, con
 		unsigned int);
 static unsigned int __ip6_packet_header(unsigned char *, const endpoint_t *, const endpoint_t *,
 		unsigned int);
+static void __ip4_cmsg_pktinfo(struct cmsghdr *, const sockaddr_t *);
+static void __ip6_cmsg_pktinfo(struct cmsghdr *, const sockaddr_t *);
 
 
 
@@ -97,6 +99,7 @@ static struct socket_family __socket_families[__SF_LAST] = {
 		.endpoint2kernel	= __ip4_endpoint2kernel,
 		.kernel2endpoint	= __ip4_kernel2endpoint,
 		.packet_header		= __ip4_packet_header,
+		.cmsg_pktinfo		= __ip4_cmsg_pktinfo,
 	},
 	[SF_IP6] = {
 		.af			= AF_INET6,
@@ -130,6 +133,7 @@ static struct socket_family __socket_families[__SF_LAST] = {
 		.endpoint2kernel	= __ip6_endpoint2kernel,
 		.kernel2endpoint	= __ip6_kernel2endpoint,
 		.packet_header		= __ip6_packet_header,
+		.cmsg_pktinfo		= __ip6_cmsg_pktinfo,
 	},
 };
 
@@ -439,12 +443,28 @@ static int __ip4_pktinfo(socket_t *s) {
 		return -1;
 	return 0;
 }
+static void __ip4_cmsg_pktinfo(struct cmsghdr *cm, const sockaddr_t *addr) {
+	cm->cmsg_level = IPPROTO_IP;
+	cm->cmsg_type = IP_PKTINFO;
+	struct in_pktinfo *pi = (void *) CMSG_DATA(cm);
+	ZERO(*pi);
+	pi->ipi_spec_dst = addr->u.ipv4;
+	cm->cmsg_len = CMSG_LEN(sizeof(*pi));
+}
 static int __ip6_pktinfo(socket_t *s) {
 	int one = 1;
 	if (setsockopt(s->fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &one, sizeof(one)))
 		return -1;
 	return 0;
 }
+static void __ip6_cmsg_pktinfo(struct cmsghdr *cm, const sockaddr_t *addr) {
+	cm->cmsg_level = IPPROTO_IPV6;
+	cm->cmsg_type = IPV6_PKTINFO;
+	struct in6_pktinfo *pi = (void *) CMSG_DATA(cm);
+	ZERO(*pi);
+	pi->ipi6_addr = addr->u.ipv6;
+	cm->cmsg_len = CMSG_LEN(sizeof(*pi));
+}
 static void __ip4_endpoint2kernel(struct re_address *ra, const endpoint_t *ep) {
 	ZERO(*ra);
 	ra->family = AF_INET;
diff --git a/lib/socket.h b/lib/socket.h
index 5d99a5299..ded7a8391 100644
--- a/lib/socket.h
+++ b/lib/socket.h
@@ -85,6 +85,7 @@ struct socket_family {
 	void				(*kernel2endpoint)(endpoint_t *, const struct re_address *);
 	unsigned int			(*packet_header)(unsigned char *, const endpoint_t *, const endpoint_t *,
 						unsigned int);
+	void				(*cmsg_pktinfo)(struct cmsghdr *, const sockaddr_t *);
 };
 struct socket_address {
 	sockfamily_t			*family;