l2tpns/util.c

/* 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>
#include <sys/mman.h>

#include "dhcp6.h"
#include "l2tpns.h"
#ifdef BGP
#include "bgp.h"
#endif

// format ipv4 addr as a dotted-quad; n chooses one of 4 static buffers
// to use
char *fmtaddr(in_addr_t addr, int n)
{
    static char addrs[4][INET_ADDRSTRLEN];
    struct in_addr in;

    if (n < 0 || n >= 4)
	return "";

    in.s_addr = addr;
    return strcpy(addrs[n], inet_ntoa(in));
}

// format ipv6 addr; n chooses one of 4 static buffers to use
char *fmtaddr6(struct in6_addr *addr, int n)
{
    static char addrs[4][INET6_ADDRSTRLEN];

    if (n < 0 || n >= 4)
	return "";

    inet_ntop(AF_INET6, addr, addrs[n], sizeof(addrs[n]));
    return addrs[n];
}

char *fmtMacAddr(uint8_t *pMacAddr)
{
	// FF:FF:FF:FF:FF:FF (len(18)= 2*6 + 5 (:) + 1 (zero terminal))
	static char strMAC[2*ETH_ALEN+6];

	sprintf(strMAC, "%02X:%02X:%02X:%02X:%02X:%02X",
			pMacAddr[0], pMacAddr[1], pMacAddr[2],
			pMacAddr[3], pMacAddr[4], pMacAddr[5]);

  return strMAC;
}

void *shared_malloc(unsigned int size)
{
    void * p;
    p = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS, 0, 0);

    if (p == MAP_FAILED)
	p = NULL;

    return p;
}

extern int forked;
extern int cluster_sockfd, tunfd, controlfd, daefd, snoopfd, ifrfd, ifr6fd, rand_fd;
extern int pppoediscfd, pppoesessfd;
extern int *radfds;
extern int udpfd[MAX_UDPFD + 1];

pid_t fork_and_close()
{
	pid_t pid = fork();
	int i;

	if (pid)
		return pid;

	forked++;
	if (config->scheduler_fifo)
	{
		struct sched_param params = {0};
		params.sched_priority = 0;
		if (sched_setscheduler(0, SCHED_OTHER, &params))
		{
			LOG(0, 0, 0, "Error setting scheduler to OTHER after fork: %s\n", strerror(errno));
			LOG(0, 0, 0, "This is probably really really bad.\n");
		}
	}

	signal(SIGPIPE, SIG_DFL);
	signal(SIGCHLD, SIG_DFL);
	signal(SIGHUP,  SIG_DFL);
	signal(SIGUSR1, SIG_DFL);
	signal(SIGQUIT, SIG_DFL);
	signal(SIGKILL, SIG_DFL);
	signal(SIGTERM, SIG_DFL);

	// Close sockets
	if (clifd != -1)	close(clifd);
	if (cluster_sockfd != -1)	close(cluster_sockfd);
	if (tunfd != -1)	close(tunfd);

	for (i = 0; i < config->nbudpfd; i++)
	{
		if (udpfd[i] != -1)	close(udpfd[i]);
	}

	if (controlfd != -1)	close(controlfd);
	if (daefd != -1)	close(daefd);

	for (i = 0; radfds && i < RADIUS_FDS; i++)
		close(radfds[i]);

#ifdef BGP
	for (i = 0; i < BGP_NUM_PEERS; i++)
		if (bgp_peers[i].sock != -1)
			close(bgp_peers[i].sock);
#endif /* BGP */

	if (rtnlfd != -1)	close(rtnlfd);
	if (genlfd != -1)	close(genlfd);
	if (pppoediscfd != -1)	close(pppoediscfd);
	if (pppoesessfd != -1)	close(pppoesessfd);

	for (i = 0; i <= config->cluster_highest_tunnelid; i++)
	{
		if (tunn_local[i].l2tp_fd >= 0)		close(tunn_local[i].l2tp_fd);
	}

	for (i = 0; i <= config->cluster_highest_sessionid; i++)
	{
		if (sess_local[i].pppox_fd >= 0)	close(sess_local[i].pppox_fd);
		if (sess_local[i].ppp_chan_fd >= 0)	close(sess_local[i].ppp_chan_fd);
		if (sess_local[i].ppp_if_fd >= 0)	close(sess_local[i].ppp_if_fd);
	}

	if (snoopfd != -1)	close(snoopfd);
	if (rand_fd != -1)	close(rand_fd);
	if (epollfd != -1)	close(epollfd);

    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, 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_IP && cmsg->cmsg_type == IP_PKTINFO)
		{
			struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
			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;
		}
	}

	return r;
}

ssize_t sendtofrom(int s, void const *buf, size_t len, int flags,
    struct sockaddr const *to, socklen_t tolen, struct in_addr const *from)
{
    struct msghdr msg;
    struct cmsghdr *cmsg;
    struct iovec vec;
    struct in_pktinfo pktinfo;
    char cbuf[CMSG_SPACE(sizeof(pktinfo))];

    memset(&msg, 0, sizeof(msg));
    msg.msg_name = (struct sockaddr *) to;
    msg.msg_namelen = tolen;

    vec.iov_base = (void *) 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);

    cmsg = CMSG_FIRSTHDR(&msg);
    cmsg->cmsg_level = SOL_IP;
    cmsg->cmsg_type = IP_PKTINFO;
    cmsg->cmsg_len = CMSG_LEN(sizeof(pktinfo));

    memset(&pktinfo, 0, sizeof(pktinfo));
    memcpy(&pktinfo.ipi_spec_dst, from, sizeof(*from));
    memcpy(CMSG_DATA(cmsg), &pktinfo, sizeof(pktinfo));

    return sendmsg(s, &msg, flags);
}