Commit ac38cbee authored by Ondřej Zajíček's avatar Ondřej Zajíček Committed by Pavel Tvrdík

Socktest: Initial commit

Code by Ondrej Santiago Zajicek

(Commit by Pavel Tvrdik)
parent fbcffb33
rcv4
rcv6
snd4
snd6
CC=gcc
CFLAGS=-Wall -g -D LINUX
LDFLAGS=
HEADERS= common.h ipv4-linux.h ipv4-bsd.h ipv6.h
COMMANDS= rcv4 rcv6 snd4 snd6
all: $(COMMANDS)
clean:
rm -f $(COMMANDS)
rcv4: rcv.c $(HEADERS)
$(CC) $(CFLAGS) -D IPV4 rcv.c -o rcv4
rcv6: rcv.c $(HEADERS)
$(CC) $(CFLAGS) -D IPV6 rcv.c -o rcv6
snd4: snd.c $(HEADERS)
$(CC) $(CFLAGS) -D IPV4 snd.c -o snd4
snd6: snd.c $(HEADERS)
$(CC) $(CFLAGS) -D IPV6 snd.c -o snd6
#define _GNU_SOURCE 1
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/uio.h> // Workaround for OpenBSD
#include <arpa/inet.h>
#include <netinet/in.h>
#include <net/if.h>
#define PKT_MAGIC 0x36548925
#define PKT_PVAL 100
#define PKT_VALUE 0
struct my_packet
{
uint32_t magic;
uint32_t value;
uint32_t count;
};
#define ERR(x) do { perror(x); exit(-1); } while(0)
#define SK_IP 1
#define SK_UDP 2
#ifdef IPV4
#define AF_IP AF_INET
#define PF_IP PF_INET
#define SA(X) sin_##X
#define SA_ADDR sin_addr
#define SA_PORT sin_port
typedef struct in_addr inetaddr;
typedef struct sockaddr_in sockaddr;
#define INET_ANY (struct in_addr){INADDR_ANY}
#endif
#ifdef IPV6
#define AF_IP AF_INET6
#define PF_IP PF_INET6
#define SA(X) sin6_##X
#define SA_ADDR sin6_addr
#define SA_PORT sin6_port
typedef struct in6_addr inetaddr;
typedef struct sockaddr_in6 sockaddr;
#define INET_ANY in6addr_any
#endif
#ifdef LINUX
#define SA_BASE .SA(family) = AF_IP
#ifdef IPV4
#include "ipv4-linux.h"
#endif
#ifdef IPV6
#include "ipv6.h"
#endif
#endif /* LINUX */
#ifdef BSD
#define SA_BASE .SA(family) = AF_IP, .SA(len) = sizeof(sockaddr)
#define ifr_ifindex ifr_index
#ifdef IPV4
#define USE_IFADDR
inetaddr cf_ifaddr;
#include "ipv4-bsd.h"
#endif
#ifdef IPV6
#include "ipv6.h"
#endif
#endif /* BSD */
// static void ntoh_addr(inetaddr *a) { a->s_addr = ntohl(a->s_addr); }
void
init_bind(int fd, inetaddr addr, int port)
{
sockaddr lsa = { SA_BASE, .SA_ADDR = addr, .SA_PORT = port };
if (bind(fd, (struct sockaddr *) &lsa, sizeof(lsa)) < 0)
ERR("bind()");
}
void
init_ttl(int fd, int ttl)
{
#ifdef IPV4
if (setsockopt(fd, IPPROTO_IP, IP_TTL, &ttl, sizeof(ttl)) < 0)
ERR("IP_TTL");
#else
if (setsockopt(fd, IPPROTO_IPV6, IPV6_UNICAST_HOPS, &ttl, sizeof(ttl)) < 0)
ERR("IPV6_UNICAST_HOPS");
#endif
}
void
init_dontroute(int fd)
{
int one = 1;
if (setsockopt(fd, SOL_SOCKET, SO_DONTROUTE, &one, sizeof(one)) < 0)
ERR("SO_DONTROUTE");
}
void
init_bcast(int fd)
{
int one = 1;
if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one)) < 0)
ERR("SO_BROADCAST");
}
int
ipv4_skip_header(void **pbuf, int l)
{
unsigned char *pkt = *pbuf;
int q;
if (l < 20 || (*pkt & 0xf0) != 0x40)
return 0;
q = (*pkt & 0x0f) * 4;
if (q > l)
return 0;
*pbuf += q;
return l - q;
}
inetaddr cf_laddr, cf_daddr, cf_baddr;
int cf_mcast, cf_bcast, cf_bind, cf_route;
int cf_type = SK_IP;
int cf_port = PKT_PVAL;
int cf_value = PKT_VALUE;
int cf_ifindex = 0;
int cf_ttl = 1;
static inline void
parse_iface(const char *iface, int *index)
{
*index = if_nametoindex(iface);
#ifdef USE_IFADDR
int fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0)
ERR("socket()");
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
strncpy(ifr.ifr_name, iface, IFNAMSIZ);
if (ioctl(fd, SIOCGIFADDR, (char *) &ifr) < 0)
ERR("SIOCGIFADDR");
struct sockaddr_in *sin = (struct sockaddr_in *) &ifr.ifr_addr;
cf_ifaddr = sin->sin_addr;
close(fd);
printf("ADDR %i %s\n", *index, inet_ntoa(cf_ifaddr));
#endif
}
static void
parse_addr(const char *src, void *dst)
{
if (inet_pton(AF_IP, src, dst) != 1)
{
printf("Invalid address %s\n", src);
exit(-1);
}
}
static void
parse_int(const char *src, int *dst)
{
errno = 0;
*dst = strtol(src, NULL, 10);
if (errno)
{
printf("Invalid number %s\n", src);
exit(-1);
}
}
void
parse_args(int argc, char **argv, int need_daddr)
{
const char *opt_list = "umbRi:l:B:p:v:t:";
int c;
while ((c = getopt(argc, argv, opt_list)) >= 0)
switch (c)
{
case 'u':
cf_type = SK_UDP;
break;
case 'm':
cf_mcast = 1;
break;
case 'b':
cf_bcast = 1;
break;
case 'R':
cf_route = 1;
break;
case 'i':
parse_iface(optarg, &cf_ifindex);
break;
case 'l':
parse_addr(optarg, &cf_laddr);
break;
case 'B':
parse_addr(optarg, &cf_baddr);
cf_bind = 1;
break;
case 'p':
parse_int(optarg, &cf_port);
break;
case 'v':
parse_int(optarg, &cf_value);
break;
case 't':
parse_int(optarg, &cf_ttl);
break;
default:
goto usage;
}
if (optind + need_daddr != argc)
goto usage;
if (need_daddr)
parse_addr(argv[optind], &cf_daddr);
return;
usage:
printf("Usage: %s [-u] [-m|-b] [-B baddr] [-R] [-i iface] [-l addr] [-p port] [-v value] [-t ttl]%s\n",
argv[0], need_daddr ? " daddr" : "");
exit(1);
}
#include <net/if_dl.h>
#include <netinet/in_systm.h> // Workaround for some BSDs
#include <netinet/ip.h>
void
init_maddr(int fd, int ifindex, struct in_addr laddr, struct in_addr maddr, int ttl_, int mship)
{
unsigned char zero = 0;
unsigned char ttl = ttl_;
struct ip_mreq mreq = { .imr_interface = laddr, .imr_multiaddr = maddr };
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) < 0)
ERR("IP_MULTICAST_TTL");
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &zero, sizeof(zero)) < 0)
ERR("IP_MULTICAST_LOOP");
/* This defines where should we send _outgoing_ multicasts */
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &cf_ifaddr, sizeof(cf_ifaddr)) < 0)
ERR("IP_MULTICAST_IF");
if (!mship)
return;
/* And this one sets interface for _receiving_ multicasts from */
if (setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
ERR("IP_ADD_MEMBERSHIP");
}
void
init_pktinfo(int fd)
{
int ok = 1;
if (setsockopt(fd, IPPROTO_IP, IP_RECVDSTADDR, &ok, sizeof(ok)) < 0)
ERR("IP_RECVDSTADDR");
if (setsockopt(fd, IPPROTO_IP, IP_RECVIF, &ok, sizeof(ok)) < 0)
ERR("IP_RECVIF");
#ifdef IP_RECVTTL
if (setsockopt(fd, IPPROTO_IP, IP_RECVTTL, &ok, sizeof(ok)) < 0)
ERR("IP_RECVTTL");
#endif
}
void
init_hdrincl(int fd)
{
int ok = 1;
if (setsockopt(fd, IPPROTO_IP, IP_HDRINCL, &ok, sizeof(ok)) < 0)
ERR("IP_HDRINCL");
}
struct in_addr *recv_addr;
struct sockaddr_dl *recv_if;
unsigned char *recv_ttl;
void
parse_rx_cmsgs(struct msghdr *msg)
{
struct cmsghdr *cm;
recv_addr = NULL;
recv_if = NULL;
recv_ttl = NULL;
for (cm = CMSG_FIRSTHDR(msg); cm != NULL; cm = CMSG_NXTHDR(msg, cm))
{
if (cm->cmsg_level != IPPROTO_IP)
continue;
if (cm->cmsg_type == IP_RECVDSTADDR)
recv_addr = (struct in_addr *) CMSG_DATA(cm);
if (cm->cmsg_type == IP_RECVIF)
recv_if = (struct sockaddr_dl *) CMSG_DATA(cm);
#ifdef IP_RECVTTL
if (cm->cmsg_type == IP_RECVTTL)
recv_ttl = (unsigned char *) CMSG_DATA(cm);
#endif
}
}
static inline inetaddr get_recv_addr(void)
{ return recv_addr ? *recv_addr : INET_ANY; }
static inline int get_recv_iface(void)
{ return recv_if ? recv_if->sdl_index : -1; }
static inline int get_recv_ttl(void)
{ return recv_ttl ? *recv_ttl : -1; }
#define RAW_USE_HDR 1
extern int cf_type;
void
prepare_tx_cmsgs(int fd, struct msghdr *msg, void *buf, int blen, struct in_addr laddr)
{
#ifdef RAW_USE_HDR
if (cf_type == SK_IP)
return;
#endif
#ifdef IP_SENDSRCADDR
struct cmsghdr *cm;
struct in_addr *sa;
msg->msg_control = buf;
msg->msg_controllen = blen;
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);
*sa = laddr;
msg->msg_controllen = cm->cmsg_len;
#endif
}
void
ipv4_fill_header(void *buf, int proto, int ttl, struct in_addr src, struct in_addr dst, int dlen)
{
struct ip *ip = buf;
bzero(ip, sizeof(*ip));
ip->ip_v = 4;
ip->ip_hl = 5;
ip->ip_len = dlen + sizeof(*ip);
ip->ip_ttl = ttl;
ip->ip_p = proto;
ip->ip_src = src;
ip->ip_dst = dst;
#ifdef __OpenBSD__
ip->ip_len = htons(ip->ip_len);
#endif
}
void
init_maddr(int fd, int ifindex, struct in_addr laddr, struct in_addr maddr, int ttl, int mship)
{
struct ip_mreqn mreq;
int zero = 0;
mreq.imr_ifindex = ifindex;
mreq.imr_address = laddr;
mreq.imr_multiaddr = maddr;
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) < 0)
ERR("IP_MULTICAST_TTL");
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &zero, sizeof(zero)) < 0)
ERR("IP_MULTICAST_LOOP");
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &mreq, sizeof(mreq)) < 0)
ERR("IP_MULTICAST_IF");
if (!mship)
return;
if (setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)
ERR("IP_ADD_MEMBERSHIP");
}
void
init_pktinfo(int fd)
{
int ok = 1;
if (setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &ok, sizeof(ok)) < 0)
ERR("IP_PKTINFO");
if (setsockopt(fd, IPPROTO_IP, IP_RECVTTL, &ok, sizeof(ok)) < 0)
ERR("IP_RECVTTL");
}
struct in_pktinfo *recv_pi;
int *recv_ttl;
void
parse_rx_cmsgs(struct msghdr *msg)
{
struct cmsghdr *cm;
recv_pi = NULL;
recv_ttl = NULL;
for (cm = CMSG_FIRSTHDR(msg); cm != NULL; cm = CMSG_NXTHDR(msg, cm))
{
if (cm->cmsg_level != IPPROTO_IP)
continue;
if (cm->cmsg_type == IP_PKTINFO)
recv_pi = (struct in_pktinfo *) CMSG_DATA(cm);
if (cm->cmsg_type == IP_TTL)
recv_ttl = (int *) CMSG_DATA(cm);
}
}
static inline inetaddr get_recv_addr(void)
{ return recv_pi ? recv_pi->ipi_addr : INET_ANY; }
static inline int get_recv_iface(void)
{ return recv_pi ? recv_pi->ipi_ifindex : -1; }
static inline int get_recv_ttl(void)
{ return recv_ttl ? *recv_ttl : -1; }
void
prepare_tx_cmsgs(int fd, struct msghdr *msg, void *buf, int blen, struct in_addr laddr)
{
struct cmsghdr *cm;
struct in_pktinfo *pi;
msg->msg_control = buf;
msg->msg_controllen = blen;
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);
pi->ipi_ifindex = 0;
pi->ipi_spec_dst = laddr;
pi->ipi_addr = INET_ANY;
msg->msg_controllen = cm->cmsg_len;
}
void
init_maddr(int fd, int ifindex, struct in6_addr laddr, struct in6_addr maddr, int ttl, int mship)
{
struct ipv6_mreq mreq = { .ipv6mr_multiaddr = maddr, .ipv6mr_interface = ifindex };
int zero = 0;
if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &ttl, sizeof(ttl)) < 0)
ERR("IPV6_MULTICAST_HOPS");
if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &zero, sizeof(zero)) < 0)
ERR("IPV6_MULTICAST_LOOP");
if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_IF, &index, sizeof(index)) < 0)
ERR("IPV6_MULTICAST_IF");
if (!mship)
return;
if (setsockopt(fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) < 0)
ERR("IPV6_JOIN_GROUP");
}
void
init_pktinfo(int fd)
{
int ok = 1;
if (setsockopt(fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &ok, sizeof(ok)) < 0)
ERR("IPV6_RECVPKTINFO");
if (setsockopt(fd, IPPROTO_IPV6, IPV6_RECVHOPLIMIT, &ok, sizeof(ok)) < 0)
ERR("IPV6_RECVHOPLIMIT");
}
struct in6_pktinfo *recv_pi = NULL;
int *recv_hlim = NULL;
void
parse_rx_cmsgs(struct msghdr *msg)
{
struct cmsghdr *cm;
recv_pi = NULL;
recv_hlim = NULL;
for (cm = CMSG_FIRSTHDR(msg); cm != NULL; cm = CMSG_NXTHDR(msg, cm))
{
if (cm->cmsg_level != IPPROTO_IPV6)
continue;
if (cm->cmsg_type == IPV6_PKTINFO)
recv_pi = (struct in6_pktinfo *) CMSG_DATA(cm);
if (cm->cmsg_type == IPV6_HOPLIMIT)
recv_hlim = (int *) CMSG_DATA(cm);
}
}
static inline inetaddr get_recv_addr(void)
{ return recv_pi ? recv_pi->ipi6_addr : INET_ANY; }
static inline int get_recv_iface(void)
{ return recv_pi ? recv_pi->ipi6_ifindex : -1; }
static inline int get_recv_ttl(void)
{ return recv_hlim ? *recv_hlim : -1; }
void
prepare_tx_cmsgs(int fd, struct msghdr *msg, void *buf, int blen, struct in6_addr laddr)
{
struct cmsghdr *cm;
struct in6_pktinfo *pi;
msg->msg_control = buf;
msg->msg_controllen = blen;
cm = CMSG_FIRSTHDR(msg);
cm->cmsg_level = IPPROTO_IPV6;
cm->cmsg_type = IPV6_PKTINFO;
cm->cmsg_len = CMSG_LEN(sizeof(*pi));
pi = (struct in6_pktinfo *) CMSG_DATA(cm);
pi->ipi6_ifindex = 0;
pi->ipi6_addr = laddr;
msg->msg_controllen = cm->cmsg_len;
return;
}
#include "common.h"
int
do_recvmsg(int fd, inetaddr *saddr, inetaddr *daddr, void **pbuf, size_t blen)
{
char cmsg_buf[256];
sockaddr src;
struct iovec iov = {*pbuf, blen};
struct msghdr msg = {
.msg_name = (struct sockaddr *) &src,
.msg_namelen = sizeof(src),
.msg_iov = &iov,
.msg_iovlen = 1,
.msg_control = cmsg_buf,
.msg_controllen = sizeof(cmsg_buf)
};
int rv = recvmsg(fd, &msg, 0);
if (rv < 0)
ERR("recvmsg()");
#ifdef IPV4
if (cf_type == SK_IP)
rv = ipv4_skip_header(pbuf, rv);
#endif
parse_rx_cmsgs(&msg);
*saddr = src.SA_ADDR;
*daddr = get_recv_addr();
return rv;
}
int
main(int argc, char **argv)
{
int fd, type, proto, port;
parse_args(argc, argv, 0);
if (cf_type == SK_IP)
{
type = SOCK_RAW;
proto = cf_port;
port = 0;
}
else if (cf_type == SK_UDP)
{
type = SOCK_DGRAM;
proto = IPPROTO_UDP;
port = htons(cf_port);
}
fd = socket(PF_IP, type, proto);
if (fd < 0)
ERR("socket()");
if (cf_bind || port)
init_bind(fd, cf_baddr, port);
if (!cf_route)
init_dontroute(fd);
if (cf_mcast)
init_maddr(fd, cf_ifindex, cf_laddr, cf_daddr, cf_ttl, 1);
else
init_ttl(fd, cf_ttl);
if (cf_bcast)
init_bcast(fd);
init_pktinfo(fd);
while (1)
{
char buf[2048];
void *buf1 = buf;
inetaddr saddr, daddr;
int rv = do_recvmsg(fd, &saddr, &daddr, &buf1, sizeof(buf));
if (rv < 0)
ERR("recvmsg()");
struct my_packet *pkt = buf1;
int ifa = get_recv_iface();
char ifa_name[IF_NAMESIZE];
if_indextoname(ifa, ifa_name);
int ttl = get_recv_ttl();
char b1[128], b2[128];
inet_ntop(AF_IP, &saddr, b1, sizeof(b1));
inet_ntop(AF_IP, &daddr, b2, sizeof(b2));
if ((rv == sizeof(struct my_packet)) && (pkt->magic == PKT_MAGIC))
printf("%s %s: pkt %d/%d iface %s(%d) ttl %d\n", b1, b2, pkt->value, pkt->count, ifa_name, ifa, ttl);
else
printf("%s %s: foreign packet received (%d)\n", b1, b2, rv);
}
}
#include "common.h"