welterde/l2tpns
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add recvfromto6

Browse source
This commit is contained in:
Samuel Thibault 2023-05-18 00:38:19 +02:00
parent c3eb1be0b4
commit 13d7080ac1
3 changed files with 78 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

93
util.c
View file

@ -1,9 +1,11 @@
/* Misc util functions */
#define _GNU_SOURCE
#include <unistd.h>
#include <errno.h>
#include <sched.h>
#include <sys/socket.h>
#include <netinet/ip6.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
@ -132,41 +134,90 @@ pid_t fork_and_close()
return pid;
}
static ssize_t recvfromtox(int s, void *buf, size_t len, int flags,
struct sockaddr *from, socklen_t *fromlen, char *cbuf, size_t cbuflen, struct msghdr *msg)
{
ssize_t r;
struct iovec vec;
memset(msg, 0, sizeof(*msg));
msg->msg_name = from;
msg->msg_namelen = *fromlen;
vec.iov_base = buf;
vec.iov_len = len;
msg->msg_iov = &vec;
msg->msg_iovlen = 1;
msg->msg_flags = 0;
msg->msg_control = cbuf;
msg->msg_controllen = cbuflen;
if ((r = recvmsg(s, msg, flags)) < 0)
return r;
if (fromlen)
*fromlen = msg->msg_namelen;
return r;
}
ssize_t recvfromto(int s, void *buf, size_t len, int flags,
struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr)
struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr, int *ifidx)
{
ssize_t r;
struct msghdr msg;
struct cmsghdr *cmsg;
struct iovec vec;
char cbuf[128];
char cbuf[BUFSIZ];
memset(&msg, 0, sizeof(msg));
msg.msg_name = from;
msg.msg_namelen = *fromlen;
vec.iov_base = buf;
vec.iov_len = len;
msg.msg_iov = &vec;
msg.msg_iovlen = 1;
msg.msg_flags = 0;
msg.msg_control = cbuf;
msg.msg_controllen = sizeof(cbuf);
if ((r = recvmsg(s, &msg, flags)) < 0)
if ((r = recvfromtox(s, buf, len, flags, from, fromlen, cbuf, sizeof(cbuf), &msg)) < 0)
return r;
if (fromlen)
*fromlen = msg.msg_namelen;
memset(toaddr, 0, sizeof(*toaddr));
if (toaddr)
memset(toaddr, 0, sizeof(*toaddr));
if (ifidx)
*ifidx = -1;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
{
if (cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_PKTINFO)
{
struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
if (toaddr)
memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
if (ifidx)
*ifidx = i->ipi_ifindex;
break;
}
}
return r;
}
ssize_t recvfromto6(int s, void *buf, size_t len, int flags,
struct sockaddr *from, socklen_t *fromlen, struct in6_addr *toaddr, int *ifidx)
{
ssize_t r;
struct msghdr msg;
struct cmsghdr *cmsg;
char cbuf[BUFSIZ];
if ((r = recvfromtox(s, buf, len, flags, from, fromlen, cbuf, sizeof(cbuf), &msg)) < 0)
return r;
if (toaddr)
memset(toaddr, 0, sizeof(*toaddr));
if (ifidx)
*ifidx = -1;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
{
if (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_PKTINFO)
{
struct in6_pktinfo *i = (struct in6_pktinfo *) CMSG_DATA(cmsg);
if (toaddr)
memcpy(toaddr, &i->ipi6_addr, sizeof(*toaddr));
if (ifidx)
*ifidx = i->ipi6_ifindex;
break;
}
}