1 files changed, 110 insertions, 0 deletions
diff --git a/protocol.c b/protocol.c
index a1cbf8e..4b4f714 100644
--- a/protocol.c
+++ b/protocol.c
@@ -20,6 +20,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <linux/if_ether.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <netinet/ether.h>
 #include "protocol.h"
 #include "config.h"
 
@@ -175,3 +178,110 @@ int parseControlPacket(unsigned char *data, const int data_len, struct mt_mactel
 	}
 }
 
+struct mt_mndp_packet *parseMNDP(const char *data, const int packetLen) {
+	static struct mt_mndp_packet packet;
+	unsigned short nameLen = 0;
+
+	/* Check for valid packet length */
+	if (packetLen < 18) {
+		return NULL;
+	}
+
+	/* Fetch length of Identifier string */
+	memcpy(&nameLen, data + 16,2);
+	nameLen = (nameLen >> 8) | ((nameLen & 0xff) << 8);
+
+	/* Enforce maximum name length */
+	nameLen = nameLen < MT_MNDP_MAX_IDENTITY_LENGTH ? nameLen : MT_MNDP_MAX_IDENTITY_LENGTH;
+
+	/* Read Identifier string */
+	memcpy(packet.identity, data + 18, nameLen);
+
+	/* Append zero */
+	packet.identity[nameLen] = 0;
+
+	/* Read source MAC address */
+	memcpy(packet.address, data + 8, ETH_ALEN);
+
+	return &packet;
+}
+
+int queryMNDP(const unsigned char *identity, unsigned char *mac) {
+	int fastlookup = 0;
+	int sock, length;
+	int optval = 1;
+	struct sockaddr_in si_me, si_remote;
+	unsigned char buff[MT_PACKET_LEN];
+	unsigned int message = 0;
+	struct timeval timeout;
+	time_t startTime;
+	fd_set read_fds;
+	struct mt_mndp_packet *packet;
+
+	startTime = time(0);
+
+	/* Open a UDP socket handle */
+	sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+
+	/* Set initialize address/port */
+	memset((char *) &si_me, 0, sizeof(si_me));
+	si_me.sin_family = AF_INET;
+	si_me.sin_port = htons(MT_MNDP_PORT);
+	si_me.sin_addr.s_addr = htonl(INADDR_ANY);
+
+	/* Bind to specified address/port */
+	if (bind(sock, (struct sockaddr *)&si_me, sizeof(si_me)) == -1) {
+		fprintf(stderr, "Error binding to %s:%d\n", inet_ntoa(si_me.sin_addr), MT_MNDP_PORT);
+		return 0;
+	}
+
+	/* Set the socket to allow sending broadcast packets */
+	setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &optval, sizeof (optval));
+
+	/* Request routers identify themselves */
+	memset((char *) &si_remote, 0, sizeof(si_remote));
+	si_remote.sin_family = AF_INET;
+	si_remote.sin_port = htons(MT_MNDP_PORT);
+	si_remote.sin_addr.s_addr = htonl(INADDR_BROADCAST);
+
+	if (sendto(sock, &message, sizeof (message), 0, (struct sockaddr *)&si_remote, sizeof(si_remote)) == -1) {
+		fprintf(stderr, "Unable to send broadcast packet: Router lookup will be slow\n");
+		fastlookup = 0;
+	} else {
+		fastlookup = 1;
+	}
+
+	while (1) {
+		/* Timeout, in case we receive a lot of packets, but from the wrong routers */
+		if (time(0) - startTime > (fastlookup ? MT_MNDP_TIMEOUT : MT_MNDP_LONGTIMEOUT)) {
+			return 0;
+		}
+
+		FD_ZERO(&read_fds);
+		FD_SET(sock, &read_fds);
+
+		timeout.tv_sec = fastlookup ? MT_MNDP_TIMEOUT : MT_MNDP_LONGTIMEOUT;
+		timeout.tv_usec = 0;
+	
+		select(sock + 1, &read_fds, NULL, NULL, &timeout);
+		if (!FD_ISSET(sock, &read_fds)) {
+			return 0;
+		}
+
+		/* Read UDP packet */
+		length = recvfrom(sock, buff, MT_PACKET_LEN, 0, 0, 0);
+		if (length < 0) {
+			return 0;
+		}
+
+		/* Parse MNDP packet */
+		packet = parseMNDP(buff, length);
+
+		if (packet != NULL) {
+			if (strcasecmp(identity, packet->identity) == 0) {
+				memcpy(mac, packet->address, ETH_ALEN);
+				return 1;
+			}
+		}
+	}
+}