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