ethermess/ethermess.c

#define _GNU_SOURCE
#include <arpa/inet.h>
#include <assert.h>
#include <err.h>
#include <errno.h>
#include <inttypes.h>
#include <linux/if_packet.h>
#include <net/ethernet.h>
#include <net/if.h>
#include <net/if_arp.h>
#include <poll.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>

bool running = true;

int packet_socket;

unsigned char own_mac[6];

char hexify(int nybble) {
	assert(0 <= nybble && nybble <= 16);
	return "0123456789abcdef"[nybble];
}

void format_mac(const unsigned char binary_address[6], char formatted[18]) {
	for (size_t i = 0; i < 6; i++) {
		unsigned char byte = binary_address[i];
		formatted[3*i] = hexify(byte >> 4);
		formatted[3*i + 1] = hexify(byte & 0xf);
		formatted[3*i + 2] = ':';
	}
	formatted[17] = '\0';
}

void drop_privileges(void) {
	uid_t uid = getuid();
	gid_t gid = getgid();

	errno = 0;
	if (setresgid(gid, gid, gid) == -1) {
		err(1, "setresgid");
	}
	errno = 0;
	if (setresuid(uid, uid, uid) == -1) {
		err(1, "setresuid");
	}
}

void read_command(void) {
	int cmd = getchar();
	if (cmd == EOF) {
		err(1, "getchar");
	}

	if (cmd == 'q') {
		running = false;
	} else if (cmd == '\n') {
		// Ignore
	} else {
		fprintf(stderr, "?"); //debg
	}
}

void process_frame(void) {
	unsigned char frame[1518]; // Largest a 802.3 frame can be without FCS
	unsigned char broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

	errno = 0;
	if (recv(packet_socket, frame, sizeof(frame), 0) == -1) {
		errx(1, "recv");
	}

	if (memcmp(frame, own_mac, sizeof(own_mac)) == 0) {
		// Targetted at us
		fprintf(stderr, "."); // debg
	} else if (memcmp(frame, broadcast_addr, sizeof(broadcast_addr)) == 0) {
		// Broadcast
		fprintf(stderr, "^"); // debg
	} else {
		// No concern
		return;
	}
}

void eventloop(void) {
	// Listen on both stdin for commands and network interface for packets
	struct pollfd pollfds[2];

	// stdin
	pollfds[0].fd = 0;
	pollfds[0].events = POLLIN;

	// Network interface
	pollfds[1].fd = packet_socket;
	pollfds[1].events = POLLIN;

	while (running) {
		errno = 0;
		int ready = poll(pollfds, sizeof(pollfds) / sizeof(*pollfds), -1);
		if (ready == -1) {
			err(1, "poll");
		}

		// stdin
		if (ready > 0 && pollfds[0].revents != 0) {
			ready--;

			if (pollfds[0].revents & POLLIN) {
				// Read a command
				read_command();
			} else {
				errx(1, "Got poll event %hd on stdin\n", pollfds[0].revents);
			}
		}

		// packet_socket
		if (ready > 0 && pollfds[1].revents != 0) {
			ready--;

			if (pollfds[1].revents & POLLIN) {
				// Process a frame
				process_frame();
			} else {
				errx(1, "Got poll event %hd on packet socket\n", pollfds[1].revents);
			}
		}

		if (ready > 0) {
			errx(1, "poll(1) says we have ready fds, but neither was ready");
		}
	}
}

int main(int argc, char **argv) {
	if (argc != 2) {
		fprintf(stderr, "Usage: %s interface\n", argv[0]);
		exit(1);
	}

	const char *interface_name = argv[1];

	// Create a packet socket
	errno = 0;
	packet_socket = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	if (packet_socket == -1) {
		err(1, "socket");
	}

	// Only creating the socket requires root privs
	drop_privileges();

	// Find the index of the network interface
	struct ifreq ifr;
	strncpy(ifr.ifr_name, interface_name, IFNAMSIZ);
	errno = 0;
	if (ioctl(packet_socket, SIOCGIFINDEX, &ifr) == -1) {
		err(1, "ioctl");
	}

	// Bind to the network interface
	struct sockaddr_ll sll;
	sll.sll_family = AF_PACKET;
	sll.sll_protocol = htons(ETH_P_ALL);
	sll.sll_ifindex = ifr.ifr_ifindex;
	errno = 0;
	if (bind(packet_socket, (const struct sockaddr*)&sll, sizeof(sll)) == -1) {
		err(1, "bind");
	}

	// Get our own MAC
	errno = 0;
	strncpy(ifr.ifr_name, interface_name, IFNAMSIZ);
	if (ioctl(packet_socket, SIOCGIFHWADDR, &ifr) == -1) {
		err(1, "ioctl");
	}
	if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
		errx(1, "Not an Ethernet interface");
	}
	memcpy(own_mac, ifr.ifr_hwaddr.sa_data, sizeof(own_mac));

	// Print it out
	char own_mac_str[18];
	format_mac(own_mac, own_mac_str);
	fprintf(stderr, "%s\n", own_mac_str);

	// Start the event loop
	eventloop();

	// Close the socket (tho I'm not 100% sure it's needed)
	errno = 0;
	if (close(packet_socket) == -1) {
		err(1, "close");
	}

	// Flush stdout
	errno = 0;
	if (fflush(stdout) == EOF) {
		err(1, "fflush");
	}

	return 0;
}