f6c3e0dd0f45990368a096e979c13608c326a5a6
[l2tpns.git] / icmp.c
1 // L2TPNS: icmp
2
3 char const *cvs_id_icmp = "$Id: icmp.c,v 1.7 2005-01-25 04:19:05 bodea 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 <netinet/icmp6.h>
12 #include <stdio.h>
13 #include <sys/socket.h>
14 #include <unistd.h>
15 #include <sys/types.h>
16 #include <sys/wait.h>
17 #include <memory.h>
18
19 #include "l2tpns.h"
20
21 static uint16_t _checksum(unsigned char *addr, int count);
22
23 struct ipv6_pseudo_hdr {
24 struct in6_addr src;
25 struct in6_addr dest;
26 uint32_t ulp_length;
27 uint32_t zero : 24;
28 uint32_t nexthdr : 8;
29 };
30
31 void host_unreachable(in_addr_t destination, uint16_t id, in_addr_t source, char *packet, int packet_len)
32 {
33 char buf[128] = {0};
34 struct iphdr *iph;
35 struct icmphdr *icmp;
36 char *data;
37 int len = 0, on = 1, icmp_socket;
38 struct sockaddr_in whereto = {0};
39
40 if ((icmp_socket = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0)
41 return;
42
43 setsockopt(icmp_socket, IPPROTO_IP, IP_HDRINCL, (char *)&on, sizeof(on));
44
45 whereto.sin_addr.s_addr = destination;
46 whereto.sin_family = AF_INET;
47
48 iph = (struct iphdr *)(buf);
49 len = sizeof(struct iphdr);
50 icmp = (struct icmphdr *)(buf + len);
51 len += sizeof(struct icmphdr);
52 data = (char *)(buf + len);
53 len += (packet_len < 64) ? packet_len : 64;
54 memcpy(data, packet, (packet_len < 64) ? packet_len : 64);
55
56 iph->tos = 0;
57 iph->id = id;
58 iph->frag_off = 0;
59 iph->ttl = 30;
60 iph->check = 0;
61 iph->version = 4;
62 iph->ihl = 5;
63 iph->protocol = 1;
64 iph->check = 0;
65 iph->daddr = destination;
66 iph->saddr = source;
67
68 iph->tot_len = ntohs(len);
69
70 icmp->type = ICMP_DEST_UNREACH;
71 icmp->code = ICMP_HOST_UNREACH;
72 icmp->checksum = _checksum((char *) icmp, sizeof(struct icmphdr) + ((packet_len < 64) ? packet_len : 64));
73
74 iph->check = _checksum((char *) iph, sizeof(struct iphdr));
75
76 sendto(icmp_socket, (char *)buf, len, 0, (struct sockaddr *)&whereto, sizeof(struct sockaddr));
77 close(icmp_socket);
78 }
79
80 static uint16_t _checksum(unsigned char *addr, int count)
81 {
82 register long sum = 0;
83
84 for (; count > 1; count -= 2)
85 {
86 sum += ntohs(*(uint32_t *) addr);
87 addr += 2;
88 }
89
90 if (count > 1) sum += *(unsigned char *)addr;
91
92 // take only 16 bits out of the 32 bit sum and add up the carries
93 while (sum >> 16)
94 sum = (sum & 0xFFFF) + (sum >> 16);
95
96 // one's complement the result
97 sum = ~sum;
98
99 return htons((uint16_t) sum);
100 }
101
102 void send_ipv6_ra(tunnelidt t, sessionidt s, struct in6_addr *ip)
103 {
104 struct nd_opt_prefix_info *pinfo;
105 struct ipv6_pseudo_hdr *phdr;
106 uint8_t b[MAXETHER + 20];
107 uint8_t c[MAXETHER + 20];
108 int l;
109 uint8_t *o;
110
111 LOG(3, s, t, "Sending IPv6 RA\n");
112
113 memset(b, 0, sizeof(b));
114 o = makeppp(b, sizeof(b), 0, 0, t, s, PPPIPV6);
115
116 if (!o)
117 {
118 LOG(3, s, t, "failed to send IPv6 RA\n");
119 return;
120 }
121
122 *o = 0x60; // IPv6
123 *(o+1) = 0;
124 *(o+5) = 48; // Length of payload (not header)
125 *(o+6) = 58; // icmp6 is next
126 *(o+7) = 255; // Hop limit
127 memset(o+8, 0, 16); // source = FE80::1
128 *(o+8) = 0xFE;
129 *(o+9) = 0x80;
130 *(o+23) = 1;
131 if (ip != NULL)
132 memcpy(o+24, ip, 16); // dest = ip
133 else
134 {
135 // FF02::1 - all hosts
136 *(o+24) = 0xFF;
137 *(o+25) = 2;
138 *(o+39) = 1;
139 }
140 *(o+40) = 134; // RA message
141 *(o+41) = 0; // Code
142 *(o+42) = *(o+43) = 0; // Checksum
143 *(o+44) = 64; // Hop count
144 *(o+45) = 0; // Flags
145 *(o+46) = *(o+47) = 255; // Lifetime
146 *(uint32_t *)(o+48) = 0; // Reachable time
147 *(uint32_t *)(o+52) = 0; // Retrans timer
148 pinfo = (struct nd_opt_prefix_info *)(o+56);
149 pinfo->nd_opt_pi_type = ND_OPT_PREFIX_INFORMATION;
150 pinfo->nd_opt_pi_len = 4;
151 pinfo->nd_opt_pi_prefix_len = 64; // prefix length
152 pinfo->nd_opt_pi_flags_reserved = ND_OPT_PI_FLAG_ONLINK;
153 pinfo->nd_opt_pi_flags_reserved |= ND_OPT_PI_FLAG_AUTO;
154 pinfo->nd_opt_pi_valid_time = htonl(2592000);
155 pinfo->nd_opt_pi_preferred_time = htonl(604800);
156 pinfo->nd_opt_pi_reserved2 = 0;
157 pinfo->nd_opt_pi_prefix = config->ipv6_prefix;
158 l = sizeof(*pinfo) + 56;
159
160 memset(c, 0, sizeof(c));
161 phdr = (struct ipv6_pseudo_hdr *) c;
162 memcpy(&phdr->src, o+8, 16);
163 memcpy(&phdr->dest, o+24, 16);
164 phdr->ulp_length = htonl(l - 40);
165 phdr->nexthdr = IPPROTO_ICMPV6;
166
167 memcpy(c + sizeof(*phdr), o + 40, l - 40);
168
169 // Checksum is over the icmp6 payload plus the pseudo header
170 *(uint16_t *)(o+42) = _checksum(c, l - 40 + sizeof(*phdr));
171
172 tunnelsend(b, l + (o-b), t); // send it...
173 return;
174 }