tel/libtel.c

#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <stdint.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netdb.h>
#include <string.h>
#include <stdio.h>
#include "libtel.h"

int tel_sa_any_eq(union tel_sa_any *a, union tel_sa_any *b) {
	struct sockaddr *as = (struct sockaddr *)a;
	struct sockaddr *bs = (struct sockaddr *)b;
	if(as->sa_family != bs->sa_family) {
		return 0;
	}
	if(as->sa_family == AF_INET) {
		struct sockaddr_in *a4 = &a->v4;
		struct sockaddr_in *b4 = &b->v4;
		if(a4->sin_port != b4->sin_port) {
			return 0;
		}
		if(a4->sin_addr.s_addr != b4->sin_addr.s_addr) {
			return 0;
		}
		return 1;
	} else if(as->sa_family == AF_INET6) {
		struct sockaddr_in6 *a6 = &a->v6;
		struct sockaddr_in6 *b6 = &b->v6;
		if(a6->sin6_port != b6->sin6_port) {
			return 0;
		}
		if(memcmp(a6->sin6_addr.s6_addr, b6->sin6_addr.s6_addr, 16 * sizeof(uint8_t)) != 0) {
			return 0;
		}
		return 1;
	} else {
		return 0;
	}
}


int tel_get_addr(char *ip, char *port, struct sockaddr **addr, socklen_t *addr_len) {
	if(port == NULL) port = "4111";

	struct addrinfo hints;
	memset(&hints, 0, sizeof(struct addrinfo));
	hints.ai_family = AF_UNSPEC;
	hints.ai_socktype = SOCK_STREAM;
	hints.ai_flags = AI_V4MAPPED | AI_ADDRCONFIG;

	struct addrinfo *info;
	int res = getaddrinfo(ip, port, &hints, &info);
	if(res != 0) return res;

	*addr = info->ai_addr;
	*addr_len = info->ai_addrlen;

	freeaddrinfo(info);
	return 0;
}

int tel_straddr(const struct sockaddr *restrict addr, char *restrict buf) {
	uint16_t port = 0;

	if(addr->sa_family == AF_INET) {
		struct sockaddr_in *addr4 = (struct sockaddr_in *)addr;

		port = addr4->sin_port;

		const char *res = inet_ntop(AF_INET, &addr4->sin_addr, buf, INET6_ADDRSTRLEN);
		if(res == NULL) {
			return -1;
		}
	} else if(addr->sa_family == AF_INET6) {
		struct sockaddr_in6 *addr6 = (struct sockaddr_in6 *)addr;

		port = addr6->sin6_port;

		const char *res = inet_ntop(AF_INET6, &addr6->sin6_addr, buf, INET6_ADDRSTRLEN);
		if(res == NULL) {
			return -1;
		}
	} else {
		errno = 0;
		return -1;
	}

	size_t len = strlen(buf);
	sprintf(buf + len, " %u", ntohs(port));
	return 0;
}


ssize_t tel_write_all(int fd, const void *buf, size_t count) {
	const char *cbuf = (char *)buf;
	ssize_t wc;
	ssize_t total = 0;

	while(count > 0) {
		wc = write(fd, cbuf, count);
		if(wc <= 0) {
			return wc;
		}
		cbuf += wc;
		count -= wc;
		total += wc;
	}

	return total;
}

ssize_t tel_read_all(int fd, void *buf, size_t count) {
	char *cbuf = (char *)buf;
	ssize_t rc;
	ssize_t total = 0;

	while(count > 0) {
		rc = read(fd, cbuf, count);
		if(rc <= 0) {
			return rc;
		}
		cbuf += rc;
		count -= rc;
		total += rc;
	}

	return total;
}

ssize_t tel_write_u32(int fd, uint32_t n) {
	uint32_t nn = htonl(n);
	char bytes[4];
	memcpy(bytes, &nn, 4);
	return tel_write_all(fd, bytes, 4);
}

ssize_t tel_read_u32(int fd, uint32_t *n) {
	char bytes[4];
	ssize_t res = tel_read_all(fd, bytes, 4);
	if(res < 4) return res;
	uint32_t nn;
	memcpy(&nn, bytes, 4);
	*n = ntohl(nn);
	return res;
}

ssize_t tel_write_u16(int fd, uint16_t n) {
	uint16_t nn = htons(n);
	char bytes[2];
	memcpy(bytes, &nn, 2);
	return tel_write_all(fd, bytes, 2);
}

ssize_t tel_read_u16(int fd, uint16_t *n) {
	char bytes[2];
	ssize_t res = tel_read_all(fd, bytes, 2);
	if(res < 2) return res;
	uint16_t nn;
	memcpy(&nn, bytes, 2);
	*n = ntohs(nn);
	return res;
}

ssize_t tel_write_fd(int stream, int fd) {
	struct msghdr msg;
    struct iovec iov[1];
    struct cmsghdr *cmsg = NULL;
    char ctrl_buf[CMSG_SPACE(sizeof(int))];
    char data[1];

    memset(&msg, 0, sizeof(struct msghdr));
    memset(ctrl_buf, 0, CMSG_SPACE(sizeof(int)));

    data[0] = '#';
    iov[0].iov_base = data;
    iov[0].iov_len = sizeof(data);

    msg.msg_name = NULL;
    msg.msg_namelen = 0;
    msg.msg_iov = iov;
    msg.msg_iovlen = 1;
    msg.msg_controllen =  CMSG_SPACE(sizeof(int));
    msg.msg_control = ctrl_buf;

    cmsg = CMSG_FIRSTHDR(&msg);
    cmsg->cmsg_level = SOL_SOCKET;
    cmsg->cmsg_type = SCM_RIGHTS;
    cmsg->cmsg_len = CMSG_LEN(sizeof(int));

    *((int *) CMSG_DATA(cmsg)) = fd;

    return sendmsg(stream, &msg, 0);
}

ssize_t tel_read_fd(int sock, int *fd) {
	int len;
    char buf[1];
    struct iovec iov;
    struct msghdr msg;
    struct cmsghdr *cmsg;
    char cms[CMSG_SPACE(sizeof(int))];

    iov.iov_base = buf;
    iov.iov_len = sizeof(buf);

    msg.msg_name = 0;
    msg.msg_namelen = 0;
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;
    msg.msg_flags = 0;
    msg.msg_control = (caddr_t) cms;
    msg.msg_controllen = sizeof cms;

    len = recvmsg(sock, &msg, 0);

	if(len <= 0) {
		return len;
	}

    cmsg = CMSG_FIRSTHDR(&msg);
    memmove(fd, CMSG_DATA(cmsg), sizeof(int));

	return len;
}

ssize_t tel_write_sockaddr(int fd, struct sockaddr *addr) {
	ssize_t res;
	ssize_t total = 0;

	if(addr->sa_family == AF_INET) {
		struct sockaddr_in *addr4 = (struct sockaddr_in *)addr;

		res = tel_write_u32(fd, 4);
		if(res < 0) return res;
		total += res;

		res = tel_write_u32(fd, addr4->sin_addr.s_addr);
		if(res < 0) return res;
		total += res;

		res = tel_write_u16(fd, addr4->sin_port);
		if(res < 0) return res;
		total += res;

		return total;
	} else if(addr->sa_family == AF_INET6) {
		struct sockaddr_in6 *addr6 = (struct sockaddr_in6 *)addr;

		res = tel_write_u32(fd, 6);
		if(res < 0) return res;
		total += res;

		res = tel_write_all(fd, addr6->sin6_addr.s6_addr, 16);
		if(res < 0) return res;
		total += res;

		res = tel_write_u16(fd, addr6->sin6_port);
		if(res < 0) return res;
		total += res;
	} else {
		errno = ESOCKTNOSUPPORT;
		return -1;
	}
	return total;
}

ssize_t tel_read_sockaddr(int fd, union tel_sa_any *addr, socklen_t *addr_len) {
	ssize_t res;
	ssize_t total = 0;

	uint32_t ty = 0;
	res = tel_read_u32(fd, &ty);
	if(res < 0) return res;
	total += res;

	if(ty == 4) {
		if(*addr_len < sizeof(struct sockaddr_in)) {
			printf("addr_len %u too small for ipv4\n", *addr_len);
			errno = 0;
			return -1;
		}
		struct sockaddr_in *addr4 = &addr->v4;
		*addr_len = sizeof(struct sockaddr_in);
		addr4->sin_family = AF_INET;

		res = tel_read_u32(fd, &addr4->sin_addr.s_addr);
		if(res < 0) return res;
		total += res;

		res = tel_read_u16(fd, &addr4->sin_port);
		if(res < 0) return res;
		total += res;
	} else if(ty == 6) {
		if(*addr_len < sizeof(struct sockaddr_in6)) {
			printf("addr_len %u to small for ipv6\n", *addr_len);
			errno = 0;
			return -1;
		}
		struct sockaddr_in6 *addr6 = &addr->v6;
		*addr_len = sizeof(struct sockaddr_in6);
		addr6->sin6_family = AF_INET6;

		res = tel_read_all(fd, addr6->sin6_addr.s6_addr, 16);
		if(res < 0) return res;
		total += res;

		res = tel_read_u16(fd, &addr6->sin6_port);
		if(res < 0) return res;
		total += res;
	} else {
		printf("ty was %u\n", ty);
		errno = 0;
		return -1;
	}
	return total;
}

ssize_t tel_write_err(int fd, const char *msg) {
	size_t msglen = strlen(msg);
	ssize_t res1 = tel_write_u32(fd, MSG_ERROR);
	if(res1 < 0) return res1;
	ssize_t res2 = tel_write_u32(fd, msglen);
	if(res2 < 0) return res2;
	ssize_t res3 = tel_write_all(fd, msg, msglen);
	if(res3 < 0) return res3;
	return res1 + res2 + res3;
}