Start work on status messages for Ethermess

ethermess.c

@@ -19,11 +19,25 @@
 #include <sys/types.h>
 #include <unistd.h>
 
+#define EM_PROTOCOL_VERSION 0
+
+#define EMT_SPEAK_VERSION 0
+#define EMT_STATUS_REQUEST 1
+#define EMT_STATUS 2
+
+#define EMS_AVAILABLE 0
+#define EMS_UNAVAILABLE 1
+
 bool running = true;
 
 int packet_socket;
 
 unsigned char own_mac[6];
+unsigned char broadcast_mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+unsigned char own_status = EMS_AVAILABLE;
+unsigned char own_nick[256] = {'n', 'o', 'r', 't', 't', 'i'};
+unsigned char own_nick_length = 6;
 
 char hexify(int nybble) {
 	assert(0 <= nybble && nybble <= 16);
@@ -54,6 +68,67 @@ void drop_privileges(void) {
 	}
 }
 
+void send_frame(const unsigned char *frame, size_t frame_length) {
+	errno = 0;
+	if (write(packet_socket, frame, frame_length) == -1) {
+		err(1, "write");
+	}
+}
+
+void write_headers(unsigned char frame[14], const unsigned char destination_mac[6], unsigned char packet_type) {
+	// Destination MAC
+	memcpy(&frame[0], destination_mac, 6);
+
+	// Source MAC
+	memcpy(&frame[6], own_mac, 6);
+
+	// EtherType
+	frame[12] = 0xda;
+	frame[13] = 0x7a;
+
+	// Ethermess version
+	frame[14] = EM_PROTOCOL_VERSION;
+
+	// Ethermess packet type
+	frame[15] = packet_type;
+}
+
+void send_speak_version(const unsigned char destination[6]) {
+	unsigned char frame[14 + 2 + 1];
+
+	write_headers(frame, destination, EMT_SPEAK_VERSION);
+
+	// Version to speak
+	frame[16] = EM_PROTOCOL_VERSION;
+
+	send_frame(frame, sizeof(frame));
+}
+
+void send_status_request(const unsigned char destination[6]) {
+	unsigned char frame[14 + 2];
+
+	write_headers(frame, destination, EMT_STATUS_REQUEST);
+
+	send_frame(frame, sizeof(frame));
+}
+
+void send_status(const unsigned char destination[6]) {
+	unsigned char frame[14 + 2 + 1 + 1 + own_nick_length];
+
+	write_headers(frame, destination, EMT_STATUS);
+
+	// Status
+	frame[16] = own_status;
+
+	// Length of nick
+	frame[17] = own_nick_length;
+
+	// Nick
+	memcpy(&frame[18], own_nick, own_nick_length);
+
+	send_frame(frame, sizeof(frame));
+}
+
 void read_command(void) {
 	int cmd = getchar();
 	if (cmd == EOF) {
@@ -62,6 +137,8 @@ void read_command(void) {
 
 	if (cmd == 'q') {
 		running = false;
+	} else if (cmd == 's') {
+		send_status_request(broadcast_mac);
 	} else if (cmd == '\n') {
 		// Ignore
 	} else {
@@ -69,25 +146,124 @@ void read_command(void) {
 	}
 }
 
-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");
+void handle_status(const unsigned char source_mac[6], const unsigned char *data, size_t data_length) {
+	if (data_length < 2) {
+		// Too short
+		fprintf(stderr, "Data too short: %zu\n", data_length); // debg
+		return;
 	}
 
-	if (memcmp(frame, own_mac, sizeof(own_mac)) == 0) {
+	unsigned char status = data[0];
+
+	if (status != EMS_AVAILABLE && status != EMS_UNAVAILABLE) {
+		// Unknown status, throw away
+		fprintf(stderr, "Unknown status %u\n", status); // debg
+		return;
+	}
+
+	unsigned char nick_length = data[1];
+
+	if (nick_length > data_length - 2) {
+		// Malformed length field
+		fprintf(stderr, "Nick length %u, remaining packet length %zu\n", nick_length, data_length); // debg
+		return;
+	}
+
+	unsigned char nick[256];
+	memcpy(nick, &data[2], nick_length);
+
+	char mac[18];
+	format_mac(source_mac, mac);
+
+	errno = 0;
+	if (printf("%s status: ", mac) == -1) {
+		err(1, "printf");
+	}
+
+	if (status == EMS_UNAVAILABLE) {
+		errno = 0;
+		if (printf("(unavailable) ") == -1) {
+			err(1, "printf");
+		}
+	}
+
+	for (size_t i = 0; i < (size_t)nick_length; i++) {
+		errno = 0;
+		if (putchar(nick[i]) == EOF) {
+			err(1, "putchar");
+		}
+	}
+
+	errno = 0;
+	if (putchar('\n') == EOF) {
+		err(1, "putchar");
+	}
+
+	errno = 0;
+	if (fflush(stdout) == EOF) {
+		err(1, "fflush");
+	}
+}
+
+void process_frame(void) {
+	unsigned char frame[1518]; // Largest a 802.3 frame can be without FCS
+
+	errno = 0;
+	ssize_t res = recv(packet_socket, frame, sizeof(frame), 0);
+	if (res == -1) {
+		errx(1, "recv");
+	} else if (res < 16) {
+		// Frame too short to contain enough information
+		return;
+	}
+	size_t packet_length = (size_t)res;
+
+	// Check that the packet is Ethermess (EtherType DA7A)
+	if (frame[12] != 0xda || frame[13] != 0x7a) {
+		return;
+	}
+
+	if (memcmp(frame, own_mac, 6) == 0) {
 		// Targetted at us
 		fprintf(stderr, "."); // debg
-	} else if (memcmp(frame, broadcast_addr, sizeof(broadcast_addr)) == 0) {
+	} else if (memcmp(frame, broadcast_mac, 6) == 0) {
 		// Broadcast
 		fprintf(stderr, "^"); // debg
 	} else {
-		// No concern
+		// Does not concern us
 		return;
 	}
+
+	// Extract source MAC
+	unsigned char source_mac[6];
+	memcpy(source_mac, &frame[6], sizeof(source_mac));
+
+	// Extract version
+	unsigned char version = frame[14];
+
+	// If they speak a version we don't understand, tell them to speak ours
+	if (version > EM_PROTOCOL_VERSION) {
+		fprintf(stderr, "Protocol version mismatch: %u\n", version); // debg
+		send_speak_version(source_mac);
+		return;
+	}
+
+	// Extract Ethermess packet type
+	unsigned char packet_type = frame[15];
+
+	// Process the packet based on the packet type
+	switch (packet_type) {
+		case EMT_STATUS_REQUEST:
+			send_status(source_mac);
+			break;
+
+		case EMT_STATUS:
+			handle_status(source_mac, &frame[16], packet_length - 16);
+			break;
+
+		default:
+			fprintf(stderr, "Ignoring packet of type %i\n", packet_type);
+	}
 }
 
 void eventloop(void) {
@@ -191,6 +367,9 @@ int main(int argc, char **argv) {
 	format_mac(own_mac, own_mac_str);
 	fprintf(stderr, "%s\n", own_mac_str);
 
+	// Request status from everyone, so that we can get an idea of who is on the network
+	send_status_request(broadcast_mac);
+
 	// Start the event loop
 	eventloop();