Merge remote branch 'origin/master' into mactelnet-osx

Conflicts:
	mactelnet.c
	mactelnetd.c
	protocol.c

12 files changed, 771 insertions, 116 deletions

diff --git a/Makefile b/Makefile
index f3113d3..0115175 100644
--- a/Makefile
+++ b/Makefile
@@ -14,11 +14,11 @@ install: all
 	chown $(DESTDIR)/etc/mactelnetd.users
 	chmod 600 $(DESTDIR)/etc/mactelnetd.users
 
-mactelnet: config.h udp.h udp.c mactelnet.c protocol.c protocol.h console.c console.h devices.c devices.h
-	gcc -g -DUSERSFILE='"/etc/mactelnetd.users"' -o mactelnet -lcrypto mactelnet.c udp.c protocol.c console.c devices.c
+mactelnet: config.h udp.h udp.c mactelnet.c protocol.c protocol.h console.c console.h devices.c devices.h md5.h
+	gcc -g -DUSERSFILE='"/etc/mactelnetd.users"' -o mactelnet mactelnet.c udp.c protocol.c console.c devices.c md5.c
 
-mactelnetd: config.h mactelnetd.c udp.h udp.c protocol.c protocol.h console.c console.h users.c users.h
-	gcc -g -DUSERSFILE='"/etc/mactelnetd.users"' -o mactelnetd -lcrypto mactelnetd.c udp.c protocol.c console.c users.c
+mactelnetd: config.h mactelnetd.c udp.h udp.c protocol.c protocol.h console.c console.h users.c users.h md5.h
+	gcc -g -DUSERSFILE='"/etc/mactelnetd.users"' -o mactelnetd mactelnetd.c udp.c protocol.c console.c users.c md5.c
 
 mndp: config.h mndp.c protocol.h
-	gcc -g -o mndp mndp.c
+	gcc -g -o mndp mndp.c protocol.c
diff --git a/README.markdown b/README.markdown
index f6c0e51..3dc7f0e 100644
--- a/README.markdown
+++ b/README.markdown
@@ -4,16 +4,11 @@ MAC-Telnet for Linux
 A linux console tool for connecting to MikroTik RouterOS devices via their
 ethernet address.
 
-NB: Alpha stage code. Do not expect to run flawlessy.
+NB: Beta stage code. Do not expect to run flawlessy.
 
 Installation
 ------------
 
-Make sure you have installed openssl development files.
-
-For Ubuntu users:
-    sudo apt-get install libssl-dev
-
 Then download source tarball, extract, compile and install:
 
     wget http://github.com/haakonnessjoen/MAC-Telnet/tarball/master
@@ -26,6 +21,21 @@ Now you're ready.
 Usage
 -----
 
+    # mactelnet -h
+    Usage: ./mactelnet <ifname> <MAC|identity> [-h] [-n] [-u <username>] [-p <password>]
+    
+    Parameters:
+      ifname    Network interface that the RouterOS resides on. (example: eth0)
+      MAC       MAC-Address of the RouterOS device. Use mndp to discover them.
+      identity  The identity/name of your RouterOS device. Uses MNDP protocol to find it..
+      -n        Do not use broadcast packets. Less insecure but requires root privileges.
+      -u        Specify username on command line.
+      -p        Specify password on command line.
+      -h        This help.
+
+
+    Example:
+
     $ ./mactelnet eth0 0:c:42:43:58:a5 -u admin
     Password: 
     Connecting to 0:c:42:43:58:a5...done
diff --git a/debian/README b/debian/README
index f6c0e51..3dc7f0e 100644
--- a/debian/README
+++ b/debian/README
@@ -4,16 +4,11 @@ MAC-Telnet for Linux
 A linux console tool for connecting to MikroTik RouterOS devices via their
 ethernet address.
 
-NB: Alpha stage code. Do not expect to run flawlessy.
+NB: Beta stage code. Do not expect to run flawlessy.
 
 Installation
 ------------
 
-Make sure you have installed openssl development files.
-
-For Ubuntu users:
-    sudo apt-get install libssl-dev
-
 Then download source tarball, extract, compile and install:
 
     wget http://github.com/haakonnessjoen/MAC-Telnet/tarball/master
@@ -26,6 +21,21 @@ Now you're ready.
 Usage
 -----
 
+    # mactelnet -h
+    Usage: ./mactelnet <ifname> <MAC|identity> [-h] [-n] [-u <username>] [-p <password>]
+    
+    Parameters:
+      ifname    Network interface that the RouterOS resides on. (example: eth0)
+      MAC       MAC-Address of the RouterOS device. Use mndp to discover them.
+      identity  The identity/name of your RouterOS device. Uses MNDP protocol to find it..
+      -n        Do not use broadcast packets. Less insecure but requires root privileges.
+      -u        Specify username on command line.
+      -p        Specify password on command line.
+      -h        This help.
+
+
+    Example:
+
     $ ./mactelnet eth0 0:c:42:43:58:a5 -u admin
     Password: 
     Connecting to 0:c:42:43:58:a5...done
diff --git a/debian/changelog b/debian/changelog
index 9b3ecfe..57c442f 100644
--- a/debian/changelog
+++ b/debian/changelog
@@ -1,3 +1,8 @@
+mactelnet (0.1-1ubuntu1) unstable; urgency=low
+
+  * Removed libssl dependency
+
+ -- Haakon Nessjoen <haakon.nessjoen@gmail.com>  Mon, 01 Nov 2010 23:01:49 +0200
 mactelnet (0.1-0ubuntu1) unstable; urgency=low
 
   * Initial Release.
diff --git a/debian/control b/debian/control
index 04cf8f5..9cac6cb 100644
--- a/debian/control
+++ b/debian/control
@@ -2,13 +2,13 @@ Source: mactelnet
 Section: net
 Priority: extra
 Maintainer: Haakon Nessjoen <haakon.nessjoen@gmail.com>
-Build-Depends: debhelper (>= 7), linux-libc-dev (>= 2.6), libssl-dev (>= 0.9.8g-9) 
+Build-Depends: debhelper (>= 7), linux-libc-dev (>= 2.6) 
 Standards-Version: 3.8.0
 Homepage: http://github.com/haakonnessjoen/MAC-Telnet/
 
 Package: mactelnet
 Architecture: any
-Depends: libssl0.9.8 (>= 0.9.8g-9), libc6 (>= 2.9) 
+Depends: libc6 (>= 2.9) 
 Description: MAC-Telnet client for Linux
  A linux console tool for connecting to MikroTik RouterOS devices via their ethernet address.
 
diff --git a/mactelnet.c b/mactelnet.c
index 2b05431..d61ea05 100644
--- a/mactelnet.c
+++ b/mactelnet.c
@@ -36,7 +36,7 @@
 #define ETH_ALEN 6
 #include <net/ethernet.h>
 #endif
-#include <openssl/md5.h>
+#include "md5.h"
 #include "protocol.h"
 #include "udp.h"
 #include "console.h"
@@ -96,7 +96,7 @@ void sendAuthData(unsigned char *username, unsigned char *password) {
 	int result;
 	int plen;
 	int databytes;
-	MD5_CTX c;
+	md5_state_t state;
 
 	/* Concat string of 0 + password + encryptionkey */
 	md5data[0] = 0;
@@ -105,9 +105,9 @@ void sendAuthData(unsigned char *username, unsigned char *password) {
 	memcpy(md5data + 1 + strlen(password), encryptionkey, 16);
 
 	/* Generate md5 sum of md5data with a leading 0 */
-	MD5_Init(&c);
-	MD5_Update(&c, md5data, strlen(password) + 17);
-	MD5_Final(md5sum + 1, &c);
+	md5_init(&state);
+	md5_append(&state, (const md5_byte_t *)md5data, strlen(password) + 17);
+	md5_finish(&state, (md5_byte_t *)md5sum + 1);
 	md5sum[0] = 0;
 
 	/* Send combined packet to server */
@@ -291,12 +291,13 @@ int main (int argc, char **argv) {
 		}
 	}
 	if (argc - optind < 2 || printHelp) {
-		fprintf(stderr, "Usage: %s <ifname> <MAC> [-h] [-n] [-u <username>] [-p <password>]\n", argv[0]);
+		fprintf(stderr, "Usage: %s <ifname> <MAC|identity> [-h] [-n] [-u <username>] [-p <password>]\n", argv[0]);
 
 		if (printHelp) {
 			fprintf(stderr, "\nParameters:\n");
 			fprintf(stderr, "  ifname    Network interface that the RouterOS resides on. (example: eth0)\n");
 			fprintf(stderr, "  MAC       MAC-Address of the RouterOS device. Use mndp to discover them.\n");
+			fprintf(stderr, "  identity  The identity/name of your RouterOS device. Uses MNDP protocol to find it.\n");
 #ifndef __APPLE_CC__
 			fprintf(stderr, "  -n        Do not use broadcast packets. Less insecure but requires root privileges.\n");
 #endif
@@ -312,14 +313,6 @@ int main (int argc, char **argv) {
 	strncpy(devicename, argv[optind++], sizeof(devicename) - 1);
 	devicename[sizeof(devicename) - 1] = '\0';
 
-	/* Convert mac address string to ether_addr struct */
-	tmpaddr = ether_aton(argv[optind]);
-	if (tmpaddr == NULL) {
-		fprintf(stderr, "Invalid MAC address\n");
-		exit(1);
-	}
-	memcpy(dstmac, tmpaddr, sizeof(struct ether_addr));
-
 	/* Seed randomizer */
 	srand(time(NULL));
 
@@ -380,6 +373,39 @@ int main (int argc, char **argv) {
 		return 1;
 	}
 
+	/* Check for identity name or mac address */
+	{
+		unsigned char *p = argv[optind];
+		int colons = 0;
+		while (*p++) {
+			if (*p == ':') {
+				colons++;
+			}
+		}
+
+		if (colons != 5) {
+			fprintf(stderr, "Searching for '%s'...", argv[optind]);
+
+			/* Search for Router by identity name, using MNDP */
+			if (!queryMNDP(argv[optind], dstmac)) {
+				fprintf(stderr, "not found.\n");
+				return 1;
+			}
+
+			/* Router found, display mac and continue */
+			fprintf(stderr, "%s\n", ether_ntoa((struct ether_addr *)dstmac));
+
+		} else {
+			/* Convert mac address string to ether_addr struct */
+			tmpaddr = ether_aton(argv[optind]);
+			if (tmpaddr == NULL) {
+				fprintf(stderr, "Invalid MAC address\n");
+				exit(1);
+			}
+			memcpy(dstmac, tmpaddr, sizeof(struct ether_addr));
+		}
+	}
+
 	if (!haveUsername) {
 		int ret=0;
 		printf("Login: ");
@@ -482,10 +508,10 @@ int main (int argc, char **argv) {
 			}
 			/* Handle data from keyboard/local terminal */
 			if (FD_ISSET(0, &read_fds)) {
-				unsigned char keydata[100];
+				unsigned char keydata[512];
 				int datalen;
 
-				datalen = read(STDIN_FILENO, &keydata, 100);
+				datalen = read(STDIN_FILENO, &keydata, 512);
 
 				plen = initPacket(&data, MT_PTYPE_DATA, srcmac, dstmac, sessionkey, outcounter);
 				plen += addControlPacket(&data, MT_CPTYPE_PLAINDATA, &keydata, datalen);
diff --git a/mactelnetd.c b/mactelnetd.c
index afe5a94..1ed68ca 100644
--- a/mactelnetd.c
+++ b/mactelnetd.c
@@ -33,10 +33,13 @@
 #include <linux/if_ether.h>
 #include <netinet/in.h>
 #include <netinet/ether.h>
+#else
+#define ETH_ALEN 6
 #endif
-#include <openssl/md5.h>
 #include <pwd.h>
 #include <sys/ioctl.h>
+#include <pwd.h>
+#include "md5.h"
 #include "protocol.h"
 #include "udp.h"
 #include "console.h"
@@ -85,6 +88,8 @@ struct mt_connection {
 	unsigned short terminal_height;
 	unsigned char terminal_type[30];
 
+	int waitForAck;
+
 	struct mt_connection *next;
 };
 
@@ -197,6 +202,11 @@ void handlePacket(unsigned char *data, int data_len, const struct sockaddr_in *a
 			curconn = findConnection(pkthdr.seskey, (unsigned char *)&(pkthdr.srcaddr));
 			if (curconn != NULL) {
 				curconn->lastdata = time(NULL);
+
+				if (pkthdr.counter <= curconn->outcounter) {
+					curconn->waitForAck = 0;
+				}
+
 				if (pkthdr.counter == curconn->outcounter) {
 					// Answer to anti-timeout packet
 					initPacket(&pdata, MT_PTYPE_ACK, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, pkthdr.counter);
@@ -219,6 +229,15 @@ void handlePacket(unsigned char *data, int data_len, const struct sockaddr_in *a
 				initPacket(&pdata, MT_PTYPE_ACK, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, pkthdr.counter + (data_len - MT_HEADER_LEN));
 				sendUDP(curconn, &pdata);
 
+				/* Accept first packet, and all packets greater than incounter, and if counter has
+				wrapped around. */
+				if (curconn->incounter == 0 || pkthdr.counter > curconn->incounter || (curconn->incounter - pkthdr.counter) > 65535) {
+					curconn->incounter = pkthdr.counter;
+				} else {
+					/* Ignore double or old packets */
+					return;
+				}
+
 				/* Calculate how much more there is in the packet */
 				rest = data_len - MT_HEADER_LEN;
 				p += MT_HEADER_LEN;
@@ -232,8 +251,6 @@ void handlePacket(unsigned char *data, int data_len, const struct sockaddr_in *a
 					p += read;
 					rest -= read;
 
-					//read = parseControlPacket(data+22, data_len-22, &cpkt);
-
 					if (cpkt.cptype == MT_CPTYPE_BEGINAUTH) {
 						int plen,i;
 						for (i = 0; i < 16; ++i) {
@@ -241,7 +258,7 @@ void handlePacket(unsigned char *data, int data_len, const struct sockaddr_in *a
 						}
 						initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
 						plen = addControlPacket(&pdata, MT_CPTYPE_ENCRYPTIONKEY, (curconn->enckey), 16);
-						curconn->outcounter = plen;
+						curconn->outcounter += plen;
 						sendUDP(curconn, &pdata);
 						memset(trypassword, 0, sizeof(trypassword));
 
@@ -283,54 +300,54 @@ void handlePacket(unsigned char *data, int data_len, const struct sockaddr_in *a
 					}
 				}
 				if (doLogin) {
-						int plen = 0;
-						unsigned char md5sum[17];
-						unsigned char md5data[100];
-						struct mt_credentials *user;
-
-						readUserfile();
-
-						if ((user = findUser(curconn->username)) != NULL) {
-							MD5_CTX c;
-							/* Concat string of 0 + password + encryptionkey */
-							md5data[0] = 0;
-							strncpy(md5data + 1, user->password, 82);
-							memcpy(md5data + 1 + strlen(user->password), curconn->enckey, 16);
-
-							/* Generate md5 sum of md5data with a leading 0 */
-							MD5_Init(&c);
-							MD5_Update(&c, md5data, strlen(user->password) + 17);
-							MD5_Final(md5sum + 1, &c);
-							md5sum[0] = 0;
+					int plen = 0;
+					unsigned char md5sum[17];
+					unsigned char md5data[100];
+					struct mt_credentials *user;
+
+					readUserfile();
+
+					if ((user = findUser(curconn->username)) != NULL) {
+						md5_state_t state;
+						/* Concat string of 0 + password + encryptionkey */
+						md5data[0] = 0;
+						strncpy(md5data + 1, user->password, 82);
+						memcpy(md5data + 1 + strlen(user->password), curconn->enckey, 16);
+
+						/* Generate md5 sum of md5data with a leading 0 */
+						md5_init(&state);
+						md5_append(&state, (const md5_byte_t *)md5data, strlen(user->password) + 17);
+						md5_finish(&state, (md5_byte_t *)md5sum + 1);
+						md5sum[0] = 0;
 
-							initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
-							plen = addControlPacket(&pdata, MT_CPTYPE_END_AUTH, NULL, 0);
-							curconn->outcounter += plen;
-							sendUDP(curconn, &pdata);
-
-							if (curconn->state == STATE_ACTIVE)
-								return;
-						} else {
-							doLogin = 0;
-						}
+						initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
+						plen = addControlPacket(&pdata, MT_CPTYPE_END_AUTH, NULL, 0);
+						curconn->outcounter += plen;
+						sendUDP(curconn, &pdata);
 
-						if (doLogin == 1 && memcmp(md5sum, trypassword, 17) == 0) {
-							initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
-							plen = addControlPacket(&pdata, MT_CPTYPE_PLAINDATA, "Login OK!\r\n", 11);
-							sendUDP(curconn, &pdata);
-							curconn->outcounter += plen;
-							curconn->state = STATE_ACTIVE;
-							curconn->terminalMode = 1;
-						} else {
-							initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
-							plen = addControlPacket(&pdata, MT_CPTYPE_PLAINDATA, "Login FAILED!\r\n", 15);
-							sendUDP(curconn, &pdata);
-							curconn->outcounter += plen;
-							curconn->state = STATE_CLOSED;
-							initPacket(&pdata, MT_PTYPE_END, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
-							sendUDP(curconn, &pdata);
-							/* TODO: should wait some time (not with sleep) before returning, to minimalize brute force attacks */
+						if (curconn->state == STATE_ACTIVE)
 							return;
+					} else {
+						doLogin = 0;
+					}
+
+					if (doLogin == 1 && memcmp(md5sum, trypassword, 17) == 0) {
+						initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
+						plen = addControlPacket(&pdata, MT_CPTYPE_PLAINDATA, "Login OK!\r\n", 11);
+						sendUDP(curconn, &pdata);
+						curconn->outcounter += plen;
+						curconn->state = STATE_ACTIVE;
+						curconn->terminalMode = 1;
+					} else {
+						initPacket(&pdata, MT_PTYPE_DATA, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
+						plen = addControlPacket(&pdata, MT_CPTYPE_PLAINDATA, "Login FAILED!\r\n", 15);
+						sendUDP(curconn, &pdata);
+						curconn->outcounter += plen;
+						curconn->state = STATE_CLOSED;
+						initPacket(&pdata, MT_PTYPE_END, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, curconn->outcounter);
+						sendUDP(curconn, &pdata);
+						/* TODO: should wait some time (not with sleep) before returning, to minimalize brute force attacks */
+						return;
 					}
 
 					char *slavename;
@@ -403,6 +420,9 @@ void handlePacket(unsigned char *data, int data_len, const struct sockaddr_in *a
 			initPacket(&pdata, MT_PTYPE_ACK, pkthdr.dstaddr, pkthdr.srcaddr, pkthdr.seskey, pkthdr.counter);
 			sendUDP(curconn, &pdata);
 	}
+	if (curconn != NULL) {
+		printf("Packet, incounter %d, outcounter %d\n", curconn->incounter, curconn->outcounter);
+	}
 }
 
 /*
@@ -480,15 +500,15 @@ int main (int argc, char **argv) {
 
 		/* Add active connections to select queue */
 		for (p = connections_head; p != NULL; p = p->next) {
-			if (p->state == STATE_ACTIVE && p->ptsfd > 0) {
+			if (p->state == STATE_ACTIVE && p->waitForAck == 0 && p->ptsfd > 0) {
 				FD_SET(p->ptsfd, &read_fds);
 				if (p->ptsfd > maxfd)
 					maxfd = p->ptsfd;
 			}
 		}
 
-		timeout.tv_sec = 1;
-		timeout.tv_usec = 0;
+		timeout.tv_sec = 0;
+		timeout.tv_usec = 100000;
 
 		/* Wait for data or timeout */
 		reads = select(maxfd+1, &read_fds, NULL, NULL, &timeout);
@@ -504,17 +524,18 @@ int main (int argc, char **argv) {
 
 			for (p = connections_head; p != NULL; p = p->next) {
 				/* Check if we have data ready in the pty buffer for the active session */
-				if (p->state == STATE_ACTIVE && p->ptsfd > 0 && FD_ISSET(p->ptsfd, &read_fds)) {
-					unsigned char keydata[100];
+				if (p->state == STATE_ACTIVE && p->ptsfd > 0 && p->waitForAck == 0 && FD_ISSET(p->ptsfd, &read_fds)) {
+					unsigned char keydata[1024];
 					int datalen,plen;
 
 					/* Read it */
-					datalen = read(p->ptsfd, &keydata, 100);
+					datalen = read(p->ptsfd, &keydata, 1024);
 					if (datalen != -1) {
 						/* Send it */
 						initPacket(&pdata, MT_PTYPE_DATA, p->dstmac, p->srcmac, p->seskey, p->outcounter);
 						plen = addControlPacket(&pdata, MT_CPTYPE_PLAINDATA, &keydata, datalen);
 						p->outcounter += plen;
+						p->waitForAck = 1;
 						result = sendUDP(p, &pdata);
 					} else {
 						/* Bash exited */
@@ -524,6 +545,9 @@ int main (int argc, char **argv) {
 						removeConnection(p);
 					}
 				}
+				else if (p->state == STATE_ACTIVE && p->ptsfd > 0 && p->waitForAck == 1 && FD_ISSET(p->ptsfd, &read_fds)) {
+					printf("Waiting for ack on %d\n", p->seskey);
+				}
 			}
 		/* Handle select() timeout */
 		} else {
diff --git a/md5.c b/md5.c
new file mode 100644
index 0000000..c35d96c
--- /dev/null
+++ b/md5.c
@@ -0,0 +1,381 @@
+/*
+  Copyright (C) 1999, 2000, 2002 Aladdin Enterprises.  All rights reserved.
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+
+  L. Peter Deutsch
+  ghost@aladdin.com
+
+ */
+/* $Id: md5.c,v 1.6 2002/04/13 19:20:28 lpd Exp $ */
+/*
+  Independent implementation of MD5 (RFC 1321).
+
+  This code implements the MD5 Algorithm defined in RFC 1321, whose
+  text is available at
+	http://www.ietf.org/rfc/rfc1321.txt
+  The code is derived from the text of the RFC, including the test suite
+  (section A.5) but excluding the rest of Appendix A.  It does not include
+  any code or documentation that is identified in the RFC as being
+  copyrighted.
+
+  The original and principal author of md5.c is L. Peter Deutsch
+  <ghost@aladdin.com>.  Other authors are noted in the change history
+  that follows (in reverse chronological order):
+
+  2002-04-13 lpd Clarified derivation from RFC 1321; now handles byte order
+	either statically or dynamically; added missing #include <string.h>
+	in library.
+  2002-03-11 lpd Corrected argument list for main(), and added int return
+	type, in test program and T value program.
+  2002-02-21 lpd Added missing #include <stdio.h> in test program.
+  2000-07-03 lpd Patched to eliminate warnings about "constant is
+	unsigned in ANSI C, signed in traditional"; made test program
+	self-checking.
+  1999-11-04 lpd Edited comments slightly for automatic TOC extraction.
+  1999-10-18 lpd Fixed typo in header comment (ansi2knr rather than md5).
+  1999-05-03 lpd Original version.
+ */
+
+#include "md5.h"
+#include <string.h>
+
+#undef BYTE_ORDER	/* 1 = big-endian, -1 = little-endian, 0 = unknown */
+#ifdef ARCH_IS_BIG_ENDIAN
+#  define BYTE_ORDER (ARCH_IS_BIG_ENDIAN ? 1 : -1)
+#else
+#  define BYTE_ORDER 0
+#endif
+
+#define T_MASK ((md5_word_t)~0)
+#define T1 /* 0xd76aa478 */ (T_MASK ^ 0x28955b87)
+#define T2 /* 0xe8c7b756 */ (T_MASK ^ 0x173848a9)
+#define T3    0x242070db
+#define T4 /* 0xc1bdceee */ (T_MASK ^ 0x3e423111)
+#define T5 /* 0xf57c0faf */ (T_MASK ^ 0x0a83f050)
+#define T6    0x4787c62a
+#define T7 /* 0xa8304613 */ (T_MASK ^ 0x57cfb9ec)
+#define T8 /* 0xfd469501 */ (T_MASK ^ 0x02b96afe)
+#define T9    0x698098d8
+#define T10 /* 0x8b44f7af */ (T_MASK ^ 0x74bb0850)
+#define T11 /* 0xffff5bb1 */ (T_MASK ^ 0x0000a44e)
+#define T12 /* 0x895cd7be */ (T_MASK ^ 0x76a32841)
+#define T13    0x6b901122
+#define T14 /* 0xfd987193 */ (T_MASK ^ 0x02678e6c)
+#define T15 /* 0xa679438e */ (T_MASK ^ 0x5986bc71)
+#define T16    0x49b40821
+#define T17 /* 0xf61e2562 */ (T_MASK ^ 0x09e1da9d)
+#define T18 /* 0xc040b340 */ (T_MASK ^ 0x3fbf4cbf)
+#define T19    0x265e5a51
+#define T20 /* 0xe9b6c7aa */ (T_MASK ^ 0x16493855)
+#define T21 /* 0xd62f105d */ (T_MASK ^ 0x29d0efa2)
+#define T22    0x02441453
+#define T23 /* 0xd8a1e681 */ (T_MASK ^ 0x275e197e)
+#define T24 /* 0xe7d3fbc8 */ (T_MASK ^ 0x182c0437)
+#define T25    0x21e1cde6
+#define T26 /* 0xc33707d6 */ (T_MASK ^ 0x3cc8f829)
+#define T27 /* 0xf4d50d87 */ (T_MASK ^ 0x0b2af278)
+#define T28    0x455a14ed
+#define T29 /* 0xa9e3e905 */ (T_MASK ^ 0x561c16fa)
+#define T30 /* 0xfcefa3f8 */ (T_MASK ^ 0x03105c07)
+#define T31    0x676f02d9
+#define T32 /* 0x8d2a4c8a */ (T_MASK ^ 0x72d5b375)
+#define T33 /* 0xfffa3942 */ (T_MASK ^ 0x0005c6bd)
+#define T34 /* 0x8771f681 */ (T_MASK ^ 0x788e097e)
+#define T35    0x6d9d6122
+#define T36 /* 0xfde5380c */ (T_MASK ^ 0x021ac7f3)
+#define T37 /* 0xa4beea44 */ (T_MASK ^ 0x5b4115bb)
+#define T38    0x4bdecfa9
+#define T39 /* 0xf6bb4b60 */ (T_MASK ^ 0x0944b49f)
+#define T40 /* 0xbebfbc70 */ (T_MASK ^ 0x4140438f)
+#define T41    0x289b7ec6
+#define T42 /* 0xeaa127fa */ (T_MASK ^ 0x155ed805)
+#define T43 /* 0xd4ef3085 */ (T_MASK ^ 0x2b10cf7a)
+#define T44    0x04881d05
+#define T45 /* 0xd9d4d039 */ (T_MASK ^ 0x262b2fc6)
+#define T46 /* 0xe6db99e5 */ (T_MASK ^ 0x1924661a)
+#define T47    0x1fa27cf8
+#define T48 /* 0xc4ac5665 */ (T_MASK ^ 0x3b53a99a)
+#define T49 /* 0xf4292244 */ (T_MASK ^ 0x0bd6ddbb)
+#define T50    0x432aff97
+#define T51 /* 0xab9423a7 */ (T_MASK ^ 0x546bdc58)
+#define T52 /* 0xfc93a039 */ (T_MASK ^ 0x036c5fc6)
+#define T53    0x655b59c3
+#define T54 /* 0x8f0ccc92 */ (T_MASK ^ 0x70f3336d)
+#define T55 /* 0xffeff47d */ (T_MASK ^ 0x00100b82)
+#define T56 /* 0x85845dd1 */ (T_MASK ^ 0x7a7ba22e)
+#define T57    0x6fa87e4f
+#define T58 /* 0xfe2ce6e0 */ (T_MASK ^ 0x01d3191f)
+#define T59 /* 0xa3014314 */ (T_MASK ^ 0x5cfebceb)
+#define T60    0x4e0811a1
+#define T61 /* 0xf7537e82 */ (T_MASK ^ 0x08ac817d)
+#define T62 /* 0xbd3af235 */ (T_MASK ^ 0x42c50dca)
+#define T63    0x2ad7d2bb
+#define T64 /* 0xeb86d391 */ (T_MASK ^ 0x14792c6e)
+
+
+static void
+md5_process(md5_state_t *pms, const md5_byte_t *data /*[64]*/)
+{
+    md5_word_t
+	a = pms->abcd[0], b = pms->abcd[1],
+	c = pms->abcd[2], d = pms->abcd[3];
+    md5_word_t t;
+#if BYTE_ORDER > 0
+    /* Define storage only for big-endian CPUs. */
+    md5_word_t X[16];
+#else
+    /* Define storage for little-endian or both types of CPUs. */
+    md5_word_t xbuf[16];
+    const md5_word_t *X;
+#endif
+
+    {
+#if BYTE_ORDER == 0
+	/*
+	 * Determine dynamically whether this is a big-endian or
+	 * little-endian machine, since we can use a more efficient
+	 * algorithm on the latter.
+	 */
+	static const int w = 1;
+
+	if (*((const md5_byte_t *)&w)) /* dynamic little-endian */
+#endif
+#if BYTE_ORDER <= 0		/* little-endian */
+	{
+	    /*
+	     * On little-endian machines, we can process properly aligned
+	     * data without copying it.
+	     */
+	    if (!((data - (const md5_byte_t *)0) & 3)) {
+		/* data are properly aligned */
+		X = (const md5_word_t *)data;
+	    } else {
+		/* not aligned */
+		memcpy(xbuf, data, 64);
+		X = xbuf;
+	    }
+	}
+#endif
+#if BYTE_ORDER == 0
+	else			/* dynamic big-endian */
+#endif
+#if BYTE_ORDER >= 0		/* big-endian */
+	{
+	    /*
+	     * On big-endian machines, we must arrange the bytes in the
+	     * right order.
+	     */
+	    const md5_byte_t *xp = data;
+	    int i;
+
+#  if BYTE_ORDER == 0
+	    X = xbuf;		/* (dynamic only) */
+#  else
+#    define xbuf X		/* (static only) */
+#  endif
+	    for (i = 0; i < 16; ++i, xp += 4)
+		xbuf[i] = xp[0] + (xp[1] << 8) + (xp[2] << 16) + (xp[3] << 24);
+	}
+#endif
+    }
+
+#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32 - (n))))
+
+    /* Round 1. */
+    /* Let [abcd k s i] denote the operation
+       a = b + ((a + F(b,c,d) + X[k] + T[i]) <<< s). */
+#define F(x, y, z) (((x) & (y)) | (~(x) & (z)))
+#define SET(a, b, c, d, k, s, Ti)\
+  t = a + F(b,c,d) + X[k] + Ti;\
+  a = ROTATE_LEFT(t, s) + b
+    /* Do the following 16 operations. */
+    SET(a, b, c, d,  0,  7,  T1);
+    SET(d, a, b, c,  1, 12,  T2);
+    SET(c, d, a, b,  2, 17,  T3);
+    SET(b, c, d, a,  3, 22,  T4);
+    SET(a, b, c, d,  4,  7,  T5);
+    SET(d, a, b, c,  5, 12,  T6);
+    SET(c, d, a, b,  6, 17,  T7);
+    SET(b, c, d, a,  7, 22,  T8);
+    SET(a, b, c, d,  8,  7,  T9);
+    SET(d, a, b, c,  9, 12, T10);
+    SET(c, d, a, b, 10, 17, T11);
+    SET(b, c, d, a, 11, 22, T12);
+    SET(a, b, c, d, 12,  7, T13);
+    SET(d, a, b, c, 13, 12, T14);
+    SET(c, d, a, b, 14, 17, T15);
+    SET(b, c, d, a, 15, 22, T16);
+#undef SET
+
+     /* Round 2. */
+     /* Let [abcd k s i] denote the operation
+          a = b + ((a + G(b,c,d) + X[k] + T[i]) <<< s). */
+#define G(x, y, z) (((x) & (z)) | ((y) & ~(z)))
+#define SET(a, b, c, d, k, s, Ti)\
+  t = a + G(b,c,d) + X[k] + Ti;\
+  a = ROTATE_LEFT(t, s) + b
+     /* Do the following 16 operations. */
+    SET(a, b, c, d,  1,  5, T17);
+    SET(d, a, b, c,  6,  9, T18);
+    SET(c, d, a, b, 11, 14, T19);
+    SET(b, c, d, a,  0, 20, T20);
+    SET(a, b, c, d,  5,  5, T21);
+    SET(d, a, b, c, 10,  9, T22);
+    SET(c, d, a, b, 15, 14, T23);
+    SET(b, c, d, a,  4, 20, T24);
+    SET(a, b, c, d,  9,  5, T25);
+    SET(d, a, b, c, 14,  9, T26);
+    SET(c, d, a, b,  3, 14, T27);
+    SET(b, c, d, a,  8, 20, T28);
+    SET(a, b, c, d, 13,  5, T29);
+    SET(d, a, b, c,  2,  9, T30);
+    SET(c, d, a, b,  7, 14, T31);
+    SET(b, c, d, a, 12, 20, T32);
+#undef SET
+
+     /* Round 3. */
+     /* Let [abcd k s t] denote the operation
+          a = b + ((a + H(b,c,d) + X[k] + T[i]) <<< s). */
+#define H(x, y, z) ((x) ^ (y) ^ (z))
+#define SET(a, b, c, d, k, s, Ti)\
+  t = a + H(b,c,d) + X[k] + Ti;\
+  a = ROTATE_LEFT(t, s) + b
+     /* Do the following 16 operations. */
+    SET(a, b, c, d,  5,  4, T33);
+    SET(d, a, b, c,  8, 11, T34);
+    SET(c, d, a, b, 11, 16, T35);
+    SET(b, c, d, a, 14, 23, T36);
+    SET(a, b, c, d,  1,  4, T37);
+    SET(d, a, b, c,  4, 11, T38);
+    SET(c, d, a, b,  7, 16, T39);
+    SET(b, c, d, a, 10, 23, T40);
+    SET(a, b, c, d, 13,  4, T41);
+    SET(d, a, b, c,  0, 11, T42);
+    SET(c, d, a, b,  3, 16, T43);
+    SET(b, c, d, a,  6, 23, T44);
+    SET(a, b, c, d,  9,  4, T45);
+    SET(d, a, b, c, 12, 11, T46);
+    SET(c, d, a, b, 15, 16, T47);
+    SET(b, c, d, a,  2, 23, T48);
+#undef SET
+
+     /* Round 4. */
+     /* Let [abcd k s t] denote the operation
+          a = b + ((a + I(b,c,d) + X[k] + T[i]) <<< s). */
+#define I(x, y, z) ((y) ^ ((x) | ~(z)))
+#define SET(a, b, c, d, k, s, Ti)\
+  t = a + I(b,c,d) + X[k] + Ti;\
+  a = ROTATE_LEFT(t, s) + b
+     /* Do the following 16 operations. */
+    SET(a, b, c, d,  0,  6, T49);
+    SET(d, a, b, c,  7, 10, T50);
+    SET(c, d, a, b, 14, 15, T51);
+    SET(b, c, d, a,  5, 21, T52);
+    SET(a, b, c, d, 12,  6, T53);
+    SET(d, a, b, c,  3, 10, T54);
+    SET(c, d, a, b, 10, 15, T55);
+    SET(b, c, d, a,  1, 21, T56);
+    SET(a, b, c, d,  8,  6, T57);
+    SET(d, a, b, c, 15, 10, T58);
+    SET(c, d, a, b,  6, 15, T59);
+    SET(b, c, d, a, 13, 21, T60);
+    SET(a, b, c, d,  4,  6, T61);
+    SET(d, a, b, c, 11, 10, T62);
+    SET(c, d, a, b,  2, 15, T63);
+    SET(b, c, d, a,  9, 21, T64);
+#undef SET
+
+     /* Then perform the following additions. (That is increment each
+        of the four registers by the value it had before this block
+        was started.) */
+    pms->abcd[0] += a;
+    pms->abcd[1] += b;
+    pms->abcd[2] += c;
+    pms->abcd[3] += d;
+}
+
+void
+md5_init(md5_state_t *pms)
+{
+    pms->count[0] = pms->count[1] = 0;
+    pms->abcd[0] = 0x67452301;
+    pms->abcd[1] = /*0xefcdab89*/ T_MASK ^ 0x10325476;
+    pms->abcd[2] = /*0x98badcfe*/ T_MASK ^ 0x67452301;
+    pms->abcd[3] = 0x10325476;
+}
+
+void
+md5_append(md5_state_t *pms, const md5_byte_t *data, int nbytes)
+{
+    const md5_byte_t *p = data;
+    int left = nbytes;
+    int offset = (pms->count[0] >> 3) & 63;
+    md5_word_t nbits = (md5_word_t)(nbytes << 3);
+
+    if (nbytes <= 0)
+	return;
+
+    /* Update the message length. */
+    pms->count[1] += nbytes >> 29;
+    pms->count[0] += nbits;
+    if (pms->count[0] < nbits)
+	pms->count[1]++;
+
+    /* Process an initial partial block. */
+    if (offset) {
+	int copy = (offset + nbytes > 64 ? 64 - offset : nbytes);
+
+	memcpy(pms->buf + offset, p, copy);
+	if (offset + copy < 64)
+	    return;
+	p += copy;
+	left -= copy;
+	md5_process(pms, pms->buf);
+    }
+
+    /* Process full blocks. */
+    for (; left >= 64; p += 64, left -= 64)
+	md5_process(pms, p);
+
+    /* Process a final partial block. */
+    if (left)
+	memcpy(pms->buf, p, left);
+}
+
+void
+md5_finish(md5_state_t *pms, md5_byte_t digest[16])
+{
+    static const md5_byte_t pad[64] = {
+	0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+    };
+    md5_byte_t data[8];
+    int i;
+
+    /* Save the length before padding. */
+    for (i = 0; i < 8; ++i)
+	data[i] = (md5_byte_t)(pms->count[i >> 2] >> ((i & 3) << 3));
+    /* Pad to 56 bytes mod 64. */
+    md5_append(pms, pad, ((55 - (pms->count[0] >> 3)) & 63) + 1);
+    /* Append the length. */
+    md5_append(pms, data, 8);
+    for (i = 0; i < 16; ++i)
+	digest[i] = (md5_byte_t)(pms->abcd[i >> 2] >> ((i & 3) << 3));
+}
diff --git a/md5.h b/md5.h
new file mode 100644
index 0000000..698c995
--- /dev/null
+++ b/md5.h
@@ -0,0 +1,91 @@
+/*
+  Copyright (C) 1999, 2002 Aladdin Enterprises.  All rights reserved.
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+
+  L. Peter Deutsch
+  ghost@aladdin.com
+
+ */
+/* $Id: md5.h,v 1.4 2002/04/13 19:20:28 lpd Exp $ */
+/*
+  Independent implementation of MD5 (RFC 1321).
+
+  This code implements the MD5 Algorithm defined in RFC 1321, whose
+  text is available at
+	http://www.ietf.org/rfc/rfc1321.txt
+  The code is derived from the text of the RFC, including the test suite
+  (section A.5) but excluding the rest of Appendix A.  It does not include
+  any code or documentation that is identified in the RFC as being
+  copyrighted.
+
+  The original and principal author of md5.h is L. Peter Deutsch
+  <ghost@aladdin.com>.  Other authors are noted in the change history
+  that follows (in reverse chronological order):
+
+  2002-04-13 lpd Removed support for non-ANSI compilers; removed
+	references to Ghostscript; clarified derivation from RFC 1321;
+	now handles byte order either statically or dynamically.
+  1999-11-04 lpd Edited comments slightly for automatic TOC extraction.
+  1999-10-18 lpd Fixed typo in header comment (ansi2knr rather than md5);
+	added conditionalization for C++ compilation from Martin
+	Purschke <purschke@bnl.gov>.
+  1999-05-03 lpd Original version.
+ */
+
+#ifndef md5_INCLUDED
+#  define md5_INCLUDED
+
+/*
+ * This package supports both compile-time and run-time determination of CPU
+ * byte order.  If ARCH_IS_BIG_ENDIAN is defined as 0, the code will be
+ * compiled to run only on little-endian CPUs; if ARCH_IS_BIG_ENDIAN is
+ * defined as non-zero, the code will be compiled to run only on big-endian
+ * CPUs; if ARCH_IS_BIG_ENDIAN is not defined, the code will be compiled to
+ * run on either big- or little-endian CPUs, but will run slightly less
+ * efficiently on either one than if ARCH_IS_BIG_ENDIAN is defined.
+ */
+
+typedef unsigned char md5_byte_t; /* 8-bit byte */
+typedef unsigned int md5_word_t; /* 32-bit word */
+
+/* Define the state of the MD5 Algorithm. */
+typedef struct md5_state_s {
+    md5_word_t count[2];	/* message length in bits, lsw first */
+    md5_word_t abcd[4];		/* digest buffer */
+    md5_byte_t buf[64];		/* accumulate block */
+} md5_state_t;
+
+#ifdef __cplusplus
+extern "C" 
+{
+#endif
+
+/* Initialize the algorithm. */
+void md5_init(md5_state_t *pms);
+
+/* Append a string to the message. */
+void md5_append(md5_state_t *pms, const md5_byte_t *data, int nbytes);
+
+/* Finish the message and return the digest. */
+void md5_finish(md5_state_t *pms, md5_byte_t digest[16]);
+
+#ifdef __cplusplus
+}  /* end extern "C" */
+#endif
+
+#endif /* md5_INCLUDED */
diff --git a/mndp.c b/mndp.c
index 4f2b88a..d7bd33f 100644
--- a/mndp.c
+++ b/mndp.c
@@ -29,14 +29,14 @@
 #include "protocol.h"
 #include "config.h"
 
+/* Protocol data direction, not used here, but obligatory for protocol.c */
+unsigned char mt_direction_fromserver = 0;
+
 int main(int argc, char **argv)  {
 	int sock,result;
 	int optval = 1;
 	struct sockaddr_in si_me, si_remote;
 	unsigned char buff[MT_PACKET_LEN];
-	unsigned short nameLen = 0;
-	unsigned char name[MT_MNDP_MAX_NAME_LENGTH + 1];
-	unsigned char mac[ETH_ALEN];
 
 	/* Open a UDP socket handle */
 	sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
@@ -73,6 +73,7 @@ int main(int argc, char **argv)  {
 	}
 
 	while(1) {
+		struct mt_mndp_packet *packet;
 		/* Wait for a UDP packet */
 		result = recvfrom(sock, buff, MT_PACKET_LEN, 0, 0, 0);
 		if (result < 0) {
@@ -80,29 +81,13 @@ int main(int argc, char **argv)  {
 			exit(1);
 		}
 
-		/* Check for valid packet length */
-		if (result < 18) {
-			continue;
-		}
-
-		/* Fetch length of Identifier string */
-		memcpy(&nameLen, buff+16,2);
-		nameLen = (nameLen >> 8) | ((nameLen&0xff)<<8);
-
-		/* Enforce maximum name length */
-		nameLen = nameLen < sizeof(name) ? nameLen : MT_MNDP_MAX_NAME_LENGTH;
+		/* Parse MNDP packet */
+		packet = parseMNDP(buff, result);
 
-		/* Read Identifier string */
-		memcpy(&name, buff+18, nameLen);
-
-		/* Append zero */
-		name[nameLen] = 0;
-
-		/* Read source MAC address */
-		memcpy(&mac, buff+8, ETH_ALEN);
-
-		/* Print it */
-		printf("%17s %s\n", ether_ntoa((struct ether_addr *)mac), name);
+		if (packet != NULL) {
+			/* Print it */
+			printf("%17s %s\n", ether_ntoa((struct ether_addr *)packet->address), packet->identity);
+		}
 	}
 
 	/* We'll never get here.. */
diff --git a/protocol.c b/protocol.c
index 5606365..9f8a693 100644
--- a/protocol.c
+++ b/protocol.c
@@ -21,10 +21,13 @@
 #include <stdlib.h>
 #ifndef __APPLE_CC__
 #include <linux/if_ether.h>
+#include <netinet/in.h>
+#include <netinet/ether.h>
 #else
 #define ETH_ALEN 6
 #include <net/ethernet.h>
 #endif
+#include <arpa/inet.h>
 #include "protocol.h"
 #include "config.h"
 
@@ -180,3 +183,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;
+			}
+		}
+	}
+}
diff --git a/protocol.h b/protocol.h
index 017b572..36616e8 100644
--- a/protocol.h
+++ b/protocol.h
@@ -27,7 +27,9 @@
 #define MT_MACTELNET_PORT 20561
 
 #define MT_MNDP_PORT 5678
-#define MT_MNDP_MAX_NAME_LENGTH 64
+#define MT_MNDP_MAX_IDENTITY_LENGTH 64
+#define MT_MNDP_TIMEOUT 5
+#define MT_MNDP_LONGTIMEOUT 120
 
 /* Packet type */
 #define MT_PTYPE_SESSIONSTART 0
@@ -65,16 +67,27 @@ struct mt_mactelnet_control_hdr {
 	unsigned char *data;
 };
 
+/* TODO: Add all the other information obtainable from mndp */
+struct mt_mndp_packet {
+	unsigned char address[ETH_ALEN];
+	unsigned char identity[MT_MNDP_MAX_IDENTITY_LENGTH];
+};
+
 struct mt_packet {
 	int size;
 	unsigned char data[MT_PACKET_LEN];
 };
 
+/* MacTelnet/Winbox packets */
 extern int initPacket(struct mt_packet *packet, unsigned char ptype, unsigned char *srcmac, unsigned char *dstmac, unsigned short sessionkey, unsigned int counter);
 extern int addControlPacket(struct mt_packet *packet, char cptype, void *cpdata, int data_len);
 extern void parsePacket(unsigned char *data, struct mt_mactelnet_hdr *pkthdr);
 extern int parseControlPacket(unsigned char *data, const int data_len, struct mt_mactelnet_control_hdr *cpkthdr);
 
+/* MNDP packets */
+struct mt_mndp_packet *parseMNDP(const char *data, const int packetLen);
+int queryMNDP(const unsigned char *identity, unsigned char *mac);
+
 /* Control packet magic header */
 extern unsigned char mt_mactelnet_cpmagic[4];
 extern unsigned char mt_mactelnet_clienttype[2];