Added support for connecting to your router via the router identity name. (using MNDP)

6 files changed, 180 insertions, 31 deletions

diff --git a/Makefile b/Makefile
index f3113d3..0902c29 100644
--- a/Makefile
+++ b/Makefile
@@ -21,4 +21,4 @@ mactelnetd: config.h mactelnetd.c udp.h udp.c protocol.c protocol.h console.c co
 	gcc -g -DUSERSFILE='"/etc/mactelnetd.users"' -o mactelnetd -lcrypto mactelnetd.c udp.c protocol.c console.c users.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..f322f44 100644
--- a/README.markdown
+++ b/README.markdown
@@ -4,7 +4,7 @@ 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
 ------------
@@ -26,6 +26,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/mactelnet.c b/mactelnet.c
index 411413a..03233b4 100644
--- a/mactelnet.c
+++ b/mactelnet.c
@@ -283,12 +283,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");
 			fprintf(stderr, "  -n        Do not use broadcast packets. Less insecure but requires root privileges.\n");
 			fprintf(stderr, "  -u        Specify username on command line.\n");
 			fprintf(stderr, "  -p        Specify password on command line.\n");
@@ -302,8 +303,33 @@ 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 */
-	ether_aton_r(argv[optind], (struct ether_addr *)dstmac);
+	/* 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 */
+			ether_aton_r(argv[optind], (struct ether_addr *)dstmac);
+		}
+	}
 
 	/* Seed randomizer */
 	srand(time(NULL));
diff --git a/mndp.c b/mndp.c
index 006985c..1feaba9 100644
--- a/mndp.c
+++ b/mndp.c
@@ -24,14 +24,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);
@@ -68,6 +68,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) {
@@ -75,29 +76,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 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;
+			}
+		}
+	}
+}
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];