merge
[l2tpns.git] / icmp.c
1 // L2TPNS: icmp
2
3 char const *cvs_id_icmp = "$Id: icmp.c,v 1.3 2004-06-28 02:43:13 fred_nerk Exp $";
4
5 #include <arpa/inet.h>
6 #include <netdb.h>
7 #include <netinet/in.h>
8 #include <asm/types.h>
9 #include <linux/ip.h>
10 #include <linux/icmp.h>
11 #include <stdio.h>
12 #include <sys/socket.h>
13 #include <unistd.h>
14 #include <sys/types.h>
15 #include <sys/wait.h>
16 #include <memory.h>
17
18 #include "l2tpns.h"
19
20 __u16 _checksum(unsigned char *addr, int count);
21
22 void host_unreachable(ipt destination, u16 id, ipt source, char *packet, int packet_len)
23 {
24 char buf[128] = {0};
25 struct iphdr *iph;
26 struct icmphdr *icmp;
27 char *data;
28 int len = 0, on = 1, icmp_socket;
29 struct sockaddr_in whereto = {0};
30
31 if (!(icmp_socket = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)))
32 return;
33 setsockopt(icmp_socket, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on));
34
35 whereto.sin_addr.s_addr = destination;
36 whereto.sin_family = AF_INET;
37
38 iph = (struct iphdr *)(buf);
39 len = sizeof(struct iphdr);
40 icmp = (struct icmphdr *)(buf + len);
41 len += sizeof(struct icmphdr);
42 data = (char *)(buf + len);
43 len += (packet_len < 64) ? packet_len : 64;
44 memcpy(data, packet, (packet_len < 64) ? packet_len : 64);
45
46 iph->tos = 0;
47 iph->id = id;
48 iph->frag_off = 0;
49 iph->ttl = 30;
50 iph->check = 0;
51 iph->version = 4;
52 iph->ihl = 5;
53 iph->protocol = 1;
54 iph->check = 0;
55 iph->daddr = destination;
56 iph->saddr = source;
57
58 iph->tot_len = ntohs(len);
59
60 icmp->type = ICMP_DEST_UNREACH;
61 icmp->code = ICMP_HOST_UNREACH;
62 icmp->checksum = _checksum((char *)icmp, sizeof(struct icmphdr) + ((packet_len < 64) ? packet_len : 64));
63
64 iph->check = _checksum((char *)iph, sizeof(struct iphdr));
65
66 sendto(icmp_socket, (char *)buf, len, 0, (struct sockaddr *)&whereto, sizeof(struct sockaddr));
67 close(icmp_socket);
68 }
69
70 __u16 _checksum(unsigned char *addr, int count)
71 {
72 register long sum = 0;
73
74 for (; count > 1; count -= 2)
75 {
76 sum += ntohs(*(u32 *)addr);
77 addr += 2;
78 }
79
80 if (count > 1) sum += *(unsigned char *)addr;
81
82 // take only 16 bits out of the 32 bit sum and add up the carries
83 while (sum >> 16)
84 sum = (sum & 0xFFFF) + (sum >> 16);
85
86 // one's complement the result
87 sum = ~sum;
88
89 return htons((u16) sum);
90 }