merge

12 files changed, 683 insertions, 602 deletions

diff --git a/Makefile b/Makefile
index 51c39dc..7b759a4 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 
 CC?=gcc
-CFLAGS+= 
+CFLAGS+= -lrt
 
 all: macping mndp mactelnet mactelnetd
 
@@ -24,29 +24,26 @@ install-docs:
 	install -d $(DESTDIR)/usr/share/man/man1/
 	install docs/*.1 $(DESTDIR)/usr/share/man/man1/
 
-udp.o: udp.c udp.h
-	${CC} -Wall ${CFLAGS} -c udp.c
-
 users.o: users.c users.h
 	${CC} -Wall ${CFLAGS} -DUSERSFILE='"/etc/mactelnetd.users"' -c users.c
 
 protocol.o: protocol.c protocol.h
 	${CC} -Wall ${CFLAGS} -c protocol.c
 
-devices.o: devices.c devices.h
-	${CC} -Wall ${CFLAGS} -c devices.c
+interfaces.o: interfaces.c interfaces.h
+	${CC} -Wall ${CFLAGS} -c interfaces.c
 
 md5.o: md5.c md5.h
 	${CC} -Wall ${CFLAGS} -c md5.c
 
-mactelnet: config.h udp.o mactelnet.c mactelnet.h protocol.o console.c console.h devices.o md5.o
-	${CC} -Wall ${CFLAGS} -o mactelnet mactelnet.c udp.o protocol.o console.c devices.o md5.o
+mactelnet: config.h mactelnet.c mactelnet.h protocol.o console.c console.h interfaces.o md5.o
+	${CC} -Wall ${CFLAGS} -o mactelnet mactelnet.c protocol.o console.c interfaces.o md5.o
 
-mactelnetd: config.h mactelnetd.c udp.o protocol.o devices.o console.c console.h users.o users.h md5.o
-	${CC} -Wall ${CFLAGS} -o mactelnetd mactelnetd.c udp.o protocol.o console.c devices.o users.o md5.o
+mactelnetd: config.h mactelnetd.c protocol.o interfaces.o console.c console.h users.o users.h md5.o
+	${CC} -Wall ${CFLAGS} -o mactelnetd mactelnetd.c protocol.o console.c interfaces.o users.o md5.o
 
 mndp: config.h mndp.c protocol.o
 	${CC} -Wall ${CFLAGS} -o mndp mndp.c protocol.o
 
-macping: config.h macping.c udp.o devices.o protocol.o
-	${CC} -Wall ${CFLAGS} -o macping macping.c devices.o udp.o protocol.o
+macping: config.h macping.c interfaces.o protocol.o
+	${CC} -Wall ${CFLAGS} -o macping macping.c interfaces.o protocol.o
diff --git a/config.h b/config.h
index 2b472fb..c2c1fd4 100644
--- a/config.h
+++ b/config.h
@@ -21,6 +21,8 @@
 
 #define DEBUG 0
 
+#define PROGRAM_VERSION "0.3.3"
+
 #if defined(__APPLE__) && defined(__MACH__)
 #define PLATFORM_NAME "Mac OS X"
 
@@ -51,6 +53,8 @@
 #elif defined(__riscos__)
 #define PLATFORM_NAME "RISC OS"
 
+#elif defined(__FreeBSD_kernel__)
+#define PLATFORM_NAME "kFreeBSD"
 #else
 #define PLATFORM_NAME "Unknown"
 
diff --git a/devices.c b/devices.c
deleted file mode 100644
index 00bf59d..0000000
--- a/devices.c
+++ /dev/null
@@ -1,179 +0,0 @@
-/*
-    Mac-Telnet - Connect to RouterOS or mactelnetd devices via MAC address
-    Copyright (C) 2010, Håkon Nessjøen <haakon.nessjoen@gmail.com>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License along
-    with this program; if not, write to the Free Software Foundation, Inc.,
-    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
-*/
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <netinet/in.h>
-#include <linux/if_ether.h>
-#include <sys/ioctl.h>
-#include <net/if.h>
-
-#include <malloc.h>
-#include <ifaddrs.h>
-#include <sys/ioctl.h>
-#include <net/if.h>
-#include <netpacket/packet.h>
-#include <netinet/in.h>
-
-
-/* Functions using NETDEVICE api */
-
-int get_device_index(int sockfd, char *device_name) {
-	struct ifreq ifr;
-
-	/* Find interface index from device_name */
-	strncpy(ifr.ifr_name, device_name, 16);
-	if (ioctl(sockfd, SIOCGIFINDEX, &ifr) != 0) {
-		return -1;
-	}
-
-	/* Return interface index */
-	return ifr.ifr_ifindex;
-}
-
-int get_device_mac(const int sockfd, const char *device_name, unsigned char *mac) {
-	struct ifreq ifr;
-
-	/* Find interface hardware address from device_name */
-	strncpy(ifr.ifr_name, device_name, 16);
-	if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) != 0) {
-		return -1;
-	}
-
-	/* Fetch mac address */
-	memcpy(mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
-	return 1;
-}
-
-int get_device_ip(const int sockfd, const char *device_name, struct sockaddr_in *ip) {
-	struct ifconf ifc;
-	struct ifreq *ifr;
-	int i,device_count;
-
-	/*
-	 * Do a initial query without allocating ifreq structs
-	 * to count the number of ifreq structs to allocate memory for
-	*/
-	memset(&ifc, 0, sizeof(ifc));
-	if (ioctl(sockfd, SIOCGIFCONF, &ifc) != 0) {
-		return -1;
-	}
-
-	/*
-	 * Allocate memory for interfaces, multiply by two in case
-	 * the number of interfaces has increased since last ioctl
-	*/
-	if ((ifr = malloc(ifc.ifc_len * 2)) == NULL) {
-		perror("malloc");
-		exit(1);
-	}
-
-	/* Do the actual query for info about all interfaces */
-	ifc.ifc_req = ifr;
-	if (ioctl(sockfd, SIOCGIFCONF, &ifc) != 0) {
-		free(ifr);
-		return -1;
-	}
-
-	/* Iterate through all devices, searching for interface */
-	device_count = ifc.ifc_len / sizeof(struct ifreq);
-	for (i = 0; i < device_count; ++i) {
-		if (strcmp(ifr[i].ifr_name, device_name) == 0) {
-			/* Fetch IP for found interface */
-			memcpy(ip, &(ifr[i].ifr_addr), sizeof(struct sockaddr_in));
-			free(ifr);
-			return 1;
-		}
-	}
-	free(ifr);
-	return -1;
-}
-
-int get_macs(int sockfd, char *name, int name_len, unsigned char *mac) {
-	static int first = 1;
-	static struct ifaddrs *int_addrs;
-	static const struct ifaddrs *int_cursor;
-	const struct sockaddr_in *dl_addr;
-
-	if (first == 1) {
-		first = 0;
-		if (getifaddrs(&int_addrs) == 0) {
-			int_cursor = int_addrs;
-		} else {
-			first = 1;
-			return 0;
-		}
-	}
-	if (int_cursor != NULL) {
-		while (int_cursor != NULL) {
-			dl_addr = (const struct sockaddr_in *) int_cursor->ifa_addr;
-			if (dl_addr != NULL && dl_addr->sin_family == AF_PACKET) {
-				strncpy(name, int_cursor->ifa_name, name_len - 1);
-				name[name_len - 1] = '\0';
-				int_cursor = int_cursor->ifa_next;
-				if (get_device_mac(sockfd, name, mac)!=-1) {
-					return 1;
-				}
-			}
-			int_cursor = int_cursor->ifa_next;
-		}
-	}
-	if (int_addrs != NULL) {
-		freeifaddrs(int_addrs);
-		int_addrs = NULL;
-	}
-	return 0;
-}
-
-int get_ips(char *name, int name_len, struct sockaddr_in *ip) {
-	static int first = 1;
-	static struct ifaddrs *int_addrs;
-	static const struct ifaddrs *int_cursor;
-	const struct sockaddr_in *dl_addr;
-
-	if (first == 1) {
-		first = 0;
-		if (getifaddrs(&int_addrs) == 0) {
-			int_cursor = int_addrs;
-		} else {
-			first = 1;
-			return 0;
-		}
-	}
-	if (int_cursor != NULL) {
-		while (int_cursor != NULL) {
-			dl_addr = (const struct sockaddr_in *) int_cursor->ifa_addr;
-			if (dl_addr != NULL && dl_addr->sin_family == AF_INET) {
-				memcpy(ip, dl_addr, sizeof(struct sockaddr_in));
-				strncpy(name, int_cursor->ifa_name, name_len - 1);
-				name[name_len - 1] = '\0';
-				int_cursor = int_cursor->ifa_next;
-				return 1;
-			}
-			int_cursor = int_cursor->ifa_next;
-		}
-	}
-	if (int_addrs != NULL) {
-		freeifaddrs(int_addrs);
-		int_addrs = NULL;
-	}
-	return 0;
-
-}
diff --git a/devices.h b/devices.h
deleted file mode 100644
index 7c24108..0000000
--- a/devices.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
-    Mac-Telnet - Connect to RouterOS or mactelnetd devices via MAC address
-    Copyright (C) 2010, Håkon Nessjøen <haakon.nessjoen@gmail.com>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License along
-    with this program; if not, write to the Free Software Foundation, Inc.,
-    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
-*/
-extern int get_device_index(int sockfd, char *deviceName);
-extern int get_device_mac(const int sockfd, const char *deviceName, unsigned char *mac);
-extern int get_device_ip(const int sockfd, const char *deviceName, struct sockaddr_in *ip);
-int get_ips(char *name, int nameLen, struct sockaddr_in *ip);
-int get_macs(int sockfd, char *name, int name_len, unsigned char *mac);
diff --git a/interfaces.c b/interfaces.c
new file mode 100644
index 0000000..f0bff9b
--- /dev/null
+++ b/interfaces.c
@@ -0,0 +1,376 @@
+/*
+    Mac-Telnet - Connect to RouterOS or mactelnetd devices via MAC address
+    Copyright (C) 2010, Håkon Nessjøen <haakon.nessjoen@gmail.com>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License along
+    with this program; if not, write to the Free Software Foundation, Inc.,
+    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+*/
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <ifaddrs.h>
+#include <netinet/ip.h>
+#include <netinet/udp.h>
+#include <netinet/ether.h>
+#include <arpa/inet.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#ifndef __linux__
+#include <net/if_dl.h>
+#include <net/bpf.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#else
+#include <linux/if_packet.h>
+#endif
+#include "protocol.h"
+#include "interfaces.h"
+
+
+struct net_interface *net_get_interface_ptr(struct net_interface *interfaces, int max_devices, char *name, int create) {
+	int i;
+
+	for (i = 0; i < max_devices; ++i) {
+		if (!interfaces[i].in_use)
+			break;
+		
+		if (strncmp(interfaces[i].name, name, 254) == 0) {
+			return interfaces + i;
+		}
+	}
+
+	if (create && i < max_devices) {
+		interfaces[i].in_use = 1;
+		strncpy(interfaces[i].name, name, 254);
+		interfaces[i].name[254] = '\0';
+		return interfaces + i;
+	}
+
+	return NULL;
+}
+
+#ifdef __linux__
+static void net_update_mac(struct net_interface *interfaces, int max_devices) {
+	unsigned char emptymac[] = {0, 0, 0, 0, 0, 0};
+	struct ifreq ifr;
+	int i,tmpsock;
+
+	tmpsock = socket(PF_INET, SOCK_DGRAM, 0);
+	if (tmpsock < 0) {
+		perror("net_update_mac");
+		exit(1);
+	}
+
+	for (i = 0; i < max_devices; ++i) {
+		if (interfaces[i].in_use) {
+			/* Find interface hardware address from device_name */
+			strncpy(ifr.ifr_name, interfaces[i].name, 16);
+			if (ioctl(tmpsock, SIOCGIFHWADDR, &ifr) == 0) {
+				/* Fetch mac address */
+				memcpy(interfaces[i].mac_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+				if (memcmp(interfaces[i].mac_addr, &emptymac, ETH_ALEN) != 0) {
+					interfaces[i].has_mac = 1;
+				}
+			}
+ 
+		}
+	}
+	close(tmpsock);
+}
+
+static int get_device_index(char *device_name) {
+        struct ifreq ifr;
+	int tmpsock;
+	
+	tmpsock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+
+	/* Find interface index from device_name */
+	strncpy(ifr.ifr_name, device_name, 16);
+	if (ioctl(tmpsock, SIOCGIFINDEX, &ifr) != 0) {
+		return -1;
+	}
+
+	/* Return interface index */
+	return ifr.ifr_ifindex;
+}
+#endif
+
+int net_get_interfaces(struct net_interface *interfaces, int max_devices) {
+	static struct ifaddrs *int_addrs;
+	static const struct ifaddrs *int_cursor;
+	const struct sockaddr_in *dl_addr;
+	int found = 0;
+
+	if (getifaddrs(&int_addrs) < 0) {
+		perror("getifaddrs");
+		exit(1);
+	}
+
+	for (int_cursor = int_addrs; int_cursor != NULL; int_cursor = int_cursor->ifa_next) {
+		int family = int_cursor->ifa_addr->sa_family;
+		dl_addr = (const struct sockaddr_in *) int_cursor->ifa_addr;
+
+		if (int_cursor->ifa_addr == NULL)
+			continue;
+
+		if (family == AF_INET) {
+			struct net_interface *interface = net_get_interface_ptr(interfaces, max_devices, int_cursor->ifa_name, 1);
+			if (interface != NULL) {
+				found++;
+				memcpy(interface->ipv4_addr, &dl_addr->sin_addr, IPV4_ALEN);
+			}
+#ifdef __linux__
+			interface->ifindex = get_device_index(interface->name);
+#endif
+		}
+#ifndef __linux__
+		{
+			unsigned char emptymac[] = {0, 0, 0, 0, 0, 0};
+			struct sockaddr_dl *sdl = (struct sockaddr_dl *)int_cursor->ifa_addr;
+			if (sdl->sdl_alen == ETH_ALEN) {
+				struct net_interface *interface = net_get_interface_ptr(interfaces, max_devices, int_cursor->ifa_name, 1);
+				memcpy(interface->mac_addr, LLADDR(sdl), ETH_ALEN);
+				if (interface != NULL && memcmp(interface->mac_addr, &emptymac, ETH_ALEN) != 0) {
+					interface->has_mac = 1;
+				}
+			}
+		}
+#endif
+	}
+	freeifaddrs(int_addrs);
+
+#ifdef __linux__
+	net_update_mac(interfaces, max_devices);
+#endif
+
+#if 0
+	{
+		int i = 0;
+		for (i = 0; i < max_devices; ++i) {
+			if (interfaces[i].in_use) {
+				struct in_addr *addr = (struct in_addr *)interfaces[i].ipv4_addr;
+				printf("Interface %s:\n", interfaces[i].name);
+				printf("\tIP: %s\n", inet_ntoa(*addr));
+				printf("\tMAC: %s\n", ether_ntoa((struct ether_addr *)interfaces[i].mac_addr));
+#ifdef __linux__
+				printf("\tIfIndex: %d\n", interfaces[i].ifindex);
+#endif
+				printf("\n");
+			}
+		}
+	}
+#endif
+	return found;
+}
+
+unsigned short in_cksum(unsigned short *addr, int len)
+{
+	int nleft = len;
+	int sum = 0;
+	unsigned short *w = addr;
+	unsigned short answer = 0;
+	
+	while (nleft > 1) {
+		sum += *w++;
+		nleft -= 2;
+	}
+	
+	if (nleft == 1) {
+		*(unsigned char *) (&answer) = *(unsigned char *) w;
+		sum += answer;
+	}
+	
+	sum = (sum >> 16) + (sum & 0xFFFF);
+	sum += (sum >> 16);
+	answer = ~sum;
+	return (answer);
+}
+
+unsigned short udp_sum_calc(unsigned char *src_addr,unsigned char *dst_addr, unsigned char *data, unsigned short len) {
+	unsigned short prot_udp=17;
+	unsigned short padd=0;
+	unsigned short word16;
+	unsigned int sum = 0;
+	int i;
+
+	/* Padding ? */
+	padd = (len % 2);
+	if (padd) {
+		data[len] = 0;
+	}
+
+	/* header+data */
+	for (i = 0; i < len + padd; i += 2){
+		word16 = ((data[i] << 8) & 0xFF00) + (data[i + 1] & 0xFF);
+		sum += word16;
+	}
+
+	/* source ip */
+	for (i = 0; i < IPV4_ALEN; i += 2){
+		word16 = ((src_addr[i] << 8) & 0xFF00) + (src_addr[i + 1] & 0xFF);
+		sum += word16;
+	}
+
+	/* dest ip */
+	for (i = 0; i < IPV4_ALEN; i += 2){
+		word16 = ((dst_addr[i] << 8) & 0xFF00) + (dst_addr[i + 1] & 0xFF);
+		sum += word16;
+	}
+
+	sum += prot_udp + len;
+
+	while (sum>>16)
+		sum = (sum & 0xFFFF) + (sum >> 16);
+
+	sum = ~sum;
+
+	if (sum == 0)
+		sum = 0xFFFF;
+
+	return (unsigned short) sum;
+}
+
+int net_init_raw_socket(struct net_interface *interface) {
+	int fd;
+
+#ifdef __linux__
+	/* Transmit raw packets with this socket */
+	fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+	if (fd < 0) {
+		perror("raw_socket");
+		exit(1);
+	}
+#else
+	/* Transmit raw packets with bpf */
+	fd = open("/dev/bpf0", O_RDWR);
+	if (fd <= 0) {
+		perror("open_bpf");
+		exit(1);
+	}
+#endif
+
+	return fd;
+}
+
+int net_send_udp(const int fd, struct net_interface *interface, 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) {
+#ifdef __linux__
+	struct sockaddr_ll socket_address;
+#endif
+	/*
+	 * Create a buffer for the full ethernet frame
+	 * and align header pointers to the correct positions.
+	*/
+	void* buffer = (void*)malloc(ETH_FRAME_LEN);
+	struct ethhdr *eh = (struct ethhdr *)buffer;
+	struct iphdr *ip = (struct iphdr *)(buffer + 14);
+	struct udphdr *udp = (struct udphdr *)(buffer + 14 + 20);
+	unsigned char *rest = (unsigned char *)(buffer + 20 + 14 + sizeof(struct udphdr));
+
+	if (((void *)rest - (void*)buffer) + datalen  > ETH_FRAME_LEN) {
+		fprintf(stderr, "packet size too large\n");
+		free(buffer);
+		return 0;
+	}
+
+	static unsigned int id = 1;
+	int send_result = 0;
+
+	/* Abort if we couldn't allocate enough memory */
+	if (buffer == NULL) {
+		perror("malloc");
+		exit(1);
+	}
+
+	/* Init ethernet header */
+	memcpy(eh->h_source, sourcemac, ETH_ALEN);
+	memcpy(eh->h_dest, destmac, ETH_ALEN);
+	eh->h_proto = htons(ETH_P_IP);
+
+#ifdef __linux__
+	/* Init SendTo struct */
+	socket_address.sll_family   = AF_PACKET;
+	socket_address.sll_protocol = htons(ETH_P_IP);
+	socket_address.sll_ifindex  = interface->ifindex;
+	socket_address.sll_hatype   = ARPHRD_ETHER;
+	socket_address.sll_pkttype  = PACKET_OTHERHOST;
+	socket_address.sll_halen    = ETH_ALEN;         
+
+	memcpy(socket_address.sll_addr, eh->h_source, ETH_ALEN);
+	socket_address.sll_addr[6]  = 0x00;/*not used*/
+	socket_address.sll_addr[7]  = 0x00;/*not used*/
+#endif
+
+	/* Init IP Header */
+	ip->version = 4;
+	ip->ihl = 5;
+	ip->tos = 0x10;
+	ip->tot_len = htons(datalen + 8 + 20);
+	ip->id = htons(id++);
+	ip->frag_off = htons(0x4000);
+	ip->ttl = 64;
+	ip->protocol = 17; /* UDP */
+	ip->check = 0x0000;
+	ip->saddr = sourceip->s_addr;
+	ip->daddr = destip->s_addr;
+
+	/* Calculate checksum for IP header */
+	ip->check = in_cksum((unsigned short *)ip, sizeof(struct iphdr));
+
+	/* Init UDP Header */
+	udp->source = htons(sourceport);
+	udp->dest = htons(destport);
+	udp->len = htons(sizeof(struct udphdr) + datalen);
+	udp->check = 0;
+
+	/* Insert actual data */
+	memcpy(rest, data, datalen);
+
+	/* Add UDP checksum */
+	udp->check = udp_sum_calc((unsigned char *)&(ip->saddr), (unsigned char *)&(ip->daddr), (unsigned char *)udp, sizeof(struct udphdr) + datalen);
+	udp->check = htons(udp->check);
+
+#ifdef __linux__
+	/* Send the packet */
+	send_result = sendto(fd, buffer, datalen + 8 + 14 + 20, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
+	if (send_result == -1)
+		perror("sendto");
+#else
+	{
+		struct ifreq req_if;
+
+		/* Pick device to send through */
+		strcpy(req_if.ifr_name, interface->name);
+		if (ioctl(fd, BIOCSETIF, &req_if) > 0) {
+			perror("ioctl_BIOCSETIF");
+			exit(1);
+		}
+	}
+
+	send_result = write(fd, buffer, datalen + 8 + 14 + 20);
+	if (send_result == -1)
+		perror("bpf_write");
+#endif
+	free(buffer);
+	
+	/* Return amount of _data_ bytes sent */
+	if (send_result - 8 - 14 - 20 < 0) {
+		return 0;
+	}
+
+	return send_result - 8 - 14 - 20;
+}
diff --git a/udp.h b/interfaces.h
index 269195c..1dab785 100644
--- a/udp.h
+++ b/interfaces.h
@@ -16,8 +16,30 @@
     with this program; if not, write to the Free Software Foundation, Inc.,
     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */
-#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);
+#ifndef _INTERFACES_H
+#define _INTERFACES_H 1
+
+#define MAX_INTERFACES 32
+
+struct net_interface {
+	char name[256];
+	unsigned char ipv4_addr[IPV4_ALEN];
+	unsigned char mac_addr[ETH_ALEN];
+
+	/* used by mactelnetd */
+	int socketfd;
+
+#ifdef __linux__
+	int ifindex;
+#endif
+	int has_mac;
+	int in_use;
+};
+
+
+extern int net_get_interfaces(struct net_interface *interfaces, int max_devices);
+extern struct net_interface *net_get_interface_ptr(struct net_interface *interfaces, int max_devices, char *name, int create);
+extern int net_init_raw_socket();
+extern int net_send_udp(const int socket, struct net_interface *interface, 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
diff --git a/macping.c b/macping.c
index 6b169d3..2e90bc7 100644
--- a/macping.c
+++ b/macping.c
@@ -28,30 +28,23 @@
 #include <stdio.h>
 #include <float.h>
 #include "protocol.h"
-#include "udp.h"
-#include "devices.h"
+#include "interfaces.h"
 #include "config.h"
 
 #define MAX_DEVICES 128
 #define MT_INTERFACE_LEN 128
 
 #define PROGRAM_NAME "MAC-Ping"
-#define PROGRAM_VERSION "0.3.2"
 
 static int sockfd, insockfd;
 
-struct mt_device {
-	unsigned char mac[ETH_ALEN];
-	char name[MT_INTERFACE_LEN];
-	int device_index;
-};
-
 static unsigned short ping_size = 38;
-static struct mt_device devices[MAX_DEVICES];
-static int devices_count = 0;
+
+struct net_interface interfaces[MAX_INTERFACES];
+
 static struct in_addr sourceip;
 static struct in_addr destip;
-static unsigned char dstmac[6];
+static unsigned char dstmac[ETH_ALEN];
 
 static int ping_sent = 0;
 static int pong_received = 0;
@@ -66,29 +59,6 @@ static void print_version() {
 	fprintf(stderr, PROGRAM_NAME " " PROGRAM_VERSION "\n");
 }
 
-static void setup_devices() {
-	char devicename[MT_INTERFACE_LEN];
-	unsigned char mac[ETH_ALEN];
-	unsigned char emptymac[ETH_ALEN];
-	int success;
-
-	memset(emptymac, 0, ETH_ALEN);
-
-	while ((success = get_macs(insockfd, devicename, MT_INTERFACE_LEN, mac))) {
-		if (memcmp(mac, emptymac, ETH_ALEN) != 0) {
-			struct mt_device *device = &(devices[devices_count]);
-
-			memcpy(device->mac, mac, ETH_ALEN);
-			strncpy(device->name, devicename, MT_INTERFACE_LEN - 1);
-			device->name[MT_INTERFACE_LEN - 1] = '\0';
-
-			device->device_index = get_device_index(insockfd, devicename);
-
-			devices_count++;
-		}
-	}
-}
-
 static long long int toddiff(struct timeval *tod1, struct timeval *tod2)
 {
     long long t1, t2;
@@ -193,6 +163,10 @@ int main(int argc, char **argv)  {
 		exit(1);
 	}
 
+	/* Mikrotik RouterOS does not answer unless the packet has the correct recipient mac-address in
+	 * the ethernet frame. Unlike real MacTelnet connections where the OS is ok with it being a
+	 * broadcast mac address.
+	 */
 	if (geteuid() != 0) {
 		fprintf(stderr, "You need to have root privileges to use %s.\n", argv[0]);
 		return 1;
@@ -204,12 +178,7 @@ int main(int argc, char **argv)  {
 		return 1;
 	}
 
-	/* Open a UDP socket handle */
-	sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
-	if (sockfd < 0) {
-		perror("sockfd");
-		return 1;
-	}
+	sockfd = net_init_raw_socket();
 
 	insockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 	if (insockfd < 0) {
@@ -239,7 +208,8 @@ int main(int argc, char **argv)  {
 
 	srand(time(NULL));
 
-	setup_devices();
+	/* Enumerate available interfaces */
+	net_get_interfaces(interfaces, MAX_INTERFACES);
 
 	if (ping_size < sizeof(struct timeval)) {
 		ping_size = sizeof(struct timeval);
@@ -264,12 +234,20 @@ int main(int argc, char **argv)  {
 			pingdata[ii] = rand() % 256;
 		}
 
-		for (ii = 0; ii < devices_count; ++ii) {
-			struct mt_device *device = &devices[ii];
+		for (ii = 0; ii < MAX_INTERFACES; ++ii) {
+			struct net_interface *interface = &interfaces[ii];
+
+			if (!interface->in_use) {
+				break;
+			}
+
+			if (!interface->has_mac) {
+				continue;
+			}
 
-			init_pingpacket(&packet, device->mac, dstmac);
+			init_pingpacket(&packet, interface->mac_addr, dstmac);
 			add_packetdata(&packet, pingdata, ping_size);
-			result = send_custom_udp(sockfd, device->device_index, device->mac, dstmac, &sourceip, MT_MACTELNET_PORT, &destip, MT_MACTELNET_PORT, packet.data, packet.size);
+			result = net_send_udp(sockfd, interface, interface->mac_addr, dstmac, &sourceip, MT_MACTELNET_PORT, &destip, MT_MACTELNET_PORT, packet.data, packet.size);
 
 			if (result > 0) {
 				sent++;
diff --git a/mactelnet.c b/mactelnet.c
index ce91d01..ca27f15 100644
--- a/mactelnet.c
+++ b/mactelnet.c
@@ -1,4 +1,4 @@
-/*
+/*find
     Mac-Telnet - Connect to RouterOS or mactelnetd devices via MAC address
     Copyright (C) 2010, Håkon Nessjøen <haakon.nessjoen@gmail.com>
 
@@ -32,21 +32,21 @@
 #include <sys/types.h>
 #include <sys/socket.h>
 #include <string.h>
+#ifdef __LINUX__
 #include <linux/if_ether.h>
+#endif
 #include "md5.h"
 #include "protocol.h"
-#include "udp.h"
 #include "console.h"
-#include "devices.h"
+#include "interfaces.h"
 #include "config.h"
 #include "mactelnet.h"
 
 #define PROGRAM_NAME "MAC-Telnet"
-#define PROGRAM_VERSION "0.3.2"
 
-static int sockfd;
+static int sockfd = 0;
 static int insockfd;
-static int device_index;
+
 static unsigned int outcounter = 0;
 static unsigned int incounter = 0;
 static int sessionkey = 0;
@@ -73,6 +73,9 @@ static unsigned char encryptionkey[128];
 static char username[255];
 static char password[255];
 
+struct net_interface interfaces[MAX_INTERFACES];
+struct net_interface *active_interface;
+
 /* Protocol data direction */
 unsigned char mt_direction_fromserver = 0;
 
@@ -99,7 +102,7 @@ static int send_udp(struct mt_packet *packet, int retransmit) {
 
 		sent_bytes = sendto(send_socket, packet->data, packet->size, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
 	} else {
-		sent_bytes = send_custom_udp(sockfd, device_index, srcmac, dstmac, &sourceip,  sourceport, &destip, MT_MACTELNET_PORT, packet->data, packet->size);
+		sent_bytes = net_send_udp(sockfd, active_interface, srcmac, dstmac, &sourceip,  sourceport, &destip, MT_MACTELNET_PORT, packet->data, packet->size);
 	}
 
 	/* 
@@ -126,7 +129,7 @@ static int send_udp(struct mt_packet *packet, int retransmit) {
 			if (reads && FD_ISSET(insockfd, &read_fds)) {
 				unsigned char buff[1500];
 				int result;
-				
+
 				bzero(buff, 1500);
 				result = recvfrom(insockfd, buff, 1500, 0, 0, 0);
 
@@ -307,28 +310,39 @@ static int handle_packet(unsigned char *data, int data_len) {
 }
 
 static int find_interface() {
+	fd_set read_fds;
 	struct mt_packet data;
 	struct sockaddr_in myip;
-	int success;
-	char devicename[128];
-	int testsocket;
-	fd_set read_fds;
+	unsigned char emptymac[ETH_ALEN];
+	int i, testsocket;
 	struct timeval timeout;
 	int optval = 1;
-	
-	while ((success = get_ips(devicename, 128, &myip))) {
-		char str[INET_ADDRSTRLEN];
+
+	/* TODO: reread interfaces on HUP */
+	bzero(&interfaces, sizeof(struct net_interface) * MAX_INTERFACES);
+
+	bzero(emptymac, ETH_ALEN);
+
+	if (net_get_interfaces(interfaces, MAX_INTERFACES) <= 0) {
+		fprintf(stderr, "Error: No suitable devices found\n");
+		exit(1);
+	}
+
+	for (i = 0; i < MAX_INTERFACES; ++i) {
+		if (!interfaces[i].in_use) {
+			break;
+		}
 
 		/* Skip loopback interfaces */
-		if (memcmp("lo", devicename, 2) == 0) {
+		if (memcmp("lo", interfaces[i].name, 2) == 0) {
 			continue;
 		}
 
-		inet_ntop(AF_INET, &(myip.sin_addr), str, INET_ADDRSTRLEN);
-
 		/* Initialize receiving socket on the device chosen */
+		myip.sin_family = AF_INET;
+		memcpy((void *)&myip.sin_addr, interfaces[i].ipv4_addr, IPV4_ALEN);
 		myip.sin_port = htons(sourceport);
-	
+
 		/* Initialize socket and bind to udp port */
 		if ((testsocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
 			continue;
@@ -342,15 +356,16 @@ static int find_interface() {
 			continue;
 		}
 
-		/* Find the mac address for the current device */
-		if (get_device_mac(testsocket, devicename, srcmac) < 0) {
+		/* Ensure that we have mac-address for this interface  */
+		if (!interfaces[i].has_mac) {
 			close(testsocket);
 			continue;
 		}
 
-		/* Set the global socket handle for send_udp() */
+		/* Set the global socket handle and source mac address for send_udp() */
 		send_socket = testsocket;
-		device_index = get_device_index(testsocket, devicename);
+		memcpy(srcmac, interfaces[i].mac_addr, ETH_ALEN);
+		active_interface = &interfaces[i];
 
 		/* Send a SESSIONSTART message with the current device */
 		init_packet(&data, MT_PTYPE_SESSIONSTART, srcmac, dstmac, sessionkey, 0);
@@ -369,8 +384,6 @@ static int find_interface() {
 
 		close(testsocket);
 	}
-
-	/* We didn't find anything */
 	return 0;
 }
 
@@ -466,12 +479,7 @@ int main (int argc, char **argv) {
 			return 1;
 		}
 
-		/* Transmit raw packets with this socket */
-		sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
-		if (sockfd < 0) {
-			perror("sockfd");
-			return 1;
-		}
+		sockfd = net_init_raw_socket();
 	}
 
 	/* Receive regular udp packets with this socket */
@@ -522,7 +530,7 @@ int main (int argc, char **argv) {
 
 	/* Set up global info about the connection */
 	inet_pton(AF_INET, (char *)"255.255.255.255", &destip);
-	memcpy(&sourceip, &(si_me.sin_addr), 4);
+	memcpy(&sourceip, &(si_me.sin_addr), IPV4_ALEN);
 
 	/* Sessioon key */
 	sessionkey = rand() % 65535;
diff --git a/mactelnetd.c b/mactelnetd.c
index ed9c3f9..d3d462d 100644
--- a/mactelnetd.c
+++ b/mactelnetd.c
@@ -35,7 +35,11 @@
 #include <sys/types.h>
 #include <sys/socket.h>
 #include <string.h>
+#ifdef __linux__
 #include <linux/if_ether.h>
+#else
+#include <sys/time.h>
+#endif
 #include <sys/ioctl.h>
 #include <sys/stat.h>
 #include <sys/sysinfo.h>
@@ -45,14 +49,12 @@
 #include <sys/utsname.h>
 #include "md5.h"
 #include "protocol.h"
-#include "udp.h"
 #include "console.h"
-#include "devices.h"
+#include "interfaces.h"
 #include "users.h"
 #include "config.h"
 
 #define PROGRAM_NAME "MAC-Telnet Daemon"
-#define PROGRAM_VERSION "0.3.2"
 
 #define MAX_INSOCKETS 100
 
@@ -61,22 +63,13 @@
 /* Max ~5 pings per second */
 #define MT_MAXPPS MT_MNDP_BROADCAST_INTERVAL * 5
 
-struct mt_socket {
-	unsigned char ip[4];
-	unsigned char mac[ETH_ALEN];
-	char name[MT_INTERFACE_LEN];
-	int sockfd;
-	int device_index;
-};
-
 static int sockfd;
 static int insockfd;
 static int mndpsockfd;
 
 static int pings = 0;
 
-static struct mt_socket sockets[MAX_INSOCKETS];
-static int sockets_count = 0;
+struct net_interface interfaces[MAX_INTERFACES];
 
 static int use_raw_socket = 0;
 
@@ -101,7 +94,9 @@ enum mt_connection_state {
 
 /** Connection struct */
 struct mt_connection {
-	struct mt_socket *socket;
+	struct net_interface *interface;
+	char interface_name[256];
+
 	unsigned short seskey;
 	unsigned int incounter;
 	unsigned int outcounter;
@@ -117,10 +112,10 @@ struct mt_connection {
 
 	char username[30];
 	unsigned char trypassword[17];
-	unsigned char srcip[4];
-	unsigned char srcmac[6];
+	unsigned char srcip[IPV4_ALEN];
+	unsigned char srcmac[ETH_ALEN];
 	unsigned short srcport;
-	unsigned char dstmac[6];
+	unsigned char dstmac[ETH_ALEN];
 	unsigned char enckey[16];
 	unsigned short terminal_width;
 	unsigned short terminal_height;
@@ -187,7 +182,7 @@ static struct mt_connection *list_find_connection(unsigned short seskey, unsigne
 	}
 
 	for (p = connections_head; p != NULL; p = p->next) {
-		if (p->seskey == seskey && memcmp(srcmac, p->srcmac, 6) == 0) {
+		if (p->seskey == seskey && memcmp(srcmac, p->srcmac, ETH_ALEN) == 0) {
 			return p;
 		}
 	}
@@ -198,69 +193,62 @@ static struct mt_connection *list_find_connection(unsigned short seskey, unsigne
 static int find_socket(unsigned char *mac) {
 	int i;
 
-	for (i = 0; i < sockets_count; ++i) {
-		if (memcmp(mac, sockets[i].mac, ETH_ALEN) == 0) {
+	for (i = 0; i < MAX_INTERFACES; ++i) {
+		if (interfaces[i].in_use && memcmp(mac, interfaces[i].mac_addr, ETH_ALEN) == 0) {
 			return i;
 		}
+
+		if (!interfaces[i].in_use) {
+			break;
+		}
 	}
 	return -1;
 }
 
 static void setup_sockets() {
-	struct sockaddr_in myip;
-	char devicename[MT_INTERFACE_LEN];
-	unsigned char mac[ETH_ALEN];
-	unsigned char emptymac[ETH_ALEN];
-	int success;
-
-	memset(emptymac, 0, ETH_ALEN);
-
-	while ((success = get_macs(insockfd, devicename, MT_INTERFACE_LEN, mac))) {
-		if (memcmp(mac, emptymac, ETH_ALEN) != 0 && find_socket(mac) < 0) {
-			int optval = 1;
-			struct sockaddr_in si_me;
-			struct mt_socket *mysocket = &(sockets[sockets_count]);
+	int i;
 
-			memcpy(mysocket->mac, mac, ETH_ALEN);
-			strncpy(mysocket->name, devicename, MT_INTERFACE_LEN - 1);
-			mysocket->name[MT_INTERFACE_LEN - 1] = '\0';
+	for (i = 0; i < MAX_INTERFACES; ++i) {
+		int optval = 1;
+		struct sockaddr_in si_me;
+		struct ether_addr *mac = (struct ether_addr *)&(interfaces[i].mac_addr);
 
-			if (get_device_ip(insockfd, devicename, &myip) > 0) {
+		if (interfaces[i].in_use == 0 || !interfaces[i].has_mac) {
+			continue;
+		}
 
-				mysocket->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-				if (mysocket->sockfd < 0) {
-					close(mysocket->sockfd);
-					continue;
-				}
+		if (!use_raw_socket) {
+			interfaces[i].socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+			if (interfaces[i].socketfd < 0) {
+				continue;
+			}
 
-				if (setsockopt(mysocket->sockfd, SOL_SOCKET, SO_BROADCAST, &optval, sizeof (optval))==-1) {
-					perror("SO_BROADCAST");
-					continue;
-				}
+			if (setsockopt(interfaces[i].socketfd, SOL_SOCKET, SO_BROADCAST, &optval, sizeof (optval))==-1) {
+				perror("SO_BROADCAST");
+				continue;
+			}
 
-				setsockopt(mysocket->sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+			setsockopt(interfaces[i].socketfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
 
-				/* Initialize receiving socket on the device chosen */
-				si_me.sin_family = AF_INET;
-				si_me.sin_port = htons(MT_MACTELNET_PORT);
-				memcpy(&(si_me.sin_addr), &(myip.sin_addr), 4);
+			/* Initialize receiving socket on the device chosen */
+			si_me.sin_family = AF_INET;
+			si_me.sin_port = htons(MT_MACTELNET_PORT);
+			memcpy(&(si_me.sin_addr.s_addr), interfaces[i].ipv4_addr, IPV4_ALEN);
 
-				if (bind(mysocket->sockfd, (struct sockaddr *)&si_me, sizeof(si_me))==-1) {
-					fprintf(stderr, "Error binding to %s:%d, %s\n", inet_ntoa(si_me.sin_addr), sourceport, strerror(errno));
-					continue;
-				}
-				memcpy(mysocket->ip, &(myip.sin_addr), 4);
+			if (bind(interfaces[i].socketfd, (struct sockaddr *)&si_me, sizeof(si_me))==-1) {
+				fprintf(stderr, "Error binding to %s:%d, %s\n", inet_ntoa(si_me.sin_addr), sourceport, strerror(errno));
+				continue;
 			}
-			mysocket->device_index = get_device_index(insockfd, devicename);
-			
-			sockets_count++;
 		}
+
+		syslog(LOG_NOTICE, "Listening on %s for %s\n", interfaces[i].name, ether_ntoa(mac));
+
 	}
 }
 
 static int send_udp(const struct mt_connection *conn, const struct mt_packet *packet) {
 	if (use_raw_socket) {
-		return send_custom_udp(sockfd, conn->socket->device_index, conn->dstmac, conn->srcmac, &sourceip, sourceport, &destip, conn->srcport, packet->data, packet->size);
+		return net_send_udp(sockfd, conn->interface, conn->dstmac, conn->srcmac, &sourceip, sourceport, &destip, conn->srcport, packet->data, packet->size);
 	} else {
 		/* Init SendTo struct */
 		struct sockaddr_in socket_address;
@@ -268,16 +256,16 @@ static int send_udp(const struct mt_connection *conn, const struct mt_packet *pa
 		socket_address.sin_port = htons(conn->srcport);
 		socket_address.sin_addr.s_addr = htonl(INADDR_BROADCAST);
 
-		return sendto(conn->socket->sockfd, packet->data, packet->size, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
+		return sendto(conn->interface->socketfd, packet->data, packet->size, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
 	}
 }
 
-static int send_special_udp(const struct mt_socket *sock, unsigned short port, const struct mt_packet *packet) {
-	unsigned char dstmac[6];
-	
+static int send_special_udp(struct net_interface *interface, unsigned short port, const struct mt_packet *packet) {
+	unsigned char dstmac[ETH_ALEN];
+
 	if (use_raw_socket) {
-		memset(dstmac, 0xff, 6);
-		return send_custom_udp(sockfd, sock->device_index, sock->mac, dstmac, (const struct in_addr *)sock->ip, port, &destip, port, packet->data, packet->size);
+		memset(dstmac, 0xff, ETH_ALEN);
+		return net_send_udp(sockfd, interface, interface->mac_addr, dstmac, (const struct in_addr *)&interface->ipv4_addr, port, &destip, port, packet->data, packet->size);
 	} else {
 		/* Init SendTo struct */
 		struct sockaddr_in socket_address;
@@ -285,7 +273,7 @@ static int send_special_udp(const struct mt_socket *sock, unsigned short port, c
 		socket_address.sin_port = htons(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));
+		return sendto(interface->socketfd, packet->data, packet->size, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
 	}
 }
 
@@ -472,8 +460,15 @@ static void user_login(struct mt_connection *curconn, struct mt_mactelnet_hdr *p
 			setenv("SHELL", user->pw_shell, 1);
 			setenv("TERM", curconn->terminal_type, 1);
 			close(sockfd);
-			for (i = 0; i < sockets_count; ++i) {
-				close(sockets[i].sockfd);
+			close(insockfd);
+			for (i = 0; i < MAX_INTERFACES; ++i) {
+				if (interfaces[i].in_use && interfaces[i].socketfd > 0) {
+					close(interfaces[i].socketfd);
+				}
+				if (!interfaces[i].in_use) {
+					break;
+				}
+				
 			}
 			setsid();
 
@@ -612,22 +607,16 @@ static void handle_data_packet(struct mt_connection *curconn, struct mt_mactelne
 	}
 }
 
-static void terminate() {
-	syslog(LOG_NOTICE, "Exiting.");
-	exit(0);
-}
-
 static void handle_packet(unsigned char *data, int data_len, const struct sockaddr_in *address) {
 	struct mt_mactelnet_hdr pkthdr;
 	struct mt_connection *curconn = NULL;
 	struct mt_packet pdata;
-	int socketnum;
-	int i;
+	int interface_index;
 
 	parse_packet(data, &pkthdr);
 
 	/* Drop packets not belonging to us */
-	if ((socketnum = find_socket(pkthdr.dstaddr)) < 0) {
+	if ((interface_index = find_socket(pkthdr.dstaddr)) < 0) {
 		return;
 	}
 
@@ -639,11 +628,9 @@ static void handle_packet(unsigned char *data, int data_len, const struct sockad
 			}
 			init_pongpacket(&pdata, (unsigned char *)&(pkthdr.dstaddr), (unsigned char *)&(pkthdr.srcaddr));
 			add_packetdata(&pdata, pkthdr.data - 4, data_len - (MT_HEADER_LEN - 4));
-			for (i = 0; i < sockets_count; ++i) {
-				struct mt_socket *socket = &(sockets[i]);
-				if (memcmp(&(socket->mac), &(pkthdr.dstaddr), ETH_ALEN) == 0) {
-					send_special_udp(socket, MT_MACTELNET_PORT, &pdata);
-					break;
+			{
+				if (index >= 0) {
+					send_special_udp(&interfaces[interface_index], MT_MACTELNET_PORT, &pdata);
 				}
 			}
 			break;
@@ -654,11 +641,13 @@ static void handle_packet(unsigned char *data, int data_len, const struct sockad
 			curconn->seskey = pkthdr.seskey;
 			curconn->lastdata = time(NULL);
 			curconn->state = STATE_AUTH;
-			curconn->socket = &(sockets[socketnum]);
-			memcpy(curconn->srcmac, pkthdr.srcaddr, 6);
-			memcpy(curconn->srcip, &(address->sin_addr), 4);
+			curconn->interface = &interfaces[interface_index];
+			strncpy(curconn->interface_name, interfaces[interface_index].name, 254);
+			curconn->interface_name[255] = '\0';
+			memcpy(curconn->srcmac, pkthdr.srcaddr, ETH_ALEN);
+			memcpy(curconn->srcip, &(address->sin_addr), IPV4_ALEN);
 			curconn->srcport = htons(address->sin_port);
-			memcpy(curconn->dstmac, pkthdr.dstaddr, 6);
+			memcpy(curconn->dstmac, pkthdr.dstaddr, ETH_ALEN);
 
 			list_add_connection(curconn);
 
@@ -763,13 +752,10 @@ static void print_version() {
 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;
-	}
+#ifdef __linux__
+	struct sysinfo s_sysinfo;
 
 	if (sysinfo(&s_sysinfo) != 0) {
 		return;
@@ -777,13 +763,31 @@ void mndp_broadcast() {
 
 	/* Seems like ping uptime is transmitted as little endian? */
 	uptime = htole32(s_sysinfo.uptime);
+#else
+	struct timespec ts;
+
+	if (clock_gettime(CLOCK_UPTIME, &ts) != -1) {
+		uptime = htole32(((unsigned int)ts.tv_sec));
+	}
+#endif
 
-	for (i = 0; i < sockets_count; ++i) {
+	if (uname(&s_uname) != 0) {
+		return;
+	}
+
+	for (i = 0; i < MAX_INTERFACES; ++i) {
+		struct net_interface *interface = &interfaces[i];
 		struct mt_mndp_hdr *header = (struct mt_mndp_hdr *)&(pdata.data);
-		struct mt_socket *socket = &(sockets[i]);
+
+		if (interfaces[i].has_mac == 0) {
+			continue;
+		}
+		if (interfaces[i].in_use == 0) {
+			break;
+		}
 
 		mndp_init_packet(&pdata, 0, 1);
-		mndp_add_attribute(&pdata, MT_MNDPTYPE_ADDRESS, socket->mac, 6);
+		mndp_add_attribute(&pdata, MT_MNDPTYPE_ADDRESS, interface->mac_addr, ETH_ALEN);
 		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));
@@ -791,8 +795,77 @@ void mndp_broadcast() {
 		mndp_add_attribute(&pdata, MT_MNDPTYPE_TIMESTAMP, &uptime, 4);
 
 		header->cksum = in_cksum((unsigned short *)&(pdata.data), pdata.size);
+		send_special_udp(interface, MT_MNDP_PORT, &pdata);
+	}
+}
 
-		send_special_udp(socket, MT_MNDP_PORT, &pdata);
+void sigterm_handler() {
+	struct mt_connection *p;
+	struct mt_packet pdata;
+	char message[] = "\r\n\r\nDaemon shutting down.\r\n";
+
+	syslog(LOG_NOTICE, "Daemon shutting down");
+
+	for (p = connections_head; p != NULL; p = p->next) {
+		if (p->state == STATE_ACTIVE) {
+			init_packet(&pdata, MT_PTYPE_DATA, p->interface->mac_addr, p->srcmac, p->seskey, p->outcounter);
+			add_control_packet(&pdata, MT_CPTYPE_PLAINDATA, message, strlen(message));
+			send_udp(p, &pdata);
+
+			init_packet(&pdata, MT_PTYPE_END, p->interface->mac_addr, p->srcmac, p->seskey, p->outcounter);
+			send_udp(p, &pdata);
+		}
+	}
+
+	/* Doesn't hurt to tidy up */
+	close(sockfd);
+	close(insockfd);
+	if (!use_raw_socket) {
+		int i;
+		for (i = 0; i < MAX_INTERFACES; ++i) {
+			if (interfaces[i].in_use && interfaces[i].socketfd > 0)
+				close(interfaces[i].socketfd);
+		}
+	}
+	closelog();
+	exit(0);
+}
+
+void sighup_handler() {
+	int i;
+	struct mt_connection *p;
+
+	syslog(LOG_NOTICE, "SIGHUP: Reloading interfaces");
+
+	if (!use_raw_socket) {
+		for (i = 0; i < MAX_INTERFACES; ++i) {
+			close(interfaces[i].socketfd);
+		}
+	}
+
+	bzero(interfaces, sizeof(interfaces));
+
+	if (net_get_interfaces(interfaces, MAX_INTERFACES) <= 0) {
+		syslog(LOG_ERR, "No devices found! Exiting.\n");
+		exit(1);
+	}
+
+	setup_sockets();
+
+	/* Reassign outgoing interfaces to connections again, since they may have changed */
+	for (p = connections_head; p != NULL; p = p->next) {
+		if (p->interface_name != NULL) {
+			struct net_interface *interface = net_get_interface_ptr(interfaces, MAX_INTERFACES, p->interface_name, 0);
+			if (interface != NULL) {
+				p->interface = interface;
+			} else {
+				struct mt_connection tmp;
+				syslog(LOG_NOTICE, "(%d) Connection closed because interface %s is gone.", p->seskey, p->interface_name);
+				tmp.next = p->next;
+				list_remove_connection(p);
+				p = &tmp;
+			}
+		}
 	}
 }
 
@@ -809,6 +882,7 @@ int main (int argc, char **argv) {
 	int c,optval = 1;
 	int print_help = 0;
 	int foreground = 0;
+	int interface_count = 0;
 
 	while ((c = getopt(argc, argv, "fnvh?")) != -1) {
 		switch (c) {
@@ -860,11 +934,7 @@ int main (int argc, char **argv) {
 
 	if (use_raw_socket) {
 		/* Transmit raw packets with this socket */
-		sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
-		if (sockfd < 0) {
-			perror("sockfd");
-			return 1;
-		}
+		sockfd = net_init_raw_socket();
 	}
 
 	/* Receive regular udp packets with this socket */
@@ -874,7 +944,7 @@ int main (int argc, char **argv) {
 		return 1;
 	}
 
-	/* Set random source port */
+	/* Set source port */
 	sourceport = MT_MACTELNET_PORT;
 
 	/* Listen address*/
@@ -887,7 +957,7 @@ int main (int argc, char **argv) {
 	memset((char *) &si_me, 0, sizeof(si_me));
 	si_me.sin_family = AF_INET;
 	si_me.sin_port = htons(sourceport);
-	memcpy(&(si_me.sin_addr), &sourceip, 4);
+	memcpy(&(si_me.sin_addr), &sourceip, IPV4_ALEN);
 
 	setsockopt(insockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval));
 
@@ -901,23 +971,29 @@ int main (int argc, char **argv) {
 
 	/* Receive mndp udp packets with this socket */
 	mndpsockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-	if (insockfd < 0) {
-		perror("insockfd");
+	if (mndpsockfd < 0) {
+		perror("mndpsockfd");
 		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);
+	memcpy(&(si_me_mndp.sin_addr), &sourceip, IPV4_ALEN);
 
 	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));
+		fprintf(stderr, "MNDP: Error binding to %s:%d, %s\n", inet_ntoa(si_me_mndp.sin_addr), MT_MNDP_PORT, strerror(errno));
 	}
 
+	openlog("mactelnetd", LOG_PID, LOG_DAEMON);
+	syslog(LOG_NOTICE, "Bound to %s:%d", inet_ntoa(si_me.sin_addr), sourceport);
+
+	/* Enumerate available interfaces */
+	net_get_interfaces(interfaces, MAX_INTERFACES);
+
 	setup_sockets();
 
 	if (!foreground) {
@@ -929,23 +1005,20 @@ int main (int argc, char **argv) {
 	signal(SIGTSTP,SIG_IGN);
 	signal(SIGTTOU,SIG_IGN);
 	signal(SIGTTIN,SIG_IGN);	
+	signal(SIGHUP, sighup_handler);
+	signal(SIGTERM, sigterm_handler);
 
-	openlog("mactelnetd", LOG_PID, LOG_DAEMON);
-
-	syslog(LOG_NOTICE, "Bound to %s:%d", inet_ntoa(si_me.sin_addr), sourceport);
-
-	for (i = 0; i < sockets_count; ++i) {
-		struct mt_socket *socket = &(sockets[i]);
-		syslog(LOG_NOTICE, "Listening on %s for %16s\n", socket->name, ether_ntoa((struct ether_addr *)socket->mac));
+	for (i = 0; i < MAX_INTERFACES; ++i) {
+		if (interfaces[i].in_use && interfaces[i].has_mac) {
+			interface_count++;
+		}
 	}
 	
-	if (sockets_count == 0) {
-		syslog(LOG_ERR, "Unable to find the mac-address on any interfaces\n");
+	if (interface_count == 0) {
+		syslog(LOG_ERR, "Unable to find any valid network interfaces\n");
 		exit(1);
 	}
 
-	signal(SIGTERM, terminate);
-
 	while (1) {
 		int reads;
 		struct mt_connection *p;
@@ -1005,7 +1078,7 @@ int main (int argc, char **argv) {
 
 					/* Read it */
 					datalen = read(p->ptsfd, &keydata, 1024);
-					if (datalen != -1) {
+					if (datalen > 0) {
 						/* Send it */
 						init_packet(&pdata, MT_PTYPE_DATA, p->dstmac, p->srcmac, p->seskey, p->outcounter);
 						plen = add_control_packet(&pdata, MT_CPTYPE_PLAINDATA, &keydata, datalen);
@@ -1059,11 +1132,6 @@ int main (int argc, char **argv) {
 		}
 	}
 
-	close(sockfd);
-	close(insockfd);
-	for (i = 0; i < sockets_count; ++i) {
-		close(sockets[i].sockfd);
-	}
-	closelog();
+	/* Never reached */
 	return 0;
 }
diff --git a/protocol.c b/protocol.c
index 43fd6b0..53380d1 100644
--- a/protocol.c
+++ b/protocol.c
@@ -21,7 +21,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#ifdef __LINUX__
 #include <linux/if_ether.h>
+#endif
 #include <arpa/inet.h>
 #include <netinet/in.h>
 #include <netinet/ether.h>
@@ -157,10 +159,10 @@ void parse_packet(unsigned char *data, struct mt_mactelnet_hdr *pkthdr) {
 	pkthdr->ptype = data[1];
 
 	/* src ethernet addr */
-	memcpy(pkthdr->srcaddr, data+2,6);
+	memcpy(pkthdr->srcaddr, data + 2, ETH_ALEN);
 
 	/* dst ethernet addr */
-	memcpy(pkthdr->dstaddr, data+8,6);
+	memcpy(pkthdr->dstaddr, data + 8, ETH_ALEN);
 
 	if (mt_direction_fromserver) {
 		/* Session key */
@@ -168,10 +170,10 @@ void parse_packet(unsigned char *data, struct mt_mactelnet_hdr *pkthdr) {
 		pkthdr->seskey = ntohs(pkthdr->seskey);
 
 		/* server type */
-		memcpy(&(pkthdr->clienttype), data+16, 2);
+		memcpy(&(pkthdr->clienttype), data + 16, 2);
 	} else {
 		/* server type */
-		memcpy(&(pkthdr->clienttype), data+14, 2);
+		memcpy(&(pkthdr->clienttype), data + 14, 2);
 
 		/* Session key */
 		memcpy(&(pkthdr->seskey), data + 16, sizeof(pkthdr->seskey));
diff --git a/protocol.h b/protocol.h
index a450b2b..11c9aca 100644
--- a/protocol.h
+++ b/protocol.h
@@ -33,6 +33,13 @@
 #define MT_MNDP_TIMEOUT 5
 #define MT_MNDP_LONGTIMEOUT 120
 
+#ifndef ETH_ALEN
+#define ETH_ALEN 6
+#endif
+#ifndef IPV4_ALEN
+#define IPV4_ALEN 4
+#endif
+
 /* Packet type */
 enum mt_ptype {
 	MT_PTYPE_SESSIONSTART,
diff --git a/udp.c b/udp.c
deleted file mode 100644
index a3df4f9..0000000
--- a/udp.c
+++ /dev/null
@@ -1,179 +0,0 @@
-/*
-    Mac-Telnet - Connect to RouterOS or mactelnetd devices via MAC address
-    Copyright (C) 2010, Håkon Nessjøen <haakon.nessjoen@gmail.com>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License along
-    with this program; if not, write to the Free Software Foundation, Inc.,
-    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
-*/
-#include <malloc.h>
-#include <string.h>
-#include <math.h>
-#include <sys/socket.h>
-#include <linux/if_packet.h>
-#include <linux/if_ether.h>
-#include <linux/ip.h>
-#include <netinet/in.h>
-#include <linux/udp.h>
-#include <linux/if_arp.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-unsigned short in_cksum(unsigned short *addr, int len)
-{
-	int nleft = len;
-	int sum = 0;
-	unsigned short *w = addr;
-	unsigned short answer = 0;
-	
-	while (nleft > 1) {
-		sum += *w++;
-		nleft -= 2;
-	}
-	
-	if (nleft == 1) {
-		*(unsigned char *) (&answer) = *(unsigned char *) w;
-		sum += answer;
-	}
-	
-	sum = (sum >> 16) + (sum & 0xFFFF);
-	sum += (sum >> 16);
-	answer = ~sum;
-	return (answer);
-}
-
-unsigned short udp_sum_calc(unsigned char *src_addr,unsigned char *dst_addr, unsigned char *data, unsigned short len) {
-	unsigned short prot_udp=17;
-	unsigned short padd=0;
-	unsigned short word16;
-	unsigned int sum = 0;
-	int i;
-
-	/* Padding ? */
-	padd = (len % 2);
-	if (padd) {
-		data[len] = 0;
-	}
-
-	/* header+data */
-	for (i = 0; i < len + padd; i += 2){
-		word16 = ((data[i] << 8) & 0xFF00) + (data[i + 1] & 0xFF);
-		sum += word16;
-	}
-
-	/* source ip */
-	for (i = 0; i < 4; i += 2){
-		word16 = ((src_addr[i] << 8) & 0xFF00) + (src_addr[i + 1] & 0xFF);
-		sum += word16;
-	}
-
-	/* dest ip */
-	for (i = 0; i < 4; i += 2){
-		word16 = ((dst_addr[i] << 8) & 0xFF00) + (dst_addr[i + 1] & 0xFF);
-		sum += word16;
-	}
-
-	sum += prot_udp + len;
-
-	while (sum>>16)
-		sum = (sum & 0xFFFF) + (sum >> 16);
-
-	sum = ~sum;
-
-	if (sum == 0)
-		sum = 0xFFFF;
-
-	return (unsigned short) sum;
-}
-
-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) {
-	struct sockaddr_ll socket_address;
-
-	/*
-	 * Create a buffer for the full ethernet frame
-	 * and align header pointers to the correct positions.
-	*/
-	void* buffer = (void*)malloc(ETH_FRAME_LEN);
-	struct ethhdr *eh = (struct ethhdr *)buffer;
-	struct iphdr *ip = (struct iphdr *)(buffer+14);
-	struct udphdr *udp = (struct udphdr *)(buffer+14+20);
-	unsigned char *rest = (unsigned char *)(buffer+20+14+sizeof(struct udphdr));
-
-	if (((void *)rest - (void*)buffer) + datalen  > ETH_FRAME_LEN) {
-		fprintf(stderr, "packet size too large\n");
-		free(buffer);
-		return 0;
-	}
-
-	static unsigned int id = 1;
-	int send_result = 0;
-
-	/* Abort if we couldn't allocate enough memory */
-	if (buffer == NULL) {
-		perror("malloc");
-		exit(1);
-	}
-
-	/* Init ethernet header */
-	memcpy(eh->h_source, sourcemac, ETH_ALEN);
-	memcpy(eh->h_dest, destmac, ETH_ALEN);
-	eh->h_proto = htons(ETH_P_IP);
-
-	/* Init SendTo struct */
-	socket_address.sll_family   = PF_PACKET;
-	socket_address.sll_protocol = htons(ETH_P_IP);
-	socket_address.sll_ifindex  = ifindex;
-	socket_address.sll_hatype   = ARPHRD_ETHER;
-	socket_address.sll_pkttype  = PACKET_OTHERHOST;
-	socket_address.sll_halen    = ETH_ALEN;		
-
-	memcpy(socket_address.sll_addr, eh->h_source, ETH_ALEN);
-	socket_address.sll_addr[6]  = 0x00;/*not used*/
-	socket_address.sll_addr[7]  = 0x00;/*not used*/
-
-	/* Init IP Header */
-	ip->version = 4;
-	ip->ihl = 5;
-	ip->tos = 0x10;
-	ip->tot_len = htons(datalen + 8 + 20);
-	ip->id = htons(id++);
-	ip->frag_off = htons(0x4000);
-	ip->ttl = 64;
-	ip->protocol = 17; /* UDP */
-	ip->check = 0x0000;
-	ip->saddr = sourceip->s_addr;
-	ip->daddr = destip->s_addr;
-
-	/* Calculate checksum for IP header */
-	ip->check = in_cksum((unsigned short *)ip, sizeof(struct iphdr));
-
-	/* Init UDP Header */
-	udp->source = htons(sourceport);
-	udp->dest = htons(destport);
-	udp->len = htons(sizeof(struct udphdr) + datalen);
-	udp->check = 0;
-
-	/* Insert actual data */
-	memcpy(rest, data, datalen);
-
-	/* Add UDP checksum */
-	udp->check = udp_sum_calc((unsigned char *)&(ip->saddr), (unsigned char *)&(ip->daddr), (unsigned char *)udp, sizeof(struct udphdr) + datalen);
-	udp->check = htons(udp->check);
-
-	/* Send the packet */
-	send_result = sendto(socket, buffer, datalen+8+14+20, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
-	free(buffer);
-
-	/* Return amount of _data_ bytes sent */
-	return send_result-8-14-20;
-}