Added MT_PTYPE_PING/PONG support in mactelnetd and added a macping tool. For this udp-checksum code was also added for raw packets.

7 files changed, 483 insertions, 9 deletions

diff --git a/Makefile b/Makefile
index 92849ad..e3c1fa1 100644
--- a/Makefile
+++ b/Makefile
@@ -1,17 +1,18 @@
 
-all: mactelnet mactelnetd mndp
+all: mactelnet macping mactelnetd mndp
 
 clean: dist-clean
 
 dist-clean:
-	rm -f mactelnet mactelnetd mndp
+	rm -f mactelnet macping mactelnetd mndp
 
 install: all
 	cp mndp $(DESTDIR)/usr/bin/
+	cp macping $(DESTDIR)/usr/bin/
 	cp mactelnet $(DESTDIR)/usr/bin/
 	cp mactelnetd $(DESTDIR)/usr/sbin/
 	cp mactelnetd.users $(DESTDIR)/etc/
-	chown $(DESTDIR)/etc/mactelnetd.users
+	chown root $(DESTDIR)/etc/mactelnetd.users
 	chmod 600 $(DESTDIR)/etc/mactelnetd.users
 
 mactelnet: config.h udp.h udp.c mactelnet.c mactelnet.h protocol.c protocol.h console.c console.h devices.c devices.h md5.c md5.h
@@ -22,3 +23,6 @@ mactelnetd: config.h mactelnetd.c udp.h udp.c protocol.c protocol.h devices.c de
 
 mndp: config.h mndp.c protocol.c protocol.h
 	gcc -Wall -g -o mndp mndp.c protocol.c
+
+macping: config.h macping.c udp.c udp.h devices.c devices.h protocol.c protocol.h
+	gcc -Wall -g -o macping macping.c devices.c udp.c protocol.c
diff --git a/README.markdown b/README.markdown
index b10515c..07c1420 100644
--- a/README.markdown
+++ b/README.markdown
@@ -34,7 +34,7 @@ Usage
       -h        This help.
 
 
-    Example:
+Example:
 
     $ ./mactelnet 0:c:42:43:58:a5 -u admin
     Password: 
@@ -53,3 +53,30 @@ Usage
      
      [admin@HMG] >
 
+MAC-Ping usage
+--------------
+
+    # macping -h
+    Usage: ./macping <MAC> [-h] [-c <count>] [-s <packet size>]
+    
+    Parameters:
+      MAC       MAC-Address of the RouterOS/mactelnetd device.
+      -s        Specify size of ping packet.
+      -c        Number of packets to send. (0 = for ever)
+      -h        This help.
+
+Example:
+
+    # macping 0:c:42:43:58:a5
+    0:c:42:43:58:a5 56 byte, ping time 1.17 ms
+    0:c:42:43:58:a5 56 byte, ping time 1.07 ms
+    0:c:42:43:58:a5 56 byte, ping time 1.20 ms
+    0:c:42:43:58:a5 56 byte, ping time 0.65 ms
+    0:c:42:43:58:a5 56 byte, ping time 1.19 ms
+    
+    5 packets transmitted, 5 packets received, 0% packet loss
+    round-trip min/avg/max = 0.65/1.06/1.20 ms
+
+Or for use in bash-scripting:
+
+    # macping 0:c:42:43:58:a5 -c 2 >/dev/null 2>&1 || ( echo "No answer for 2 pings" | mail -s "router down" my.email@address.com )
diff --git a/macping.c b/macping.c
new file mode 100644
index 0000000..34ae0c6
--- /dev/null
+++ b/macping.c
@@ -0,0 +1,327 @@
+/*
+    Mac-Telnet - Connect to RouterOS routers 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 <stdlib.h>
+#include <signal.h>
+#include <stdio.h>
+#include <arpa/inet.h>
+#include <netinet/ether.h>
+#include <string.h>
+#include <time.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <float.h>
+#include "protocol.h"
+#include "udp.h"
+#include "devices.h"
+#include "config.h"
+
+#define MAX_DEVICES 128
+#define MT_INTERFACE_LEN 128
+
+#define PROGRAM_NAME "MAC-Ping"
+#define PROGRAM_VERSION "0.1"
+
+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;
+static struct in_addr sourceip;
+static struct in_addr destip;
+static unsigned char dstmac[6];
+
+static int ping_sent = 0;
+static int pong_received = 0;
+static float min_ms = FLT_MAX;
+static float avg_ms = 0;
+static float max_ms = 0;
+
+/* Protocol data direction, not used here, but obligatory for protocol.c */
+unsigned char mt_direction_fromserver = 0;
+
+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++;
+		}
+	}
+}
+
+long long int toddiff(struct timeval *tod1, struct timeval *tod2)
+{
+    long long t1, t2;
+    t1 = tod1->tv_sec * 1000000 + tod1->tv_usec;
+    t2 = tod2->tv_sec * 1000000 + tod2->tv_usec;
+    return t1 - t2;
+}
+
+void display_results() {
+	int percent = (int)((100.f/ping_sent) * pong_received);
+	if (percent > 100)
+		percent = 0;
+
+	if (percent < 0)
+		percent = 0;
+
+	if (min_ms == FLT_MAX)
+		min_ms = 0;
+
+	fprintf(stderr, "\n");
+	fprintf(stderr, "%d packets transmitted, %d packets received, %d%% packet loss\n", ping_sent, pong_received, 100 - percent);
+	fprintf(stderr, "round-trip min/avg/max = %.2f/%.2f/%.2f ms\n", min_ms, avg_ms/pong_received, max_ms);
+
+	/* For bash scripting */
+	if (pong_received == 0) {
+		exit(1);
+	}
+
+	exit(0);
+}
+
+int main(int argc, char **argv)  {
+	int optval = 1;
+	int print_help = 0;
+	int send_packets = 5;
+	int c;
+	struct sockaddr_in si_me;
+	struct mt_packet packet;
+	int i;
+
+	while (1) {
+		c = getopt(argc, argv, "s:c:hv?");
+
+		if (c == -1)
+			break;
+
+		switch (c) {
+			case 's':
+				ping_size = atoi(optarg) - 18;
+				break;
+
+			case 'v':
+				print_version();
+				exit(0);
+				break;
+
+			case 'c':
+				send_packets = atoi(optarg);
+				break;
+
+			case 'h':
+			case '?':
+				print_help = 1;
+				break;
+
+		}
+	}
+
+	if (argc - optind < 1 || print_help) {
+		print_version();
+		fprintf(stderr, "Usage: %s <MAC> [-h] [-c <count>] [-s <packet size>]\n", argv[0]);
+
+		if (print_help) {
+			fprintf(stderr, "\nParameters:\n");
+			fprintf(stderr, "  MAC       MAC-Address of the RouterOS/mactelnetd device.\n");
+			fprintf(stderr, "  -s        Specify size of ping packet.\n");
+			fprintf(stderr, "  -c        Number of packets to send. (0 = unlimited)\n");
+			fprintf(stderr, "  -h        This help.\n");
+			fprintf(stderr, "\n");
+		}
+		return 1;
+	}
+
+	if (ping_size > ETH_FRAME_LEN - 42) {
+		fprintf(stderr, "Packet size must be between 18 and %d\n", ETH_FRAME_LEN - 42 + 18);
+		exit(1);
+	}
+
+	if (geteuid() != 0) {
+		fprintf(stderr, "You need to have root privileges to use %s.\n", argv[0]);
+		return 1;
+	}
+
+	ether_aton_r(argv[optind], (struct ether_addr *)dstmac);
+
+	/* Open a UDP socket handle */
+	sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+	if (sockfd < 0) {
+		perror("sockfd");
+		return 1;
+	}
+
+	insockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (insockfd < 0) {
+		perror("insockfd");
+		return 1;
+	}
+
+	/* Set initialize address/port */
+	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_addr.s_addr = htonl(INADDR_ANY);
+
+	setsockopt(insockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval));
+
+	/* Bind to specified address/port */
+	if (bind(insockfd, (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 1;
+	}
+
+	/* Listen address*/
+	inet_pton(AF_INET, (char *)"0.0.0.0", &sourceip);
+
+	/* Set up global info about the connection */
+	inet_pton(AF_INET, (char *)"255.255.255.255", &destip);
+
+	srand(time(NULL));
+
+	setup_devices();
+
+	if (ping_size < sizeof(struct timeval)) {
+		ping_size = sizeof(struct timeval);
+	}
+
+	signal(SIGINT, display_results);
+
+	for (i = 0; i < send_packets || send_packets == 0; ++i) {
+		fd_set read_fds;
+		static struct timeval lasttimestamp;
+		int reads, result;
+		struct timeval timeout;
+		int ii;
+		int sent = 0;
+		int waitforpacket;
+		struct timeval timestamp;
+		unsigned char pingdata[1500];
+
+		gettimeofday(&timestamp, NULL);
+		memcpy(pingdata, &timestamp, sizeof(timestamp));
+		for (ii = sizeof(timestamp); ii < ping_size; ++ii) {
+			pingdata[ii] = rand() % 256;
+		}
+
+		for (ii = 0; ii < devices_count; ++ii) {
+			struct mt_device *device = &devices[ii];
+
+			init_pingpacket(&packet, device->mac, 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);
+
+			if (result > 0) {
+				sent++;
+			}
+
+		}
+		if (sent == 0) {
+			fprintf(stderr, "Error sending packet.\n");
+			continue;
+		}
+		ping_sent++;
+
+		FD_ZERO(&read_fds);
+		FD_SET(insockfd, &read_fds);
+
+		timeout.tv_sec = 1;
+		timeout.tv_usec = 0;
+
+		waitforpacket = 1;
+
+		while (waitforpacket) {
+			/* Wait for data or timeout */
+			reads = select(insockfd+1, &read_fds, NULL, NULL, &timeout);
+			if (reads > 0) {
+				unsigned char buff[1500];
+				struct sockaddr_in saddress;
+				unsigned int slen = sizeof(saddress);
+				struct mt_mactelnet_hdr pkthdr;
+
+				result = recvfrom(insockfd, buff, 1500, 0, (struct sockaddr *)&saddress, &slen);
+				parse_packet(buff, &pkthdr);
+
+				/* TODO: Check that we are the receiving host */
+				if (pkthdr.ptype == MT_PTYPE_PONG) {
+					struct timeval pongtimestamp;
+					struct timeval nowtimestamp;
+
+					waitforpacket = 0;
+					gettimeofday(&nowtimestamp, NULL);
+
+					memcpy(&pongtimestamp, pkthdr.data - 4, sizeof(pongtimestamp));
+					if (memcmp(pkthdr.data - 4, pingdata, ping_size) == 0) {
+						float diff = toddiff(&nowtimestamp, &pongtimestamp) / 1000.0f;
+
+						if (diff < min_ms)
+							min_ms = diff;
+
+						if (diff > max_ms)
+							max_ms = diff;
+
+						avg_ms += diff;
+
+						printf("%s %d byte, ping time %.2f ms%s\n", ether_ntoa((struct ether_addr *)&(pkthdr.srcaddr)), result, diff, (char *)(memcmp(&pongtimestamp,&lasttimestamp,sizeof(lasttimestamp)) == 0 ? " DUP" : ""));
+					} else {
+						printf("%s Reply of %d bytes of unequal data\n", ether_ntoa((struct ether_addr *)&(pkthdr.srcaddr)), result);
+					}
+					pong_received++;
+					memcpy(&lasttimestamp, &pongtimestamp, sizeof(pongtimestamp));
+					sleep(1);
+				} else {
+					/* Wait for the correct packet */
+					continue;
+				}
+			} else {
+				waitforpacket = 0;
+				fprintf(stderr, "%s ping timeout\n", ether_ntoa((struct ether_addr *)&dstmac));
+			}
+		}
+	}
+
+	/* Display statistics and exit */
+	display_results();
+
+	return 0;
+}
diff --git a/mactelnetd.c b/mactelnetd.c
index 7e2b1dc..ef05bb7 100644
--- a/mactelnetd.c
+++ b/mactelnetd.c
@@ -56,6 +56,9 @@
 
 #define MT_INTERFACE_LEN 128
 
+/* 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];
@@ -68,6 +71,8 @@ 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;
 
@@ -262,17 +267,17 @@ 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) {
+static int send_special_udp(const struct mt_socket *sock, unsigned short port, const struct mt_packet *packet) {
 	unsigned char dstmac[6];
 	
 	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, MT_MNDP_PORT, &destip, MT_MNDP_PORT, packet->data, packet->size);
+		return send_custom_udp(sockfd, sock->device_index, sock->mac, dstmac, (const struct in_addr *)sock->ip, port, &destip, 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_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));
@@ -322,7 +327,7 @@ static void uwtmp_login(struct mt_connection *conn) {
 	strncpy(utent.ut_line, line, sizeof(utent.ut_line));
 	strncpy(utent.ut_id, utent.ut_line + 3, sizeof(utent.ut_id));
 	strncpy(utent.ut_host,ether_ntoa((const struct ether_addr *)conn->srcmac), sizeof(utent.ut_host));
-	time(&utent.ut_time);
+	time((time_t *)&(utent.ut_time));
 	
 	/* Update utmp and/or wtmp */
 	setutent();
@@ -600,6 +605,7 @@ static void handle_packet(unsigned char *data, int data_len, const struct sockad
 	struct mt_connection *curconn = NULL;
 	struct mt_packet pdata;
 	int socketnum;
+	int i;
 
 	parse_packet(data, &pkthdr);
 
@@ -610,6 +616,21 @@ static void handle_packet(unsigned char *data, int data_len, const struct sockad
 
 	switch (pkthdr.ptype) {
 
+		case MT_PTYPE_PING:
+			if (pings++ > MT_MAXPPS) {
+				break;
+			}
+			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;
+				}
+			}
+			break;
+
 		case MT_PTYPE_SESSIONSTART:
 			syslog(LOG_DEBUG, "(%d) New connection from %s.", pkthdr.seskey, ether_ntoa((struct ether_addr*)&(pkthdr.srcaddr)));
 			curconn = calloc(1, sizeof(struct mt_connection));
@@ -754,7 +775,7 @@ void mndp_broadcast() {
 
 		header->cksum = in_cksum((unsigned short *)&(pdata.data), pdata.size);
 
-		send_mndp_udp(socket, &pdata);
+		send_special_udp(socket, MT_MNDP_PORT, &pdata);
 	}
 }
 
@@ -984,6 +1005,7 @@ int main (int argc, char **argv) {
 		time(&now);
 		
 		if (now - last_mndp_time > MT_MNDP_BROADCAST_INTERVAL) {
+			pings = 0;
 			mndp_broadcast();
 			last_mndp_time = now;
 		}
diff --git a/protocol.c b/protocol.c
index 84ecb1e..3c3e6f8 100644
--- a/protocol.c
+++ b/protocol.c
@@ -110,6 +110,40 @@ int add_control_packet(struct mt_packet *packet, enum mt_cptype cptype, void *cp
 	return MT_CPHEADER_LEN + data_len;
 }
 
+int init_pingpacket(struct mt_packet *packet, unsigned char *srcmac, unsigned char *dstmac) {
+	init_packet(packet, MT_PTYPE_PING, srcmac, dstmac, 0, 0);
+
+	/* Zero out sessionkey & counter */
+	bzero(packet->data + 14, 4);
+
+	/* Remove data counter field from header */
+	packet->size -= 4;
+	return packet->size;
+}
+
+int init_pongpacket(struct mt_packet *packet, unsigned char *srcmac, unsigned char *dstmac) {
+	init_packet(packet, MT_PTYPE_PONG, srcmac, dstmac, 0, 0);
+
+	/* Zero out sessionkey & counter */
+	bzero(packet->data + 14, 4);
+
+	/* Remove data counter field from header */
+	packet->size -= 4;
+	return packet->size;
+}
+
+int add_packetdata(struct mt_packet *packet, unsigned char *data, unsigned short length) {
+	if (packet->size + length > MT_PACKET_LEN) {
+		fprintf(stderr, "add_control_packet: ERROR, too large packet. Exceeds %d bytes\n", MT_PACKET_LEN);
+		return -1;
+	}
+
+	memcpy(packet->data + packet->size, data, length);
+	packet->size += length;
+
+	return length;
+}
+
 void parse_packet(unsigned char *data, struct mt_mactelnet_hdr *pkthdr) {
 	/* Packet version */
 	pkthdr->ver = data[0];
diff --git a/protocol.h b/protocol.h
index b2066ba..3ca641f 100644
--- a/protocol.h
+++ b/protocol.h
@@ -38,6 +38,8 @@ enum mt_ptype {
 	MT_PTYPE_SESSIONSTART,
 	MT_PTYPE_DATA,
 	MT_PTYPE_ACK,
+	MT_PTYPE_PING = 4,
+	MT_PTYPE_PONG,
 	MT_PTYPE_END = 255
 };
 
@@ -114,6 +116,11 @@ extern int add_control_packet(struct mt_packet *packet, enum mt_cptype cptype, v
 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);
 
+/* MAC-Ping packets */
+int init_pingpacket(struct mt_packet *packet, unsigned char *srcmac, unsigned char *dstmac);
+int init_pongpacket(struct mt_packet *packet, unsigned char *srcmac, unsigned char *dstmac);
+int add_packetdata(struct mt_packet *packet, unsigned char *data, unsigned short length);
+
 /* MNDP packets */
 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);
diff --git a/udp.c b/udp.c
index 68cb6c5..b1e6540 100644
--- a/udp.c
+++ b/udp.c
@@ -34,6 +34,50 @@ unsigned short in_cksum(unsigned short *addr, int len)
 	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 + padd]=0;
+	}
+
+	/* header+data */
+	for (i = 0; i < len + padd; i += 2){
+		word16 = ((data[i] << 8) & 0xFF00) + (data[i + 1] & 0xFF);
+		sum += (unsigned long)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;
 
@@ -47,6 +91,12 @@ int send_custom_udp(const int socket, const int ifindex, const unsigned char *so
 	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;
 
@@ -85,6 +135,7 @@ int send_custom_udp(const int socket, const int ifindex, const unsigned char *so
 	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));
 
@@ -97,6 +148,8 @@ int send_custom_udp(const int socket, const int ifindex, const unsigned char *so
 	/* Insert actual data */
 	memcpy(rest, data, datalen);
 
+	udp->check = htons(udp_sum_calc((unsigned char *)&(ip->saddr), (unsigned char *)&(ip->daddr), (unsigned char *)udp, sizeof(struct udphdr) + datalen));
+
 	/* Send the packet */
 	send_result = sendto(socket, buffer, datalen+8+14+20, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
 	free(buffer);