Cleaned up src/dst mac/ip handling a bit. Now fully automates source ip/mac addresses according to ethernet device chosen.

7 files changed, 70 insertions, 40 deletions

diff --git a/devices.c b/devices.c
index 387c73c..9293783 100644
--- a/devices.c
+++ b/devices.c
@@ -3,6 +3,7 @@
 #include <malloc.h>
 #include <unistd.h>
 #include <netinet/in.h>
+#include <linux/if_ether.h>
 #include <sys/ioctl.h>
 #include <net/if.h>
 
@@ -17,6 +18,19 @@ int getDeviceIndex(int sockfd, unsigned char *deviceName) {
 	return ifr.ifr_ifindex;
 }
 
+int getDeviceMAC(const int sockfd, const unsigned char *deviceName, unsigned char *mac) {
+	struct ifreq ifr;
+
+	strncpy(ifr.ifr_name, deviceName, 16);
+	if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) != 0) {
+		return -1;
+	}
+
+	memcpy(mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+
+	return 1;
+}
+
 int getDeviceIp(const int sockfd, const unsigned char *deviceName, struct sockaddr_in *ip) {
 	struct ifconf ifc;
 	struct ifreq *ifr;
@@ -36,8 +50,8 @@ int getDeviceIp(const int sockfd, const unsigned char *deviceName, struct sockad
 
 	if (ioctl(sockfd, SIOCGIFCONF, &ifc) != 0) {
 		free(ifr);
-                return -1;
-        }
+		return -1;
+	}
 
 	numDevices = ifc.ifc_len / sizeof(struct ifreq);
 	for (i = 0; i < numDevices; ++i) {
diff --git a/devices.h b/devices.h
index 69ba246..e2c260c 100644
--- a/devices.h
+++ b/devices.h
@@ -1,2 +1,3 @@
 extern int getDeviceIndex(int sockfd, unsigned char *deviceName);
+extern int getDeviceMAC(const int sockfd, const unsigned char *deviceName, unsigned char *mac);
 extern int getDeviceIp(const int sockfd, const unsigned char *deviceName, struct sockaddr_in *ip);
diff --git a/mactelnet.c b/mactelnet.c
index 4094974..8cfab77 100644
--- a/mactelnet.c
+++ b/mactelnet.c
@@ -18,10 +18,11 @@
 */
 #include <string.h>
 #include <stdio.h>
+#include <linux/if_ether.h>
 #include "mactelnet.h"
 #include "config.h"
 
-int initPacket(unsigned char *data, unsigned char ptype, unsigned char *src, unsigned char *dst, unsigned short sessionkey, unsigned short counter) {
+int initPacket(unsigned char *data, unsigned char ptype, unsigned char *srcmac, unsigned char *dstmac, unsigned short sessionkey, unsigned short counter) {
 
 	// PACKET VERSION
 	data[0] = 1;
@@ -30,10 +31,10 @@ int initPacket(unsigned char *data, unsigned char ptype, unsigned char *src, uns
 	data[1] = ptype;
 
 	// src ethernet address
-	etherAddrton(data + 2, src);
+	memcpy(data + 2, srcmac, ETH_ALEN);
 
 	// dst ethernet address
-	etherAddrton(data + 8, dst);
+	memcpy(data + 8, dstmac, ETH_ALEN);
 
 	data[14] = sessionkey >> 8;
 	data[15] = sessionkey & 0xff;
@@ -49,7 +50,7 @@ int initPacket(unsigned char *data, unsigned char ptype, unsigned char *src, uns
 	return 22;
 }
 
-int addControlPacket(unsigned char *data, unsigned char cptype, unsigned char *cpdata, int data_len) {
+int addControlPacket(unsigned char *data, unsigned char cptype, void *cpdata, int data_len) {
 	data[0] = 0x56;
 	data[1] = 0x34;
 	data[2] = 0x12;
diff --git a/mactelnet.h b/mactelnet.h
index 44f5ca1..c9c5c02 100644
--- a/mactelnet.h
+++ b/mactelnet.h
@@ -54,4 +54,9 @@ struct mt_mactelnet_control_hdr {
 	unsigned char *data;
 };
 
+extern int initPacket(unsigned char *data, unsigned char ptype, unsigned char *srcmac, unsigned char *dstmac, unsigned short sessionkey, unsigned short counter);
+extern int addControlPacket(unsigned char *data, unsigned 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);
+
 #endif
diff --git a/main.c b/main.c
index 60dbd3c..5644448 100644
--- a/main.c
+++ b/main.c
@@ -20,6 +20,7 @@
 #include <stdio.h>
 #include <arpa/inet.h>
 #include <netinet/in.h>
+#include <netinet/ether.h>
 #include <sys/types.h>
 #include <sys/socket.h>
 #include <string.h>
@@ -32,12 +33,17 @@
 #include "config.h"
 
 int sockfd;
+int deviceIndex;
 int counter=0;
 int outcounter=0;
 int sessionkey=0;
-unsigned char *src = "00:e0:81:b5:ac:8e";
-unsigned char dstmem[] = "00:0c:42:43:58:a4";
-unsigned char *dst = dstmem;
+
+unsigned char srcmac[ETH_ALEN];
+unsigned char dstmac[ETH_ALEN];
+
+struct in_addr sourceip; 
+struct in_addr destip;
+
 unsigned char encryptionkey[128];
 unsigned char username[255];
 unsigned char password[255];
@@ -53,7 +59,7 @@ void sendAuthData(unsigned char *username, unsigned char *password) {
 	int plen;
 	int databytes;
 
-	plen = initPacket(data, MT_PTYPE_DATA, src, dst, sessionkey, outcounter);
+	plen = initPacket(data, MT_PTYPE_DATA, srcmac, dstmac, sessionkey, outcounter);
 	databytes = plen;
 	plen += addControlPacket(data + plen, MT_CPTYPE_PASSWORD, password, 17);
 	plen += addControlPacket(data + plen, MT_CPTYPE_USERNAME, username, userLen);
@@ -66,7 +72,7 @@ void sendAuthData(unsigned char *username, unsigned char *password) {
 
 	outcounter += plen - databytes;
 
-	result = sendCustomUDP(sockfd, src, dst, "213.236.240.252", 20561, "255.255.255.255", 20561, data, plen);
+	result = sendCustomUDP(sockfd, deviceIndex, srcmac, dstmac, &sourceip, 20561, &destip, 20561, data, plen);
 }
 
 void handlePacket(unsigned char *data, int data_len) {
@@ -83,8 +89,8 @@ void handlePacket(unsigned char *data, int data_len) {
 		int rest = 0;
 		unsigned char *p = data;
 		counter += data_len - 22;
-		plen = initPacket(odata, MT_PTYPE_ACK, src, dst, pkthdr.seskey, counter);
-		result = sendCustomUDP(sockfd, src, dst, "213.236.240.252", 20561, "255.255.255.255", 20561, odata, plen);
+		plen = initPacket(odata, MT_PTYPE_ACK, srcmac, dstmac, pkthdr.seskey, counter);
+		result = sendCustomUDP(sockfd, deviceIndex, srcmac, dstmac, &sourceip, 20561, &destip, 20561, odata, plen);
 
 		if (DEBUG)
 			printf("ACK: Plen = %d, Send result: %d\n", plen, result);
@@ -131,12 +137,12 @@ void handlePacket(unsigned char *data, int data_len) {
 	else if (pkthdr.ptype == MT_PTYPE_END) {
 		char odata[200];
 		int plen=0,result=0;
-		plen = initPacket(odata, MT_PTYPE_END, src, dst, pkthdr.seskey, 0);
-		result = sendCustomUDP(sockfd, src, dst, "213.236.240.252", 20561, "255.255.255.255", 20561, odata, plen);
+		plen = initPacket(odata, MT_PTYPE_END, srcmac, dstmac, pkthdr.seskey, 0);
+		result = sendCustomUDP(sockfd, deviceIndex, srcmac, dstmac, &sourceip, 20561, &destip, 20561, odata, plen);
 		fprintf(stderr, "Connection closed.\n");
 		exit(0);
 	} else {
-		fprintf(stderr, "Unhandeled packet type: %d received from server %s\n", pkthdr.ptype, dst);
+		fprintf(stderr, "Unhandeled packet type: %d received from server %s\n", pkthdr.ptype, ether_ntoa((struct ether_addr *)dstmac));
 	}
 }
 
@@ -147,14 +153,13 @@ int main (int argc, char **argv) {
 	struct sockaddr_in si_me;
 	char buff[1500];
 	int plen = 0;
-	int deviceIndex;
 
 	if (argc < 4) {
 		fprintf(stderr, "Usage: %s <ifname> <MAC> <username> <password>\n", argv[0]);
 		return 1;
 	}
 
-	strncpy(dst, argv[2], 17);
+	etherAddrton(dstmac, argv[2]);
 	strncpy(username, argv[3], 254);
 	strncpy(password, argv[4], 254);
 
@@ -181,6 +186,16 @@ int main (int argc, char **argv) {
 		return 1;
 	}
 
+	result = getDeviceMAC(sockfd, argv[1], srcmac);
+	if (result < 0) {
+		fprintf(stderr, "Cannot determine MAC address of device %s\n", argv[1]);
+		return 1;
+	}
+
+	// Set up global info about the connection
+	inet_pton(AF_INET, (char *)"255.255.255.255", &destip);
+	memcpy(&sourceip, &(si_me.sin_addr), 4);
+
 	// Initialize receiving socket on the device chosen
 	memset((char *) &si_me, 0, sizeof(si_me));
 	si_me.sin_family = AF_INET;
@@ -195,10 +210,10 @@ int main (int argc, char **argv) {
 	// Sessioon key
 	sessionkey = rand() % 65535;
 
-	printf("Connecting to %s...\n", dst);
+	printf("Connecting to %s...\n", ether_ntoa((struct ether_addr *)dstmac));
 
-	plen = initPacket(data, MT_PTYPE_SESSIONSTART, src, dst, sessionkey, 0);
-	result = sendCustomUDP(sockfd, src, dst, inet_ntoa(si_me.sin_addr), 20561, "255.255.255.255", 20561, data, plen);
+	plen = initPacket(data, MT_PTYPE_SESSIONSTART, srcmac, dstmac, sessionkey, 0);
+	result = sendCustomUDP(sockfd, deviceIndex, srcmac, dstmac, &sourceip, 20561, &destip, 20561, data, plen);
 	if (DEBUG)
 		printf("Plen = %d, Send result: %d\n", plen, result);
 	if (DEBUG)
@@ -209,11 +224,11 @@ int main (int argc, char **argv) {
 
 	// TODO: Should resubmit whenever a PTYPE_DATA packet is sent, and an ACK packet with correct datacounter is received
 	// or time out the connection, in all other cases.
-	plen = initPacket(data, MT_PTYPE_DATA, src, dst, sessionkey, 0);
+	plen = initPacket(data, MT_PTYPE_DATA, srcmac, dstmac, sessionkey, 0);
 	plen += addControlPacket(data + plen, MT_CPTYPE_BEGINAUTH, NULL, 0);
 	outcounter += 9;
 
-	result = sendCustomUDP(sockfd, src, dst, "213.236.240.252", 20561, "255.255.255.255", 20561, data, plen);
+	result = sendCustomUDP(sockfd, deviceIndex, srcmac, dstmac, &sourceip, 20561, &destip, 20561, data, plen);
 	if (DEBUG)
 		printf("Plen = %d, Send result: %d\n", plen, result);
 
diff --git a/udp.c b/udp.c
index 2881f89..066ec32 100644
--- a/udp.c
+++ b/udp.c
@@ -63,51 +63,45 @@ void etherAddrton(unsigned char *dest, const unsigned char *mac) {
 	} while (*p++ && *p);
 }
 
-int sendCustomUDP(const int socket, const char *sourcemac, const char *destmac, const char *sourceip, const int sourceport, const char *destip, const int destport, const char *data, const int datalen) {
+int sendCustomUDP(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 char *data, const int datalen) {
 	struct sockaddr_ll socket_address;
-	struct in_addr srcipaddr;
-	struct in_addr dstipaddr;
 	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 *resten = (unsigned char *)(buffer+20+14+sizeof(struct udphdr));
+	unsigned char *rest = (unsigned char *)(buffer+20+14+sizeof(struct udphdr));
 	static unsigned int id = 1;
 	int send_result = 0;
 
 	// Ethernet header
-	etherAddrton(eh->h_source, sourcemac);
-	etherAddrton(eh->h_dest, destmac);
+	memcpy(eh->h_source, sourcemac, ETH_ALEN);
+	memcpy(eh->h_dest, destmac, ETH_ALEN);
 	eh->h_proto = 8;
 
 	// SendTo struct
 	socket_address.sll_family   = PF_PACKET;	
 	socket_address.sll_protocol = htons(ETH_P_IP);	
-	socket_address.sll_ifindex  = 2;
+	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, 6);
+	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*/
 
-	// TODO: errorhandling
-	inet_aton(sourceip, &srcipaddr);
-	inet_aton(destip, &dstipaddr);
-
 	// IP Header
 	ip->version = 4;
 	ip->ihl = 5;
 	ip->tos = 0x10;
-	ip->tot_len = htons(datalen+8+20);
+	ip->tot_len = htons(datalen + 8 + 20);
 	ip->id = htons(id++);
 	ip->frag_off = 0x0040;
 	ip->ttl = 64;
 	ip->protocol = 17; // UDP
 	ip->check = 0x0000;
-	ip->saddr = srcipaddr.s_addr;
-	ip->daddr = dstipaddr.s_addr;
+	ip->saddr = sourceip->s_addr;
+	ip->daddr = destip->s_addr;
 	ip->check = in_cksum((unsigned short *)ip, sizeof(struct iphdr));
 
 	// UDP Header
@@ -116,7 +110,7 @@ int sendCustomUDP(const int socket, const char *sourcemac, const char *destmac,
 	udp->check = 0;
 	udp->len = htons(sizeof(struct udphdr) + datalen);
 
-	memcpy(resten, data, datalen);
+	memcpy(rest, data, datalen);
 
 	/*send the packet*/
 	send_result = sendto(socket, buffer, datalen+8+14+20, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
diff --git a/udp.h b/udp.h
index d368371..9f3644c 100644
--- a/udp.h
+++ b/udp.h
@@ -1,5 +1,5 @@
 #ifndef _UDP_H
 #define _UDP_H 1
-extern int sendCustomUDP(const int socket, const char *sourcemac, const char *destmac, const char *sourceip, const int sourceport, const char *destip, const int destport, const char *data, const int datalen);
+extern int sendCustomUDP(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 char *data, const int datalen);
 extern void etherAddrton(unsigned char *dest, const unsigned char *mac);
 #endif