Add util mostly for socket-based tasks

2 files changed, 240 insertions, 0 deletions

diff --git a/util.c b/util.c
new file mode 100644
index 0000000..be62ac1
--- /dev/null
+++ b/util.c
@@ -0,0 +1,212 @@
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <netdb.h>
+#include <net/if.h>
+#include <net/if_arp.h>
+#include <netinet/in.h>
+#include <errno.h>
+
+#include <poll.h>
+
+#include "util.h"
+
+uint32_t
+sock_get_ipv4_addr(int fd)
+{
+	struct sockaddr_storage storage;
+	struct sockaddr_in *own = (struct sockaddr_in *) &storage;
+	socklen_t addrlen = sizeof(struct sockaddr_storage);
+
+	assert(getsockname(fd, (struct sockaddr *) &storage, &addrlen) == 0);
+	
+	return own->sin_addr.s_addr;
+}
+
+uint32_t
+sock_get_netmask(int fd)
+{
+	struct ifreq ifreq;
+
+	memset(&ifreq, 0, sizeof ifreq);
+
+	/* FIXME: do not hardcode interface name */
+	strcpy(ifreq.ifr_name, "wlp3s0");
+
+	if (ioctl(fd, SIOCGIFNETMASK, &ifreq) < 0) {
+		fprintf(stderr, "get netmask failed: %s\n", strerror(errno));
+		return 0;
+	}
+
+	return ((struct sockaddr_in *) &ifreq.ifr_netmask)->sin_addr.s_addr;
+}
+
+uint32_t
+sock_get_peer_ipv4_addr(int fd)
+{
+	struct sockaddr_storage storage;
+	struct sockaddr_in *own = (struct sockaddr_in *) &storage;
+	socklen_t addrlen = sizeof(struct sockaddr_storage);
+
+	assert(getpeername(fd, (struct sockaddr *) &storage, &addrlen) == 0);
+	
+	return own->sin_addr.s_addr;
+}
+
+#if 0
+uint32_t
+sock_get_peer_mac_addr(int fd)
+{
+
+}
+#endif
+
+void
+set_ip(uint8_t *ipbuf, uint32_t ip)
+{
+	ipbuf[0] = (ip & 0xFF);
+	ipbuf[1] = (ip & 0xFF00) >> 8;
+	ipbuf[2] = (ip & 0xFF0000) >> 16;
+	ipbuf[3] = (ip & 0xFF000000) >> 24;
+}
+
+
+int
+bind_socket(int socktype, const char *host, const char *port)
+{
+	struct addrinfo hints, *result, *rp;
+	int reuseaddr = 1, s;
+	int fd;
+
+	memset(&hints, 0, sizeof hints);
+	hints.ai_family = AF_INET;
+	hints.ai_socktype = socktype;
+
+	printf("bind to host: %s, port: %s\n", host, port);
+
+	s = getaddrinfo(host, port, &hints, &result);
+	if (s != 0) {
+		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(s));
+		return -1;
+	}
+	for (rp = result; rp != NULL; rp = rp->ai_next) {
+		fd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+		if (fd == -1)
+			continue;
+		if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR,
+			       &reuseaddr, sizeof(reuseaddr)) == -1)
+			continue;
+
+		/*ip = ((struct sockaddr_in *)rp->ai_addr)->sin_addr.s_addr;*/
+		if (bind(fd, rp->ai_addr, rp->ai_addrlen) == 0)
+			break;
+
+		close(fd);
+	}
+	freeaddrinfo(result);
+	if (rp == NULL) {
+		fprintf(stderr, "Failed to bind: %s\n", strerror(errno));
+		return -1;
+	}
+
+	if (socktype == SOCK_STREAM &&
+	    listen(fd, 1) < 0) {
+		fprintf(stderr, "Failed to listen: %s\n", strerror(errno));
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+int
+connect_to_host(int socktype, const char *host, const char *port)
+{
+	struct addrinfo hints, *result, *rp;
+	int fd, s;
+
+	memset(&hints, 0, sizeof hints);
+	hints.ai_family = AF_INET;
+	hints.ai_socktype = socktype;
+
+	printf("connect to host: %s, port: %s\n", host, port);
+
+	s = getaddrinfo(host, port, &hints, &result);
+	if (s != 0) {
+		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(s));
+		return -1;
+	}
+	for (rp = result; rp != NULL; rp = rp->ai_next) {
+		fd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+		if (fd == -1)
+			continue;
+
+		if (connect(fd, rp->ai_addr, rp->ai_addrlen) != -1)
+			break;
+
+		close(fd);
+	}
+	freeaddrinfo(result);
+	if (rp == NULL) {
+		fprintf(stderr, "Failed to bind: %s\n", strerror(errno));
+		return -1;
+	}
+
+	return fd;
+}
+
+/* From systemd:src/shared/util.c */
+ssize_t
+loop_read(int fd, void *buf, size_t nbytes, uint8_t do_poll)
+{
+        uint8_t *p;
+        ssize_t n = 0;
+
+        assert(fd >= 0);
+        assert(buf);
+
+        p = buf;
+
+        while (nbytes > 0) {
+                ssize_t k;
+
+                if ((k = read(fd, p, nbytes)) <= 0) {
+
+                        if (k < 0 && errno == EINTR)
+                                continue;
+
+                        if (k < 0 && errno == EAGAIN && do_poll) {
+                                struct pollfd pollfd;
+
+                                memset(&pollfd, 0, sizeof pollfd);
+                                pollfd.fd = fd;
+                                pollfd.events = POLLIN;
+
+                                if (poll(&pollfd, 1, -1) < 0) {
+                                        if (errno == EINTR)
+                                                continue;
+
+                                        return n > 0 ? n : -errno;
+                                }
+
+                                if (pollfd.revents != POLLIN)
+                                        return n > 0 ? n : -EIO;
+
+                                continue;
+                        }
+
+                        return n > 0 ? n : (k < 0 ? -errno : 0);
+                }
+
+                p += k;
+                nbytes -= k;
+                n += k;
+        }
+
+        return n;
+}
diff --git a/util.h b/util.h
new file mode 100644
index 0000000..4d0e27d
--- /dev/null
+++ b/util.h
@@ -0,0 +1,28 @@
+#ifndef _UTIL_H_
+#define _UTIL_H_
+
+#include <stdint.h>
+
+#define MAX(a, b) (((a) > (b)) ? (a) : (b))
+#define ARRAY_SIZE(arr) (sizeof (arr) / sizeof ((arr)[0]))
+
+#define STR_EXPAND(tok) #tok
+#define STR(str) STR_EXPAND(str)
+
+uint32_t
+sock_get_ipv4_addr(int fd);
+uint32_t
+sock_get_peer_ipv4_addr(int fd);
+uint32_t
+sock_get_netmask(int fd);
+
+void
+set_ip(uint8_t *ipbuf, uint32_t ip);
+int
+bind_socket(int socktype, const char *host, const char *port);
+int
+connect_to_host(int socktype, const char *host, const char *port);
+ssize_t
+loop_read(int fd, void *buf, size_t nbytes, uint8_t do_poll);
+
+#endif /* _UTIL_H_ */