Rewrote MNDP parser, and added MNDP broadcast routines for mactelnetd. Thanks to "Marcin Ulikowski" <elceef@itsec.pl> for MNDP reverse engineering. mactelnetd will now be recognized by other routeros devices/applications as neighbours.

6 files changed, 319 insertions, 41 deletions

diff --git a/config.h b/config.h
index 15fce0d..cab40be 100644
--- a/config.h
+++ b/config.h
@@ -3,4 +3,39 @@
 
 #define DEBUG 0
 
+#if defined(__APPLE__) && defined(__MACH__)
+#define PLATFORM_NAME "Mac OS X"
+
+#elif defined(__FreeBSD__)
+#define PLATFORM_NAME "FreeBSD"
+
+#elif defined(__NetBSD__)
+#define PLATFORM_NAME "NetBSD"
+
+#elif defined(__OpenBSD__)
+#define PLATFORM_NAME "OpenBSD"
+
+#elif defined(__MINT__)
+#define PLATFORM_NAME "FreeMiNT"
+
+#elif defined(__bsdi__)
+#define PLATFORM_NAME "BSD/OS"
+
+#elif defined(linux) || defined(__linux__)
+#define PLATFORM_NAME "Linux"
+
+#elif defined(sun)
+#define PLATFORM_NAME "Solaris"
+
+#elif defined(__hpux)
+#define PLATFORM_NAME "HPUX"
+
+#elif defined(__riscos__)
+#define PLATFORM_NAME "RISC OS"
+
+#else
+#define PLATFORM_NAME "Unknown"
+
+#endif
+
 #endif
diff --git a/mactelnetd.c b/mactelnetd.c
index 6c92b10..e7b64fa 100644
--- a/mactelnetd.c
+++ b/mactelnetd.c
@@ -36,9 +36,11 @@
 #include <linux/if_ether.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
+#include <sys/sysinfo.h>
 #include <pwd.h>
 #include <utmp.h>
 #include <syslog.h>
+#include <sys/utsname.h>
 #include "md5.h"
 #include "protocol.h"
 #include "udp.h"
@@ -64,6 +66,8 @@ struct mt_socket {
 
 static int sockfd;
 static int insockfd;
+static int mndpsockfd;
+
 static struct mt_socket sockets[MAX_INSOCKETS];
 static int sockets_count = 0;
 
@@ -75,6 +79,8 @@ static int sourceport;
 
 static unsigned char trypassword[17];
 
+static time_t last_mndp_time = 0;
+
 /* Protocol data direction */
 unsigned char mt_direction_fromserver = 1;
 
@@ -256,6 +262,24 @@ static int send_udp(const struct mt_connection *conn, const struct mt_packet *pa
 	}
 }
 
+static int send_mndp_udp(const struct mt_socket *sock, const struct mt_packet *packet) {
+	unsigned char dstmac[6];
+	
+	if (use_raw_socket) {
+		memset(dstmac, 255, 6);
+		return send_custom_udp(sockfd, sock->device_index, sock->mac, dstmac, &sourceip, MT_MNDP_PORT, &destip, MT_MNDP_PORT, packet->data, packet->size);
+	} else {
+		/* Init SendTo struct */
+		struct sockaddr_in socket_address;
+		socket_address.sin_family = AF_INET;
+		socket_address.sin_port = htons(MT_MNDP_PORT);
+		socket_address.sin_addr.s_addr = htonl(INADDR_BROADCAST);
+
+		return sendto(sock->sockfd, packet->data, packet->size, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
+	}
+}
+
+
 static void display_motd() {
 	FILE *fp;
 	int c;
@@ -701,12 +725,46 @@ static void print_version() {
 	fprintf(stderr, PROGRAM_NAME " " PROGRAM_VERSION "\n");
 }
 
+void mndp_broadcast() {
+	struct mt_packet pdata;
+	struct utsname s_uname;
+	struct sysinfo s_sysinfo;
+	int i;
+	unsigned int uptime;
+
+	if (uname(&s_uname) != 0)
+		return;
+	
+	if (sysinfo(&s_sysinfo) != 0)
+		return;
+	
+	uptime = s_sysinfo.uptime;
+
+	for (i = 0; i < sockets_count; ++i) {
+		struct mt_mndp_hdr *header = (struct mt_mndp_hdr *)&(pdata.data);
+		struct mt_socket *socket = &(sockets[i]);
+
+		mndp_init_packet(&pdata, 0, 1);
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_ADDRESS, socket->mac, 6);
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_IDENTITY, s_uname.nodename, strlen(s_uname.nodename));
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_VERSION, s_uname.release, strlen(s_uname.release));
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_PLATFORM, PLATFORM_NAME, strlen(PLATFORM_NAME));
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_HARDWARE, s_uname.machine, strlen(s_uname.machine));
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_TIMESTAMP, &uptime, 4);
+
+		header->cksum = in_cksum((unsigned short *)&(pdata.data), pdata.size);
+
+		send_mndp_udp(socket, &pdata);
+	}
+}
+
 /*
  * TODO: Rewrite main() when all sub-functionality is tested
  */
 int main (int argc, char **argv) {
 	int result,i;
 	struct sockaddr_in si_me;
+	struct sockaddr_in si_me_mndp;
 	struct timeval timeout;
 	struct mt_packet pdata;
 	fd_set read_fds;
@@ -785,7 +843,7 @@ int main (int argc, char **argv) {
 	/* Initialize receiving socket on the device chosen */
 	memset((char *) &si_me, 0, sizeof(si_me));
 	si_me.sin_family = AF_INET;
-	si_me.sin_port = htons(MT_MACTELNET_PORT);
+	si_me.sin_port = htons(sourceport);
 	memcpy(&(si_me.sin_addr), &sourceip, 4);
 
 	setsockopt(insockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval));
@@ -796,6 +854,27 @@ int main (int argc, char **argv) {
 		return 1;
 	}
 
+	/* TODO: Move socket initialization out of main() */
+
+	/* Receive mndp udp packets with this socket */
+	mndpsockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (insockfd < 0) {
+		perror("insockfd");
+		return 1;
+	}
+
+	memset((char *)&si_me_mndp, 0, sizeof(si_me_mndp));
+	si_me_mndp.sin_family = AF_INET;
+	si_me_mndp.sin_port = htons(MT_MNDP_PORT);
+	memcpy(&(si_me_mndp.sin_addr), &sourceip, 4);
+
+	setsockopt(mndpsockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval));
+
+	/* Bind to udp port */
+	if (bind(mndpsockfd, (struct sockaddr *)&si_me_mndp, sizeof(si_me_mndp))==-1) {
+		fprintf(stderr, "Error binding to %s:%d, %s\n", inet_ntoa(si_me_mndp.sin_addr), MT_MNDP_PORT, strerror(errno));
+	}
+
 	setup_sockets();
 
 	daemonize();
@@ -820,11 +899,13 @@ int main (int argc, char **argv) {
 		int reads;
 		struct mt_connection *p;
 		int maxfd=0;
+		time_t now;
 
 		/* Init select */
 		FD_ZERO(&read_fds);
 		FD_SET(insockfd, &read_fds);
-		maxfd = insockfd;
+		FD_SET(mndpsockfd, &read_fds);
+		maxfd = insockfd > mndpsockfd ? insockfd : mndpsockfd;
 
 		/* Add active connections to select queue */
 		for (p = connections_head; p != NULL; p = p->next) {
@@ -851,6 +932,18 @@ int main (int argc, char **argv) {
 				result = recvfrom(insockfd, buff, 1500, 0, (struct sockaddr *)&saddress, &slen);
 				handle_packet(buff, result, &saddress);
 			}
+			if (FD_ISSET(mndpsockfd, &read_fds)) {
+				unsigned char buff[1500];
+				struct sockaddr_in saddress;
+				unsigned int slen = sizeof(saddress);
+				result = recvfrom(mndpsockfd, buff, 1500, 0, (struct sockaddr *)&saddress, &slen);
+
+				/* Handle MNDP broadcast request, max 1 rps */
+				if (result == 4 && time(NULL) - last_mndp_time > 0) {
+					mndp_broadcast();
+					time(&last_mndp_time);
+				}
+			}
 			/* Handle data from terminal sessions */
 			for (p = connections_head; p != NULL; p = p->next) {
 				/* Check if we have data ready in the pty buffer for the active session */
@@ -887,22 +980,27 @@ int main (int argc, char **argv) {
 				}
 			}
 		/* Handle select() timeout */
-		} else {
-			if (connections_head != NULL) {
-				struct mt_connection *p,tmp;
-				for (p = connections_head; p != NULL; p = p->next) {
-					if (time(NULL) - p->lastdata >= MT_CONNECTION_TIMEOUT) {
-						syslog(LOG_INFO, "(%d) Session timed out", p->seskey);
-						init_packet(&pdata, MT_PTYPE_DATA, p->dstmac, p->srcmac, p->seskey, p->outcounter);
-						add_control_packet(&pdata, MT_CPTYPE_PLAINDATA, "Timeout\r\n", 9);
-						send_udp(p, &pdata);
-						init_packet(&pdata, MT_PTYPE_END, p->dstmac, p->srcmac, p->seskey, p->outcounter);
-						send_udp(p, &pdata);
-
-						tmp.next = p->next;
-						list_remove_connection(p);
-						p = &tmp;
-					}
+		}
+		time(&now);
+		
+		if (now - last_mndp_time > MT_MNDP_BROADCAST_INTERVAL) {
+			mndp_broadcast();
+			last_mndp_time = now;
+		}
+		if (connections_head != NULL) {
+			struct mt_connection *p,tmp;
+			for (p = connections_head; p != NULL; p = p->next) {
+				if (now - p->lastdata >= MT_CONNECTION_TIMEOUT) {
+					syslog(LOG_INFO, "(%d) Session timed out", p->seskey);
+					init_packet(&pdata, MT_PTYPE_DATA, p->dstmac, p->srcmac, p->seskey, p->outcounter);
+					add_control_packet(&pdata, MT_CPTYPE_PLAINDATA, "Timeout\r\n", 9);
+					send_udp(p, &pdata);
+					init_packet(&pdata, MT_PTYPE_END, p->dstmac, p->srcmac, p->seskey, p->outcounter);
+					send_udp(p, &pdata);
+
+					tmp.next = p->next;
+					list_remove_connection(p);
+					p = &tmp;
 				}
 			}
 		}
diff --git a/mndp.c b/mndp.c
index 716103c..0245746 100644
--- a/mndp.c
+++ b/mndp.c
@@ -42,6 +42,8 @@ int main(int argc, char **argv)  {
 	si_me.sin_port = htons(MT_MNDP_PORT);
 	si_me.sin_addr.s_addr = htonl(INADDR_ANY);
 
+	setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval));
+
 	/* 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);
@@ -68,7 +70,7 @@ int main(int argc, char **argv)  {
 	}
 
 	while(1) {
-		struct mt_mndp_packet *packet;
+		struct mt_mndp_info *packet;
 		/* Wait for a UDP packet */
 		result = recvfrom(sock, buff, MT_PACKET_LEN, 0, 0, 0);
 		if (result < 0) {
@@ -81,7 +83,14 @@ int main(int argc, char **argv)  {
 
 		if (packet != NULL) {
 			/* Print it */
-			printf("%17s %s\n", ether_ntoa((struct ether_addr *)packet->address), packet->identity);
+			printf(" %-17s %s", ether_ntoa((struct ether_addr *)packet->address), packet->identity);
+			if (packet->platform != NULL) {
+				printf(" (%s %s %s)", packet->platform, packet->version, packet->hardware);
+			}
+			if (packet->uptime > 0) {
+				printf(" up %d days %d hours", packet->uptime / 86400, packet->uptime % 86400 / 3600);
+			}
+			putchar('\n');
 		}
 	}
 
diff --git a/protocol.c b/protocol.c
index acf0b0b..84ecb1e 100644
--- a/protocol.c
+++ b/protocol.c
@@ -202,31 +202,137 @@ int parse_control_packet(unsigned char *packetdata, int data_len, struct mt_mact
 	}
 }
 
-struct mt_mndp_packet *parse_mndp(const unsigned char *data, const int packet_len) {
-	static struct mt_mndp_packet packet;
-	unsigned short name_len = 0;
+int mndp_init_packet(struct mt_packet *packet, unsigned char version, unsigned char ttl) {
+	struct mt_mndp_hdr *header = (struct mt_mndp_hdr *)packet->data;
+
+	header->version = version;
+	header->ttl = ttl;
+	header->cksum = 0;
+	
+	packet->size = sizeof(header);
+	
+	return sizeof(header);
+}
+
+int mndp_add_attribute(struct mt_packet *packet, enum mt_mndp_attrtype attrtype, void *attrdata, unsigned short data_len) {
+	unsigned char *data = packet->data + packet->size;
+	unsigned short type = attrtype;
+	unsigned short len = data_len;
+
+	/* Something is really wrong. Packets should never become over 1500 bytes */
+	if (packet->size + 4 + data_len > MT_PACKET_LEN) {
+		fprintf(stderr, "mndp_add_attribute: ERROR, too large packet. Exceeds %d bytes\n", MT_PACKET_LEN);
+		return -1;
+	}
+
+	/* TODO: Should check all host-to-network/network-to-host conversions in code
+	 * and add defines to check the current host's endianness.
+	 */
+	type = htons(type);
+	memcpy(data, &type, 2);
+
+	len = htons(len);
+	memcpy(data + 2, &len, 2);
+
+	memcpy(data + 4, attrdata, data_len);
+	
+	packet->size += 4 + data_len;
+	
+	return 4 + data_len;
+}
+
+
+struct mt_mndp_info *parse_mndp(const unsigned char *data, const int packet_len) {
+	const unsigned char *p;
+	static struct mt_mndp_info packet;
+	struct mt_mndp_hdr *mndp_hdr;
 
 	/* Check for valid packet length */
 	if (packet_len < 18) {
 		return NULL;
 	}
 
-	/* Fetch length of Identifier string */
-	memcpy(&name_len, data + 16,2);
-	name_len = (name_len >> 8) | ((name_len & 0xff) << 8);
+	bzero(&packet, sizeof(packet));
 
-	/* Enforce maximum name length */
-	name_len = name_len < MT_MNDP_MAX_IDENTITY_LENGTH ? name_len : MT_MNDP_MAX_IDENTITY_LENGTH;
+	mndp_hdr = (struct mt_mndp_hdr*)data;
 
-	/* Read Identifier string */
-	memcpy(packet.identity, data + 18, name_len);
-
-	/* Append zero */
-	packet.identity[name_len] = 0;
-
-	/* Read source MAC address */
-	memcpy(packet.address, data + 8, ETH_ALEN);
+	memcpy(&(packet.header), mndp_hdr, sizeof(mndp_hdr));
 
+	p = data + sizeof(struct mt_mndp_hdr);
+	
+	while(p < data + packet_len) {
+		unsigned short type, len;
+		
+		memcpy(&type, p, 2);
+		memcpy(&len, p + 2, 2);
+		
+		type = ntohs(type);
+		len = ntohs(len);
+		p += 4;
+
+		switch (type) {
+			case MT_MNDPTYPE_ADDRESS:
+				if (len >= ETH_ALEN) {
+					memcpy(packet.address, p, ETH_ALEN);
+				}
+				break;
+
+			case MT_MNDPTYPE_IDENTITY:
+				if (len > MT_MNDP_MAX_STRING_LENGTH) {
+					len = MT_MNDP_MAX_STRING_LENGTH;
+				}
+
+				memcpy(packet.identity, p, len);
+				packet.identity[len] = '\0';
+				break;
+
+			case MT_MNDPTYPE_PLATFORM:
+				if (len > MT_MNDP_MAX_STRING_LENGTH) {
+					len = MT_MNDP_MAX_STRING_LENGTH;
+				}
+
+				memcpy(packet.platform, p, len);
+				packet.platform[len] = '\0';
+				break;
+
+			case MT_MNDPTYPE_VERSION:
+				if (len > MT_MNDP_MAX_STRING_LENGTH) {
+					len = MT_MNDP_MAX_STRING_LENGTH;
+				}
+
+				memcpy(packet.version, p, len);
+				packet.version[len] = '\0';
+				break;
+
+			case MT_MNDPTYPE_TIMESTAMP:
+				memcpy(&(packet.uptime), p, 4);
+				break;
+
+			case MT_MNDPTYPE_HARDWARE:
+				if (len > MT_MNDP_MAX_STRING_LENGTH) {
+					len = MT_MNDP_MAX_STRING_LENGTH;
+				}
+
+				memcpy(packet.hardware, p, len);
+				packet.hardware[len] = '\0';
+				break;
+
+			case MT_MNDPTYPE_SOFTID:
+				if (len > MT_MNDP_MAX_STRING_LENGTH) {
+					len = MT_MNDP_MAX_STRING_LENGTH;
+				}
+				
+				memcpy(packet.softid, p, len);
+				packet.softid[len] = '\0';
+				break;
+
+			//default:
+				// Unhandled MNDP type
+		}
+		
+		p += len;
+	}
+	
 	return &packet;
 }
 
@@ -240,7 +346,7 @@ int query_mndp(const char *identity, unsigned char *mac) {
 	struct timeval timeout;
 	time_t start_time;
 	fd_set read_fds;
-	struct mt_mndp_packet *packet;
+	struct mt_mndp_info *packet;
 
 	start_time = time(0);
 
diff --git a/protocol.h b/protocol.h
index c55bf74..b2066ba 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_IDENTITY_LENGTH 64
+#define MT_MNDP_MAX_STRING_LENGTH 128
+#define MT_MNDP_BROADCAST_INTERVAL 30
+
 #define MT_MNDP_TIMEOUT 5
 #define MT_MNDP_LONGTIMEOUT 120
 
@@ -54,6 +56,24 @@ enum mt_cptype {
 	MT_CPTYPE_PLAINDATA = -1
 };
 
+/* MNDP attribute type */
+enum mt_mndp_attrtype {
+	MT_MNDPTYPE_ADDRESS   = 0x0001,
+	MT_MNDPTYPE_IDENTITY  = 0x0005,
+	MT_MNDPTYPE_VERSION   = 0x0007,
+	MT_MNDPTYPE_PLATFORM  = 0x0008,
+	MT_MNDPTYPE_TIMESTAMP = 0x000a,
+	MT_MNDPTYPE_SOFTID    = 0x000b,
+	MT_MNDPTYPE_HARDWARE  = 0x000c
+};
+
+/* MNDP packet header */
+struct mt_mndp_hdr {
+  unsigned char version;
+  unsigned char ttl;
+  unsigned short cksum;
+};
+
 struct mt_mactelnet_hdr {
 	unsigned char ver;
 	enum mt_ptype ptype;
@@ -72,9 +92,15 @@ struct mt_mactelnet_control_hdr {
 };
 
 /* TODO: Add all the other information obtainable from mndp */
-struct mt_mndp_packet {
+struct mt_mndp_info {
+	struct mt_mndp_hdr header;
 	unsigned char address[ETH_ALEN];
-	char identity[MT_MNDP_MAX_IDENTITY_LENGTH];
+	char identity[MT_MNDP_MAX_STRING_LENGTH];
+	char version[MT_MNDP_MAX_STRING_LENGTH];
+	char platform[MT_MNDP_MAX_STRING_LENGTH];
+	char hardware[MT_MNDP_MAX_STRING_LENGTH];
+	char softid[MT_MNDP_MAX_STRING_LENGTH];
+	unsigned int uptime;
 };
 
 struct mt_packet {
@@ -89,7 +115,10 @@ extern void parse_packet(unsigned char *data, struct mt_mactelnet_hdr *pkthdr);
 extern int parse_control_packet(unsigned char *data, int data_len, struct mt_mactelnet_control_hdr *cpkthdr);
 
 /* MNDP packets */
-struct mt_mndp_packet *parse_mndp(const unsigned char *data, const int packet_len);
+extern int mndp_init_packet(struct mt_packet *packet, unsigned char version, unsigned char ttl);
+extern int mndp_add_attribute(struct mt_packet *packet, enum mt_mndp_attrtype attrtype, void *attrdata, unsigned short data_len);
+
+extern struct mt_mndp_info *parse_mndp(const unsigned char *data, const int packet_len);
 int query_mndp(const char *identity, unsigned char *mac);
 
 /* Number of milliseconds between each retransmission */
diff --git a/udp.h b/udp.h
index 90968cd..79cf3a2 100644
--- a/udp.h
+++ b/udp.h
@@ -1,4 +1,5 @@
 #ifndef _UDP_H
 #define _UDP_H 1
 extern int send_custom_udp(const int socket, const int ifindex, const unsigned char *sourcemac, const unsigned char *destmac, const struct in_addr *sourceip, const int sourceport, const struct in_addr *destip, const int destport, const unsigned char *data, const int datalen);
+extern unsigned short in_cksum(unsigned short *addr, int len);
 #endif