From: Joseph C. Lehner <joseph.c.lehner@gmail.com>
Date: Mon, 25 Jan 2016 15:58:07 +0000 (+0100)
Subject: Add rawsock stuff (WIP)
X-Git-Tag: v0.9~90
X-Git-Url: https://git.librecmc.org/?a=commitdiff_plain;h=53a57860537338f816e55f222e29387ad90e1f72;p=oweals%2Fnmrpflash.git

Add rawsock stuff (WIP)
---

diff --git a/rawsock.c b/rawsock.c
new file mode 100644
index 0000000..27888f6
--- /dev/null
+++ b/rawsock.c
@@ -0,0 +1,181 @@
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <net/bpf.h>
+#include <net/if.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdio.h>
+
+#ifndef MIN
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
+struct ethsock
+{
+	int fd;
+	struct timeval timeout;
+};
+
+static int init_filter(struct ethsock *sock, int protocol, int mtu)
+{
+	struct bpf_program prog;
+	struct bpf_insn insns[] = {
+			BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 12),
+			BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, protocol, 0, 1),
+			BPF_STMT(BPF_RET+BPF_K, mtu),
+			BPF_STMT(BPF_RET+BPF_K, 0)
+     };
+
+	prog.bf_insns = insns;
+	prog.bf_len = sizeof(insns) / sizeof(insns[0]);
+
+	if (ioctl(sock->fd, BIOCSETFNR, &prog) != 0) {
+		perror("ioctl(BIOCSETFNR)");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int get_mtu(struct ifreq *ifr)
+{
+	int fd, stat;
+
+	fd = socket(AF_INET, SOCK_DGRAM, 0);
+	if (fd < 0) {
+		perror("socket");
+		return -1;
+	}
+
+	stat = ioctl(fd, SIOCGIFMTU, ifr);
+	if (stat < 0) {
+		perror("ioctl(SIOCGIFMTU)");
+		stat = -1;
+	} else {
+		stat = 0;
+	}
+
+	close(fd);
+	return stat;
+}
+
+int ethsock_create(struct ethsock *sock, const char *interface, int protocol)
+{
+	struct ifreq ifr;
+	struct bpf_program bf;
+	int i, val;
+	char buf[12];
+
+	for (i = 0; i < 100; ++i) {
+		sprintf(buf, "/dev/bpf%d", i);
+		sock->fd = open(buf, O_RDWR);
+		if (sock->fd != -1) {
+			break;
+		}
+	}
+
+	if (sock->fd == -1) {
+		fprintf(stderr, "Failed to open bpf device\n");
+		return -1;
+	}
+
+	strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1);
+
+	if (ioctl(sock->fd, BIOCSETIF, &ifr) != 0) {
+		perror("ioctl(BIOCSETIF)");
+		return -1;
+	}
+
+	val = 1;
+
+	if (ioctl(sock->fd, BIOCIMMEDIATE, &val) != 0) {
+		perror("ioctl(BIOCIMMEDIATE)");
+		return -1;
+	}
+
+	if (ioctl(sock->fd, BIOCGBLEN, &val) != 0) {
+		perror("ioctl(BIOCGBLEN)");
+		return -1;
+	}
+
+	if (get_mtu(&ifr) != 0) {
+		return -1;
+	}
+
+	if (init_filter(sock, protocol, ifr.ifr_mtu) != 0) {
+		return -1;
+	}
+
+	sock->timeout.tv_sec = 0;
+	sock->timeout.tv_usec = 0;
+
+	return 0;
+}
+
+int ethsock_close(struct ethsock *sock)
+{
+	close(sock->fd);
+	return 0;
+}
+
+int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
+{
+	sock->timeout.tv_sec = msec / 1000;
+	sock->timeout.tv_usec = (msec % 1000) * 1000;
+	return 0;
+}
+
+ssize_t ethsock_read(struct ethsock *sock, void *buf, size_t size)
+{
+	struct bpf_hdr *bh;
+	ssize_t len;
+	fd_set fds;
+	int err;
+
+	if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
+		FD_ZERO(&fds);
+		FD_SET(sock->fd, &fds);
+
+		err = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
+		if (err == -1) {
+			perror("select");
+			return -1;
+		} else if (!err) {
+			return 0;
+		}
+	}
+
+	len = read(sock->fd, buf, size);
+	if (len < 0) {
+		perror("read");
+	}
+
+	bh = (struct bpf_hdr*)buf;
+	len = MIN(size, bh->bh_datalen);
+	memmove(buf, (char*)buf + bh->bh_hdrlen, len);
+
+	return len;
+}
+
+int main()
+{
+	struct ethsock sock;
+	ssize_t len;
+	char buf[1024];
+
+	if (ethsock_create(&sock, "en0", 0x0912) != 0) {
+		return 1;
+	}
+
+	ethsock_set_timeout(&sock, 1000);
+
+	len = ethsock_read(&sock, buf, sizeof(buf));
+	if (len > 0) {
+		write(1, buf, len);
+	}
+
+	ethsock_close(&sock);
+	return len > 0 ? 0 : 1;
+}