X-Git-Url: http://git.sameswireless.fr/l2tpns.git/blobdiff_plain/7c1104efffac3af9f068ace3834f2720518ff54a..fc0a36320874bea43b9fd73df0e0990bfd3b59cd:/icmp.c

diff --git a/icmp.c b/icmp.c
new file mode 100644
index 0000000..0a56740
--- /dev/null
+++ b/icmp.c
@@ -0,0 +1,86 @@
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <linux/ip.h>
+#include <linux/icmp.h>
+#include <stdio.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/wait.h>
+#include <memory.h>
+#include "l2tpns.h"
+
+extern ipt myip;
+
+__u16 _checksum(unsigned char *addr, int count);
+
+void host_unreachable(ipt destination, u16 id, ipt source, char *packet, int packet_len)
+{
+	char buf[128] = {0};
+	struct iphdr *iph;
+	struct icmphdr *icmp;
+	char *data;
+	int len = 0, on = 1, icmp_socket;
+	struct sockaddr_in whereto = {0};
+
+	if (!(icmp_socket = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)))
+		return;
+	setsockopt(icmp_socket, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on));
+
+	whereto.sin_addr.s_addr = destination;
+	whereto.sin_family = AF_INET;
+
+	iph = (struct iphdr *)(buf);
+	len = sizeof(struct iphdr);
+	icmp = (struct icmphdr *)(buf + len);
+	len += sizeof(struct icmphdr);
+	data = (char *)(buf + len);
+	len += (packet_len < 64) ? packet_len : 64;
+	memcpy(data, packet, (packet_len < 64) ? packet_len : 64);
+
+	iph->tos = 0;
+	iph->id = id;
+	iph->frag_off = 0;
+	iph->ttl = 30;
+	iph->check = 0;
+	iph->version = 4;
+	iph->ihl = 5;
+	iph->protocol = 1;
+	iph->check = 0;
+	iph->daddr = destination;
+	iph->saddr = source;
+
+	iph->tot_len = ntohs(len);
+
+	icmp->type = ICMP_DEST_UNREACH;
+	icmp->code = ICMP_HOST_UNREACH;
+	icmp->checksum = _checksum((char *)icmp, sizeof(struct icmphdr) + ((packet_len < 64) ? packet_len : 64));
+
+	iph->check = _checksum((char *)iph, sizeof(struct iphdr));
+
+	sendto(icmp_socket, (char *)buf, len, 0, (struct sockaddr *)&whereto, sizeof(struct sockaddr));
+	close(icmp_socket);
+}
+
+__u16 _checksum(unsigned char *addr, int count)
+{
+	register long sum = 0;
+
+	for (; count > 1; count -= 2)
+	{
+		sum += ntohs(*(u32 *)addr);
+		addr += 2;
+	}
+
+	if (count > 1) sum += *(unsigned char *)addr;
+
+	// take only 16 bits out of the 32 bit sum and add up the carries
+	while (sum >> 16)
+		sum = (sum & 0xFFFF) + (sum >> 16);
+
+	// one's complement the result
+	sum = ~sum;
+
+	return htons((u16) sum);
+}