c1be6cb0ae4a607ce54f1bb1cf0519c1ff49a484
[oweals/tinc.git] / src / route.c
1 /*
2     route.c -- routing
3     Copyright (C) 2000-2005 Ivo Timmermans,
4                   2000-2014 Guus Sliepen <guus@tinc-vpn.org>
5
6     This program is free software; you can redistribute it and/or modify
7     it under the terms of the GNU General Public License as published by
8     the Free Software Foundation; either version 2 of the License, or
9     (at your option) any later version.
10
11     This program is distributed in the hope that it will be useful,
12     but WITHOUT ANY WARRANTY; without even the implied warranty of
13     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14     GNU General Public License for more details.
15
16     You should have received a copy of the GNU General Public License along
17     with this program; if not, write to the Free Software Foundation, Inc.,
18     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
19 */
20
21 #include "system.h"
22
23 #include "avl_tree.h"
24 #include "connection.h"
25 #include "ethernet.h"
26 #include "ipv4.h"
27 #include "ipv6.h"
28 #include "logger.h"
29 #include "net.h"
30 #include "protocol.h"
31 #include "route.h"
32 #include "subnet.h"
33 #include "utils.h"
34
35 rmode_t routing_mode = RMODE_ROUTER;
36 fmode_t forwarding_mode = FMODE_INTERNAL;
37 bmode_t broadcast_mode = BMODE_MST;
38 bool decrement_ttl = false;
39 bool directonly = false;
40 bool priorityinheritance = false;
41 int macexpire = 600;
42 bool overwrite_mac = false;
43 mac_t mymac = {{0xFE, 0xFD, 0, 0, 0, 0}};
44
45 /* Sizes of various headers */
46
47 static const size_t ether_size = sizeof(struct ether_header);
48 static const size_t arp_size = sizeof(struct ether_arp);
49 static const size_t ip_size = sizeof(struct ip);
50 static const size_t icmp_size = sizeof(struct icmp) - sizeof(struct ip);
51 static const size_t ip6_size = sizeof(struct ip6_hdr);
52 static const size_t icmp6_size = sizeof(struct icmp6_hdr);
53 static const size_t ns_size = sizeof(struct nd_neighbor_solicit);
54 static const size_t opt_size = sizeof(struct nd_opt_hdr);
55
56 #ifndef MAX
57 #define MAX(a, b) ((a) > (b) ? (a) : (b))
58 #endif
59
60 /* RFC 1071 */
61
62 static uint16_t inet_checksum(void *data, int len, uint16_t prevsum) {
63         uint16_t *p = data;
64         uint32_t checksum = prevsum ^ 0xFFFF;
65
66         while(len >= 2) {
67                 checksum += *p++;
68                 len -= 2;
69         }
70         
71         if(len)
72                 checksum += *(uint8_t *)p;
73
74         while(checksum >> 16)
75                 checksum = (checksum & 0xFFFF) + (checksum >> 16);
76
77         return ~checksum;
78 }
79
80 static bool ratelimit(int frequency) {
81         static time_t lasttime = 0;
82         static int count = 0;
83         
84         if(lasttime == now) {
85                 if(count >= frequency)
86                         return true;
87         } else {
88                 lasttime = now;
89                 count = 0;
90         }
91
92         count++;
93         return false;
94 }
95
96 static bool checklength(node_t *source, vpn_packet_t *packet, length_t length) {
97         if(packet->len < length) {
98                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Got too short packet from %s (%s)", source->name, source->hostname);
99                 return false;
100         } else
101                 return true;
102 }
103
104 static void clamp_mss(const node_t *source, const node_t *via, vpn_packet_t *packet) {
105         if(!source || !via || !(via->options & OPTION_CLAMP_MSS))
106                 return;
107
108         uint16_t mtu = source->mtu;
109         if(via != myself && via->mtu < mtu)
110                 mtu = via->mtu;
111
112         /* Find TCP header */
113         int start = ether_size;
114         uint16_t type = packet->data[12] << 8 | packet->data[13];
115
116         if(type == ETH_P_8021Q) {
117                 start += 4;
118                 type = packet->data[16] << 8 | packet->data[17];
119         }
120
121         if(type == ETH_P_IP && packet->data[start + 9] == 6)
122                 start += (packet->data[start] & 0xf) * 4;
123         else if(type == ETH_P_IPV6 && packet->data[start + 6] == 6)
124                 start += 40;
125         else
126                 return;
127
128         if(packet->len <= start + 20)
129                 return;
130
131         /* Use data offset field to calculate length of options field */
132         int len = ((packet->data[start + 12] >> 4) - 5) * 4;
133
134         if(packet->len < start + 20 + len)
135                 return;
136
137         /* Search for MSS option header */
138         for(int i = 0; i < len;) {
139                 if(packet->data[start + 20 + i] == 0)
140                         break;
141
142                 if(packet->data[start + 20 + i] == 1) {
143                         i++;
144                         continue;
145                 }
146
147                 if(i > len - 2 || i > len - packet->data[start + 21 + i])
148                         break;
149
150                 if(packet->data[start + 20 + i] != 2) {
151                         if(packet->data[start + 21 + i] < 2)
152                                 break;
153                         i += packet->data[start + 21 + i];
154                         continue;
155                 }
156
157                 if(packet->data[start + 21] != 4)
158                         break;
159
160                 /* Found it */
161                 uint16_t oldmss = packet->data[start + 22 + i] << 8 | packet->data[start + 23 + i];
162                 uint16_t newmss = mtu - start - 20;
163                 uint32_t csum = packet->data[start + 16] << 8 | packet->data[start + 17];
164
165                 if(oldmss <= newmss)
166                         break;
167                 
168                 ifdebug(TRAFFIC) logger(LOG_INFO, "Clamping MSS of packet from %s to %s to %d", source->name, via->name, newmss);
169
170                 /* Update the MSS value and the checksum */
171                 packet->data[start + 22 + i] = newmss >> 8;
172                 packet->data[start + 23 + i] = newmss & 0xff;
173                 csum ^= 0xffff;
174                 csum += oldmss ^ 0xffff;
175                 csum += newmss;
176                 csum = (csum & 0xffff) + (csum >> 16);
177                 csum += csum >> 16;
178                 csum ^= 0xffff;
179                 packet->data[start + 16] = csum >> 8;
180                 packet->data[start + 17] = csum;
181                 break;
182         }
183 }
184
185 static void swap_mac_addresses(vpn_packet_t *packet) {
186         mac_t tmp;
187         memcpy(&tmp, &packet->data[0], sizeof tmp);
188         memcpy(&packet->data[0], &packet->data[6], sizeof tmp);
189         memcpy(&packet->data[6], &tmp, sizeof tmp);
190 }
191         
192 static void learn_mac(mac_t *address) {
193         subnet_t *subnet;
194         avl_node_t *node;
195         connection_t *c;
196
197         subnet = lookup_subnet_mac(myself, address);
198
199         /* If we don't know this MAC address yet, store it */
200
201         if(!subnet) {
202                 ifdebug(TRAFFIC) logger(LOG_INFO, "Learned new MAC address %x:%x:%x:%x:%x:%x",
203                                    address->x[0], address->x[1], address->x[2], address->x[3],
204                                    address->x[4], address->x[5]);
205
206                 subnet = new_subnet();
207                 subnet->type = SUBNET_MAC;
208                 subnet->expires = now + macexpire;
209                 subnet->net.mac.address = *address;
210                 subnet->weight = 10;
211                 subnet_add(myself, subnet);
212                 subnet_update(myself, subnet, true);
213
214                 /* And tell all other tinc daemons it's our MAC */
215
216                 for(node = connection_tree->head; node; node = node->next) {
217                         c = node->data;
218                         if(c->status.active)
219                                 send_add_subnet(c, subnet);
220                 }
221         }
222
223         if(subnet->expires)
224                 subnet->expires = now + macexpire;
225 }
226
227 void age_subnets(void) {
228         subnet_t *s;
229         connection_t *c;
230         avl_node_t *node, *next, *node2;
231
232         for(node = myself->subnet_tree->head; node; node = next) {
233                 next = node->next;
234                 s = node->data;
235                 if(s->expires && s->expires <= now) {
236                         ifdebug(TRAFFIC) {
237                                 char netstr[MAXNETSTR];
238                                 if(net2str(netstr, sizeof netstr, s))
239                                         logger(LOG_INFO, "Subnet %s expired", netstr);
240                         }
241
242                         for(node2 = connection_tree->head; node2; node2 = node2->next) {
243                                 c = node2->data;
244                                 if(c->status.active)
245                                         send_del_subnet(c, s);
246                         }
247
248                         subnet_update(myself, s, false);
249                         subnet_del(myself, s);
250                 }
251         }
252 }
253
254 /* RFC 792 */
255
256 static void route_ipv4_unreachable(node_t *source, vpn_packet_t *packet, length_t ether_size, uint8_t type, uint8_t code) {
257         struct ip ip = {0};
258         struct icmp icmp = {0};
259         
260         struct in_addr ip_src;
261         struct in_addr ip_dst;
262         uint32_t oldlen;
263
264         if(ratelimit(3))
265                 return;
266         
267         /* Swap Ethernet source and destination addresses */
268
269         swap_mac_addresses(packet);
270
271         /* Copy headers from packet into properly aligned structs on the stack */
272
273         memcpy(&ip, packet->data + ether_size, ip_size);
274
275         /* Remember original source and destination */
276         
277         ip_src = ip.ip_src;
278         ip_dst = ip.ip_dst;
279
280         /* Try to reply with an IP address assigned to the local machine */
281
282         if (type == ICMP_TIME_EXCEEDED && code == ICMP_EXC_TTL) {
283                 int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
284                 if (sockfd != -1) {
285                         struct sockaddr_in addr;
286                         memset(&addr, 0, sizeof(addr));
287                         addr.sin_family = AF_INET;
288                         addr.sin_addr = ip.ip_src;
289                         if (!connect(sockfd, (const struct sockaddr*) &addr, sizeof(addr))) {
290                                 memset(&addr, 0, sizeof(addr));
291                                 addr.sin_family = AF_INET;
292                                 socklen_t addrlen = sizeof(addr);
293                                 if (!getsockname(sockfd, (struct sockaddr*) &addr, &addrlen) && addrlen <= sizeof(addr)) {
294                                         ip_dst = addr.sin_addr;
295                                 }
296                         }
297                         close(sockfd);
298                 }
299         }
300
301         oldlen = packet->len - ether_size;
302
303         if(type == ICMP_DEST_UNREACH && code == ICMP_FRAG_NEEDED)
304                 icmp.icmp_nextmtu = htons(packet->len - ether_size);
305
306         if(oldlen >= IP_MSS - ip_size - icmp_size)
307                 oldlen = IP_MSS - ip_size - icmp_size;
308         
309         /* Copy first part of original contents to ICMP message */
310         
311         memmove(packet->data + ether_size + ip_size + icmp_size, packet->data + ether_size, oldlen);
312
313         /* Fill in IPv4 header */
314         
315         ip.ip_v = 4;
316         ip.ip_hl = ip_size / 4;
317         ip.ip_tos = 0;
318         ip.ip_len = htons(ip_size + icmp_size + oldlen);
319         ip.ip_id = 0;
320         ip.ip_off = 0;
321         ip.ip_ttl = 255;
322         ip.ip_p = IPPROTO_ICMP;
323         ip.ip_sum = 0;
324         ip.ip_src = ip_dst;
325         ip.ip_dst = ip_src;
326
327         ip.ip_sum = inet_checksum(&ip, ip_size, ~0);
328         
329         /* Fill in ICMP header */
330         
331         icmp.icmp_type = type;
332         icmp.icmp_code = code;
333         icmp.icmp_cksum = 0;
334         
335         icmp.icmp_cksum = inet_checksum(&icmp, icmp_size, ~0);
336         icmp.icmp_cksum = inet_checksum(packet->data + ether_size + ip_size + icmp_size, oldlen, icmp.icmp_cksum);
337
338         /* Copy structs on stack back to packet */
339
340         memcpy(packet->data + ether_size, &ip, ip_size);
341         memcpy(packet->data + ether_size + ip_size, &icmp, icmp_size);
342         
343         packet->len = ether_size + ip_size + icmp_size + oldlen;
344
345         send_packet(source, packet);
346 }
347
348 /* RFC 791 */
349
350 static void fragment_ipv4_packet(node_t *dest, vpn_packet_t *packet, length_t ether_size) {
351         struct ip ip;
352         vpn_packet_t fragment;
353         int len, maxlen, todo;
354         uint8_t *offset;
355         uint16_t ip_off, origf;
356         
357         memcpy(&ip, packet->data + ether_size, ip_size);
358         fragment.priority = packet->priority;
359
360         if(ip.ip_hl != ip_size / 4)
361                 return;
362         
363         todo = ntohs(ip.ip_len) - ip_size;
364
365         if(ether_size + ip_size + todo != packet->len) {
366                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Length of packet (%d) doesn't match length in IPv4 header (%d)", packet->len, (int)(ether_size + ip_size + todo));
367                 return;
368         }
369
370         ifdebug(TRAFFIC) logger(LOG_INFO, "Fragmenting packet of %d bytes to %s (%s)", packet->len, dest->name, dest->hostname);
371
372         offset = packet->data + ether_size + ip_size;
373         maxlen = (dest->mtu - ether_size - ip_size) & ~0x7;
374         ip_off = ntohs(ip.ip_off);
375         origf = ip_off & ~IP_OFFMASK;
376         ip_off &= IP_OFFMASK;
377         
378         while(todo) {
379                 len = todo > maxlen ? maxlen : todo;
380                 memcpy(fragment.data + ether_size + ip_size, offset, len);
381                 todo -= len;
382                 offset += len;
383
384                 ip.ip_len = htons(ip_size + len);
385                 ip.ip_off = htons(ip_off | origf | (todo ? IP_MF : 0));
386                 ip.ip_sum = 0;
387                 ip.ip_sum = inet_checksum(&ip, ip_size, ~0);
388                 memcpy(fragment.data, packet->data, ether_size);
389                 memcpy(fragment.data + ether_size, &ip, ip_size);
390                 fragment.len = ether_size + ip_size + len;
391
392                 send_packet(dest, &fragment);
393
394                 ip_off += len / 8;
395         }       
396 }
397
398 static void route_ipv4_unicast(node_t *source, vpn_packet_t *packet) {
399         subnet_t *subnet;
400         node_t *via;
401         ipv4_t dest;
402
403         memcpy(&dest, &packet->data[30], sizeof dest);
404         subnet = lookup_subnet_ipv4(&dest);
405
406         if(!subnet) {
407                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet from %s (%s): unknown IPv4 destination address %d.%d.%d.%d",
408                                 source->name, source->hostname,
409                                 dest.x[0],
410                                 dest.x[1],
411                                 dest.x[2],
412                                 dest.x[3]);
413
414                 route_ipv4_unreachable(source, packet, ether_size, ICMP_DEST_UNREACH, ICMP_NET_UNKNOWN);
415                 return;
416         }
417         
418         if(subnet->owner == source) {
419                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Packet looping back to %s (%s)!", source->name, source->hostname);
420                 return;
421         }
422
423         if(!subnet->owner->status.reachable)
424                 return route_ipv4_unreachable(source, packet, ether_size, ICMP_DEST_UNREACH, ICMP_NET_UNREACH);
425
426         if(forwarding_mode == FMODE_OFF && source != myself && subnet->owner != myself)
427                 return route_ipv4_unreachable(source, packet, ether_size, ICMP_DEST_UNREACH, ICMP_NET_ANO);
428
429         if(priorityinheritance)
430                 packet->priority = packet->data[15];
431
432         via = (subnet->owner->via == myself) ? subnet->owner->nexthop : subnet->owner->via;
433
434         if(via == source) {
435                 ifdebug(TRAFFIC) logger(LOG_ERR, "Routing loop for packet from %s (%s)!", source->name, source->hostname);
436                 return;
437         }
438         
439         if(directonly && subnet->owner != via)
440                 return route_ipv4_unreachable(source, packet, ether_size, ICMP_DEST_UNREACH, ICMP_NET_ANO);
441
442         if(via && packet->len > MAX(via->mtu, 590) && via != myself) {
443                 ifdebug(TRAFFIC) logger(LOG_INFO, "Packet for %s (%s) length %d larger than MTU %d", subnet->owner->name, subnet->owner->hostname, packet->len, via->mtu);
444                 if(packet->data[20] & 0x40) {
445                         packet->len = MAX(via->mtu, 590);
446                         route_ipv4_unreachable(source, packet, ether_size, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED);
447                 } else {
448                         fragment_ipv4_packet(via, packet, ether_size);
449                 }
450
451                 return;
452         }
453
454         clamp_mss(source, via, packet);
455  
456         send_packet(subnet->owner, packet);
457 }
458
459 static void route_ipv4(node_t *source, vpn_packet_t *packet) {
460         if(!checklength(source, packet, ether_size + ip_size))
461                 return;
462
463         if(broadcast_mode && (((packet->data[30] & 0xf0) == 0xe0) || (
464                         packet->data[30] == 255 &&
465                         packet->data[31] == 255 &&
466                         packet->data[32] == 255 &&
467                         packet->data[33] == 255)))
468                 broadcast_packet(source, packet);
469         else
470                 route_ipv4_unicast(source, packet);
471 }
472
473 /* RFC 2463 */
474
475 static void route_ipv6_unreachable(node_t *source, vpn_packet_t *packet, length_t ether_size, uint8_t type, uint8_t code) {
476         struct ip6_hdr ip6;
477         struct icmp6_hdr icmp6 = {0};
478         uint16_t checksum;      
479
480         struct {
481                 struct in6_addr ip6_src;        /* source address */
482                 struct in6_addr ip6_dst;        /* destination address */
483                 uint32_t length;
484                 uint32_t next;
485         } pseudo;
486
487         if(ratelimit(3))
488                 return;
489         
490         /* Swap Ethernet source and destination addresses */
491
492         swap_mac_addresses(packet);
493
494         /* Copy headers from packet to structs on the stack */
495
496         memcpy(&ip6, packet->data + ether_size, ip6_size);
497
498         /* Remember original source and destination */
499         
500         pseudo.ip6_src = ip6.ip6_dst;
501         pseudo.ip6_dst = ip6.ip6_src;
502
503         /* Try to reply with an IP address assigned to the local machine */
504
505         if (type == ICMP6_TIME_EXCEEDED && code == ICMP6_TIME_EXCEED_TRANSIT) {
506                 int sockfd = socket(AF_INET6, SOCK_DGRAM, 0);
507                 if (sockfd != -1) {
508                         struct sockaddr_in6 addr;
509                         memset(&addr, 0, sizeof(addr));
510                         addr.sin6_family = AF_INET6;
511                         addr.sin6_addr = ip6.ip6_src;
512                         if (!connect(sockfd, (const struct sockaddr*) &addr, sizeof(addr))) {
513                                 memset(&addr, 0, sizeof(addr));
514                                 addr.sin6_family = AF_INET6;
515                                 socklen_t addrlen = sizeof(addr);
516                                 if (!getsockname(sockfd, (struct sockaddr*) &addr, &addrlen) && addrlen <= sizeof(addr)) {
517                                         pseudo.ip6_src = addr.sin6_addr;
518                                 }
519                         }
520                         close(sockfd);
521                 }
522         }
523
524         pseudo.length = packet->len - ether_size;
525
526         if(type == ICMP6_PACKET_TOO_BIG)
527                 icmp6.icmp6_mtu = htonl(pseudo.length);
528         
529         if(pseudo.length >= IP_MSS - ip6_size - icmp6_size)
530                 pseudo.length = IP_MSS - ip6_size - icmp6_size;
531         
532         /* Copy first part of original contents to ICMP message */
533         
534         memmove(packet->data + ether_size + ip6_size + icmp6_size, packet->data + ether_size, pseudo.length);
535
536         /* Fill in IPv6 header */
537         
538         ip6.ip6_flow = htonl(0x60000000UL);
539         ip6.ip6_plen = htons(icmp6_size + pseudo.length);
540         ip6.ip6_nxt = IPPROTO_ICMPV6;
541         ip6.ip6_hlim = 255;
542         ip6.ip6_src = pseudo.ip6_src;
543         ip6.ip6_dst = pseudo.ip6_dst;
544
545         /* Fill in ICMP header */
546         
547         icmp6.icmp6_type = type;
548         icmp6.icmp6_code = code;
549         icmp6.icmp6_cksum = 0;
550
551         /* Create pseudo header */
552                 
553         pseudo.length = htonl(icmp6_size + pseudo.length);
554         pseudo.next = htonl(IPPROTO_ICMPV6);
555
556         /* Generate checksum */
557         
558         checksum = inet_checksum(&pseudo, sizeof(pseudo), ~0);
559         checksum = inet_checksum(&icmp6, icmp6_size, checksum);
560         checksum = inet_checksum(packet->data + ether_size + ip6_size + icmp6_size, ntohl(pseudo.length) - icmp6_size, checksum);
561
562         icmp6.icmp6_cksum = checksum;
563
564         /* Copy structs on stack back to packet */
565
566         memcpy(packet->data + ether_size, &ip6, ip6_size);
567         memcpy(packet->data + ether_size + ip6_size, &icmp6, icmp6_size);
568         
569         packet->len = ether_size + ip6_size + ntohl(pseudo.length);
570         
571         send_packet(source, packet);
572 }
573
574 static void route_ipv6_unicast(node_t *source, vpn_packet_t *packet) {
575         subnet_t *subnet;
576         node_t *via;
577         ipv6_t dest;
578
579         memcpy(&dest, &packet->data[38], sizeof dest);
580         subnet = lookup_subnet_ipv6(&dest);
581
582         if(!subnet) {
583                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet from %s (%s): unknown IPv6 destination address %hx:%hx:%hx:%hx:%hx:%hx:%hx:%hx",
584                                 source->name, source->hostname,
585                                 ntohs(dest.x[0]),
586                                 ntohs(dest.x[1]),
587                                 ntohs(dest.x[2]),
588                                 ntohs(dest.x[3]),
589                                 ntohs(dest.x[4]),
590                                 ntohs(dest.x[5]),
591                                 ntohs(dest.x[6]),
592                                 ntohs(dest.x[7]));
593
594                 route_ipv6_unreachable(source, packet, ether_size, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_ADDR);
595                 return;
596         }
597
598         if(subnet->owner == source) {
599                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Packet looping back to %s (%s)!", source->name, source->hostname);
600                 return;
601         }
602
603         if(!subnet->owner->status.reachable)
604                 return route_ipv6_unreachable(source, packet, ether_size, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_NOROUTE);
605
606         if(forwarding_mode == FMODE_OFF && source != myself && subnet->owner != myself)
607                 return route_ipv6_unreachable(source, packet, ether_size, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_ADMIN);
608
609         via = (subnet->owner->via == myself) ? subnet->owner->nexthop : subnet->owner->via;
610         
611         if(via == source) {
612                 ifdebug(TRAFFIC) logger(LOG_ERR, "Routing loop for packet from %s (%s)!", source->name, source->hostname);
613                 return;
614         }
615         
616         if(directonly && subnet->owner != via)
617                 return route_ipv6_unreachable(source, packet, ether_size, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_ADMIN);
618
619         if(via && packet->len > MAX(via->mtu, 1294) && via != myself) {
620                 ifdebug(TRAFFIC) logger(LOG_INFO, "Packet for %s (%s) length %d larger than MTU %d", subnet->owner->name, subnet->owner->hostname, packet->len, via->mtu);
621                 packet->len = MAX(via->mtu, 1294);
622                 route_ipv6_unreachable(source, packet, ether_size, ICMP6_PACKET_TOO_BIG, 0);
623                 return;
624         }
625
626         clamp_mss(source, via, packet);
627  
628         send_packet(subnet->owner, packet);
629 }
630
631 /* RFC 2461 */
632
633 static void route_neighborsol(node_t *source, vpn_packet_t *packet) {
634         struct ip6_hdr ip6;
635         struct nd_neighbor_solicit ns;
636         struct nd_opt_hdr opt;
637         subnet_t *subnet;
638         uint16_t checksum;
639         bool has_opt;
640
641         struct {
642                 struct in6_addr ip6_src;        /* source address */
643                 struct in6_addr ip6_dst;        /* destination address */
644                 uint32_t length;
645                 uint32_t next;
646         } pseudo;
647
648         if(!checklength(source, packet, ether_size + ip6_size + ns_size))
649                 return;
650         
651         has_opt = packet->len >= ether_size + ip6_size + ns_size + opt_size + ETH_ALEN;
652         
653         if(source != myself) {
654                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Got neighbor solicitation request from %s (%s) while in router mode!", source->name, source->hostname);
655                 return;
656         }
657
658         /* Copy headers from packet to structs on the stack */
659
660         memcpy(&ip6, packet->data + ether_size, ip6_size);
661         memcpy(&ns, packet->data + ether_size + ip6_size, ns_size);
662         if(has_opt)
663                 memcpy(&opt, packet->data + ether_size + ip6_size + ns_size, opt_size);
664
665         /* First, snatch the source address from the neighbor solicitation packet */
666
667         if(overwrite_mac)
668                 memcpy(mymac.x, packet->data + ETH_ALEN, ETH_ALEN);
669
670         /* Check if this is a valid neighbor solicitation request */
671
672         if(ns.nd_ns_hdr.icmp6_type != ND_NEIGHBOR_SOLICIT ||
673            (has_opt && opt.nd_opt_type != ND_OPT_SOURCE_LINKADDR)) {
674                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet: received unknown type neighbor solicitation request");
675                 return;
676         }
677
678         /* Create pseudo header */
679
680         pseudo.ip6_src = ip6.ip6_src;
681         pseudo.ip6_dst = ip6.ip6_dst;
682         if(has_opt)
683                 pseudo.length = htonl(ns_size + opt_size + ETH_ALEN);
684         else
685                 pseudo.length = htonl(ns_size);
686         pseudo.next = htonl(IPPROTO_ICMPV6);
687
688         /* Generate checksum */
689
690         checksum = inet_checksum(&pseudo, sizeof(pseudo), ~0);
691         checksum = inet_checksum(&ns, ns_size, checksum);
692         if(has_opt) {
693                 checksum = inet_checksum(&opt, opt_size, checksum);
694                 checksum = inet_checksum(packet->data + ether_size + ip6_size + ns_size + opt_size, ETH_ALEN, checksum);
695         }
696
697         if(checksum) {
698                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet: checksum error for neighbor solicitation request");
699                 return;
700         }
701
702         /* Check if the IPv6 address exists on the VPN */
703
704         subnet = lookup_subnet_ipv6((ipv6_t *) &ns.nd_ns_target);
705
706         if(!subnet) {
707                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet: neighbor solicitation request for unknown address %hx:%hx:%hx:%hx:%hx:%hx:%hx:%hx",
708                                    ntohs(((uint16_t *) &ns.nd_ns_target)[0]),
709                                    ntohs(((uint16_t *) &ns.nd_ns_target)[1]),
710                                    ntohs(((uint16_t *) &ns.nd_ns_target)[2]),
711                                    ntohs(((uint16_t *) &ns.nd_ns_target)[3]),
712                                    ntohs(((uint16_t *) &ns.nd_ns_target)[4]),
713                                    ntohs(((uint16_t *) &ns.nd_ns_target)[5]),
714                                    ntohs(((uint16_t *) &ns.nd_ns_target)[6]),
715                                    ntohs(((uint16_t *) &ns.nd_ns_target)[7]));
716
717                 return;
718         }
719
720         /* Check if it is for our own subnet */
721
722         if(subnet->owner == myself)
723                 return;                                 /* silently ignore */
724
725         /* Create neighbor advertation reply */
726
727         memcpy(packet->data, packet->data + ETH_ALEN, ETH_ALEN);        /* copy destination address */
728         packet->data[ETH_ALEN * 2 - 1] ^= 0xFF; /* mangle source address so it looks like it's not from us */
729
730         ip6.ip6_dst = ip6.ip6_src;                      /* swap destination and source protocoll address */
731         ip6.ip6_src = ns.nd_ns_target;
732
733         if(has_opt)
734                 memcpy(packet->data + ether_size + ip6_size + ns_size + opt_size, packet->data + ETH_ALEN, ETH_ALEN);   /* add fake source hard addr */
735
736         ns.nd_ns_cksum = 0;
737         ns.nd_ns_type = ND_NEIGHBOR_ADVERT;
738         ns.nd_ns_reserved = htonl(0x40000000UL);        /* Set solicited flag */
739         opt.nd_opt_type = ND_OPT_TARGET_LINKADDR;
740
741         /* Create pseudo header */
742
743         pseudo.ip6_src = ip6.ip6_src;
744         pseudo.ip6_dst = ip6.ip6_dst;
745         if(has_opt)
746                 pseudo.length = htonl(ns_size + opt_size + ETH_ALEN);
747         else
748                 pseudo.length = htonl(ns_size);
749         pseudo.next = htonl(IPPROTO_ICMPV6);
750
751         /* Generate checksum */
752
753         checksum = inet_checksum(&pseudo, sizeof(pseudo), ~0);
754         checksum = inet_checksum(&ns, ns_size, checksum);
755         if(has_opt) {
756                 checksum = inet_checksum(&opt, opt_size, checksum);
757                 checksum = inet_checksum(packet->data + ether_size + ip6_size + ns_size + opt_size, ETH_ALEN, checksum);
758         }
759
760         ns.nd_ns_hdr.icmp6_cksum = checksum;
761
762         /* Copy structs on stack back to packet */
763
764         memcpy(packet->data + ether_size, &ip6, ip6_size);
765         memcpy(packet->data + ether_size + ip6_size, &ns, ns_size);
766         if(has_opt)
767                 memcpy(packet->data + ether_size + ip6_size + ns_size, &opt, opt_size);
768
769         send_packet(source, packet);
770 }
771
772 static void route_ipv6(node_t *source, vpn_packet_t *packet) {
773         if(!checklength(source, packet, ether_size + ip6_size))
774                 return;
775
776         if(packet->data[20] == IPPROTO_ICMPV6 && checklength(source, packet, ether_size + ip6_size + icmp6_size) && packet->data[54] == ND_NEIGHBOR_SOLICIT) {
777                 route_neighborsol(source, packet);
778                 return;
779         }
780
781         if(broadcast_mode && packet->data[38] == 255)
782                 broadcast_packet(source, packet);
783         else
784                 route_ipv6_unicast(source, packet);
785 }
786
787 /* RFC 826 */
788
789 static void route_arp(node_t *source, vpn_packet_t *packet) {
790         struct ether_arp arp;
791         subnet_t *subnet;
792         struct in_addr addr;
793
794         if(!checklength(source, packet, ether_size + arp_size))
795                 return;
796
797         if(source != myself) {
798                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Got ARP request from %s (%s) while in router mode!", source->name, source->hostname);
799                 return;
800         }
801
802         /* First, snatch the source address from the ARP packet */
803
804         if(overwrite_mac)
805                 memcpy(mymac.x, packet->data + ETH_ALEN, ETH_ALEN);
806
807         /* Copy headers from packet to structs on the stack */
808
809         memcpy(&arp, packet->data + ether_size, arp_size);
810
811         /* Check if this is a valid ARP request */
812
813         if(ntohs(arp.arp_hrd) != ARPHRD_ETHER || ntohs(arp.arp_pro) != ETH_P_IP ||
814            arp.arp_hln != ETH_ALEN || arp.arp_pln != sizeof(addr) || ntohs(arp.arp_op) != ARPOP_REQUEST) {
815                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet: received unknown type ARP request");
816                 return;
817         }
818
819         /* Check if the IPv4 address exists on the VPN */
820
821         subnet = lookup_subnet_ipv4((ipv4_t *) &arp.arp_tpa);
822
823         if(!subnet) {
824                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet: ARP request for unknown address %d.%d.%d.%d",
825                                    arp.arp_tpa[0], arp.arp_tpa[1], arp.arp_tpa[2],
826                                    arp.arp_tpa[3]);
827                 return;
828         }
829
830         /* Check if it is for our own subnet */
831
832         if(subnet->owner == myself)
833                 return;                                 /* silently ignore */
834
835         memcpy(packet->data, packet->data + ETH_ALEN, ETH_ALEN);        /* copy destination address */
836         packet->data[ETH_ALEN * 2 - 1] ^= 0xFF; /* mangle source address so it looks like it's not from us */
837
838         memcpy(&addr, arp.arp_tpa, sizeof(addr));       /* save protocol addr */
839         memcpy(arp.arp_tpa, arp.arp_spa, sizeof(addr)); /* swap destination and source protocol address */
840         memcpy(arp.arp_spa, &addr, sizeof(addr));       /* ... */
841
842         memcpy(arp.arp_tha, arp.arp_sha, ETH_ALEN);     /* set target hard/proto addr */
843         memcpy(arp.arp_sha, packet->data + ETH_ALEN, ETH_ALEN); /* add fake source hard addr */
844         arp.arp_op = htons(ARPOP_REPLY);
845
846         /* Copy structs on stack back to packet */
847
848         memcpy(packet->data + ether_size, &arp, arp_size);
849
850         send_packet(source, packet);
851 }
852
853 static void route_mac(node_t *source, vpn_packet_t *packet) {
854         subnet_t *subnet;
855         mac_t dest;
856
857         /* Learn source address */
858
859         if(source == myself) {
860                 mac_t src;
861                 memcpy(&src, &packet->data[6], sizeof src);
862                 learn_mac(&src);
863         }
864
865         /* Lookup destination address */
866
867         memcpy(&dest, &packet->data[0], sizeof dest);
868         subnet = lookup_subnet_mac(NULL, &dest);
869
870         if(!subnet) {
871                 broadcast_packet(source, packet);
872                 return;
873         }
874
875         if(subnet->owner == source) {
876                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Packet looping back to %s (%s)!", source->name, source->hostname);
877                 return;
878         }
879
880         if(forwarding_mode == FMODE_OFF && source != myself && subnet->owner != myself)
881                 return;
882
883         uint16_t type = packet->data[12] << 8 | packet->data[13];
884
885         if(priorityinheritance && type == ETH_P_IP && packet->len >= ether_size + ip_size)
886                 packet->priority = packet->data[15];
887
888         // Handle packets larger than PMTU
889
890         node_t *via = (subnet->owner->via == myself) ? subnet->owner->nexthop : subnet->owner->via;
891
892         if(directonly && subnet->owner != via)
893                 return;
894         
895         if(via && packet->len > via->mtu && via != myself) {
896                 ifdebug(TRAFFIC) logger(LOG_INFO, "Packet for %s (%s) length %d larger than MTU %d", subnet->owner->name, subnet->owner->hostname, packet->len, via->mtu);
897                 length_t ethlen = 14;
898
899                 if(type == ETH_P_8021Q) {
900                         type = packet->data[16] << 8 | packet->data[17];
901                         ethlen += 4;
902                 }
903
904                 if(type == ETH_P_IP && packet->len > 576 + ethlen) {
905                         if(packet->data[6 + ethlen] & 0x40) {
906                                 packet->len = via->mtu;
907                                 route_ipv4_unreachable(source, packet, ethlen, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED);
908                         } else {
909                                 fragment_ipv4_packet(via, packet, ethlen);
910                         }
911                         return;
912                 } else if(type == ETH_P_IPV6 && packet->len > 1280 + ethlen) {
913                         packet->len = via->mtu;
914                         route_ipv6_unreachable(source, packet, ethlen, ICMP6_PACKET_TOO_BIG, 0);
915                         return;
916                 }
917         }
918
919         clamp_mss(source, via, packet);
920  
921         send_packet(subnet->owner, packet);
922 }
923
924 static bool do_decrement_ttl(node_t *source, vpn_packet_t *packet) {
925         uint16_t type = packet->data[12] << 8 | packet->data[13];
926         length_t ethlen = ether_size;
927
928         if(type == ETH_P_8021Q) {
929                 type = packet->data[16] << 8 | packet->data[17];
930                 ethlen += 4;
931         }
932
933         switch (type) {
934                 case ETH_P_IP:
935                         if(!checklength(source, packet, ethlen + ip_size))
936                                 return false;
937
938                         if(packet->data[ethlen + 8] <= 1) {
939                                 if(packet->data[ethlen + 11] != IPPROTO_ICMP || packet->data[ethlen + 32] != ICMP_TIME_EXCEEDED)
940                                         route_ipv4_unreachable(source, packet, ethlen, ICMP_TIME_EXCEEDED, ICMP_EXC_TTL);
941                                 return false;
942                         }
943
944                         uint16_t old = packet->data[ethlen + 8] << 8 | packet->data[ethlen + 9];
945                         packet->data[ethlen + 8]--;
946                         uint16_t new = packet->data[ethlen + 8] << 8 | packet->data[ethlen + 9];
947
948                         uint32_t checksum = packet->data[ethlen + 10] << 8 | packet->data[ethlen + 11];
949                         checksum += old + (~new & 0xFFFF);
950                         while(checksum >> 16)
951                                 checksum = (checksum & 0xFFFF) + (checksum >> 16);
952                         packet->data[ethlen + 10] = checksum >> 8;
953                         packet->data[ethlen + 11] = checksum & 0xff;
954
955                         return true;
956
957                 case ETH_P_IPV6:
958                         if(!checklength(source, packet, ethlen + ip6_size))
959                                 return false;
960
961                         if(packet->data[ethlen + 7] <= 1) {
962                                 if(packet->data[ethlen + 6] != IPPROTO_ICMPV6 || packet->data[ethlen + 40] != ICMP6_TIME_EXCEEDED)
963                                         route_ipv6_unreachable(source, packet, ethlen, ICMP6_TIME_EXCEEDED, ICMP6_TIME_EXCEED_TRANSIT);
964                                 return false;
965                         }
966
967                         packet->data[ethlen + 7]--;
968
969                         return true;
970
971                 default:
972                         return true;
973         }
974 }
975
976 void route(node_t *source, vpn_packet_t *packet) {
977         if(forwarding_mode == FMODE_KERNEL && source != myself) {
978                 send_packet(myself, packet);
979                 return;
980         }
981
982         if(!checklength(source, packet, ether_size))
983                 return;
984
985         if(decrement_ttl && source != myself)
986                 if(!do_decrement_ttl(source, packet))
987                         return;
988
989         switch (routing_mode) {
990                 case RMODE_ROUTER:
991                         {
992                                 uint16_t type = packet->data[12] << 8 | packet->data[13];
993
994                                 switch (type) {
995                                         case ETH_P_ARP:
996                                                 route_arp(source, packet);
997                                                 break;
998
999                                         case ETH_P_IP:
1000                                                 route_ipv4(source, packet);
1001                                                 break;
1002
1003                                         case ETH_P_IPV6:
1004                                                 route_ipv6(source, packet);
1005                                                 break;
1006
1007                                         default:
1008                                                 ifdebug(TRAFFIC) logger(LOG_WARNING, "Cannot route packet from %s (%s): unknown type %hx", source->name, source->hostname, type);
1009                                                 break;
1010                                 }
1011                         }
1012                         break;
1013
1014                 case RMODE_SWITCH:
1015                         route_mac(source, packet);
1016                         break;
1017
1018                 case RMODE_HUB:
1019                         broadcast_packet(source, packet);
1020                         break;
1021         }
1022 }