+static void route_ipv4_unreachable(node_t *source, vpn_packet_t *packet, uint8_t type, uint8_t code)
+{
+ struct ip ip = {0};
+ struct icmp icmp = {0};
+
+ struct in_addr ip_src;
+ struct in_addr ip_dst;
+ uint32_t oldlen;
+
+ if(ratelimit(3))
+ return;
+
+ cp();
+
+ /* Copy headers from packet into properly aligned structs on the stack */
+
+ memcpy(&ip, packet->data + ether_size, ip_size);
+
+ /* Remember original source and destination */
+
+ ip_src = ip.ip_src;
+ ip_dst = ip.ip_dst;
+
+ oldlen = packet->len - ether_size;
+
+ if(type == ICMP_DEST_UNREACH && code == ICMP_FRAG_NEEDED)
+ icmp.icmp_nextmtu = htons(packet->len - ether_size);
+
+ if(oldlen >= IP_MSS - ip_size - icmp_size)
+ oldlen = IP_MSS - ip_size - icmp_size;
+
+ /* Copy first part of original contents to ICMP message */
+
+ memmove(packet->data + ether_size + ip_size + icmp_size, packet->data + ether_size, oldlen);
+
+ /* Fill in IPv4 header */
+
+ ip.ip_v = 4;
+ ip.ip_hl = ip_size / 4;
+ ip.ip_tos = 0;
+ ip.ip_len = htons(ip_size + icmp_size + oldlen);
+ ip.ip_id = 0;
+ ip.ip_off = 0;
+ ip.ip_ttl = 255;
+ ip.ip_p = IPPROTO_ICMP;
+ ip.ip_sum = 0;
+ ip.ip_src = ip_dst;
+ ip.ip_dst = ip_src;
+
+ ip.ip_sum = inet_checksum(&ip, ip_size, ~0);
+
+ /* Fill in ICMP header */
+
+ icmp.icmp_type = type;
+ icmp.icmp_code = code;
+ icmp.icmp_cksum = 0;
+
+ icmp.icmp_cksum = inet_checksum(&icmp, icmp_size, ~0);
+ icmp.icmp_cksum = inet_checksum(packet->data + ether_size + ip_size + icmp_size, oldlen, icmp.icmp_cksum);
+
+ /* Copy structs on stack back to packet */
+
+ memcpy(packet->data + ether_size, &ip, ip_size);
+ memcpy(packet->data + ether_size + ip_size, &icmp, icmp_size);
+
+ packet->len = ether_size + ip_size + icmp_size + oldlen;
+
+ send_packet(source, packet);
+}
+
+static __inline__ void route_ipv4_unicast(node_t *source, vpn_packet_t *packet)
+{
+ subnet_t *subnet;
+
+ cp();
+
+ subnet = lookup_subnet_ipv4((ipv4_t *) &packet->data[30]);
+
+ if(!subnet) {
+ ifdebug(TRAFFIC) logger(LOG_WARNING, _("Cannot route packet from %s (%s): unknown IPv4 destination address %d.%d.%d.%d"),
+ source->name, source->hostname,
+ packet->data[30],
+ packet->data[31],
+ packet->data[32],
+ packet->data[33]);
+
+ route_ipv4_unreachable(source, packet, ICMP_DEST_UNREACH, ICMP_NET_UNKNOWN);
+ return;
+ }
+
+ if(subnet->owner == source) {
+ ifdebug(TRAFFIC) logger(LOG_WARNING, _("Packet looping back to %s (%s)!"), source->name, source->hostname);
+ return;
+ }
+
+ if(!subnet->owner->status.reachable)
+ route_ipv4_unreachable(source, packet, ICMP_DEST_UNREACH, ICMP_NET_UNREACH);
+
+ if(subnet->owner->options & OPTION_DONTFRAGMENT && packet->len > subnet->owner->mtu && subnet->owner != myself) {
+ ifdebug(TRAFFIC) logger(LOG_INFO, _("Packet for %s (%s) length %d larger than MTU %d"), subnet->owner->name, subnet->owner->hostname, packet->len, subnet->owner->mtu);
+ packet->len = subnet->owner->mtu;
+ route_ipv4_unreachable(source, packet, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED);
+ return;
+ }
+
+ if(priorityinheritance)
+ packet->priority = packet->data[15];
+
+ send_packet(subnet->owner, packet);
+}
+
+static __inline__ void route_ipv4(node_t *source, vpn_packet_t *packet)
+{
+ cp();
+
+ if(!checklength(source, packet, ether_size + ip_size))
+ return;
+
+ route_ipv4_unicast(source, packet);
+}
+
+/* RFC 2463 */
+
+static void route_ipv6_unreachable(node_t *source, vpn_packet_t *packet, uint8_t type, uint8_t code)
+{
+ struct ip6_hdr ip6;
+ struct icmp6_hdr icmp6 = {0};
+ uint16_t checksum;
+
+ struct {
+ struct in6_addr ip6_src; /* source address */
+ struct in6_addr ip6_dst; /* destination address */
+ uint32_t length;
+ uint32_t next;
+ } pseudo;
+
+ if(ratelimit(3))
+ return;
+
+ cp();
+
+ /* Copy headers from packet to structs on the stack */
+
+ memcpy(&ip6, packet->data + ether_size, ip6_size);
+
+ /* Remember original source and destination */
+
+ pseudo.ip6_src = ip6.ip6_dst;
+ pseudo.ip6_dst = ip6.ip6_src;
+
+ pseudo.length = packet->len - ether_size;
+
+ if(type == ICMP6_PACKET_TOO_BIG)
+ icmp6.icmp6_mtu = htonl(pseudo.length);
+
+ if(pseudo.length >= IP_MSS - ip6_size - icmp6_size)
+ pseudo.length = IP_MSS - ip6_size - icmp6_size;
+
+ /* Copy first part of original contents to ICMP message */
+
+ memmove(packet->data + ether_size + ip6_size + icmp6_size, packet->data + ether_size, pseudo.length);
+
+ /* Fill in IPv6 header */
+
+ ip6.ip6_flow = htonl(0x60000000UL);
+ ip6.ip6_plen = htons(icmp6_size + pseudo.length);
+ ip6.ip6_nxt = IPPROTO_ICMPV6;
+ ip6.ip6_hlim = 255;
+ ip6.ip6_src = pseudo.ip6_src;
+ ip6.ip6_dst = pseudo.ip6_dst;
+
+ /* Fill in ICMP header */
+
+ icmp6.icmp6_type = type;
+ icmp6.icmp6_code = code;
+ icmp6.icmp6_cksum = 0;
+
+ /* Create pseudo header */
+
+ pseudo.length = htonl(icmp6_size + pseudo.length);
+ pseudo.next = htonl(IPPROTO_ICMPV6);
+
+ /* Generate checksum */
+
+ checksum = inet_checksum(&pseudo, sizeof(pseudo), ~0);
+ checksum = inet_checksum(&icmp6, icmp6_size, checksum);
+ checksum = inet_checksum(packet->data + ether_size + ip6_size + icmp6_size, ntohl(pseudo.length) - icmp6_size, checksum);
+
+ icmp6.icmp6_cksum = checksum;
+
+ /* Copy structs on stack back to packet */
+
+ memcpy(packet->data + ether_size, &ip6, ip6_size);
+ memcpy(packet->data + ether_size + ip6_size, &icmp6, icmp6_size);
+
+ packet->len = ether_size + ip6_size + ntohl(pseudo.length);
+
+ send_packet(source, packet);
+}
+
+static __inline__ void route_ipv6_unicast(node_t *source, vpn_packet_t *packet)
+{
+ subnet_t *subnet;
+
+ cp();
+
+ subnet = lookup_subnet_ipv6((ipv6_t *) &packet->data[38]);
+
+ if(!subnet) {
+ ifdebug(TRAFFIC) logger(LOG_WARNING, _("Cannot route packet from %s (%s): unknown IPv6 destination address %hx:%hx:%hx:%hx:%hx:%hx:%hx:%hx"),
+ source->name, source->hostname,
+ ntohs(*(uint16_t *) &packet->data[38]),
+ ntohs(*(uint16_t *) &packet->data[40]),
+ ntohs(*(uint16_t *) &packet->data[42]),
+ ntohs(*(uint16_t *) &packet->data[44]),
+ ntohs(*(uint16_t *) &packet->data[46]),
+ ntohs(*(uint16_t *) &packet->data[48]),
+ ntohs(*(uint16_t *) &packet->data[50]),
+ ntohs(*(uint16_t *) &packet->data[52]));
+
+ route_ipv6_unreachable(source, packet, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_ADDR);
+ return;
+ }
+
+ if(subnet->owner == source) {
+ ifdebug(TRAFFIC) logger(LOG_WARNING, _("Packet looping back to %s (%s)!"), source->name, source->hostname);
+ return;
+ }
+
+ if(!subnet->owner->status.reachable)
+ route_ipv6_unreachable(source, packet, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_NOROUTE);
+
+ if(subnet->owner->options & OPTION_DONTFRAGMENT && packet->len > subnet->owner->mtu && subnet->owner != myself) {
+ ifdebug(TRAFFIC) logger(LOG_INFO, _("Packet for %s (%s) length %d larger than MTU %d"), subnet->owner->name, subnet->owner->hostname, packet->len, subnet->owner->mtu);
+ packet->len = subnet->owner->mtu;
+ route_ipv6_unreachable(source, packet, ICMP6_PACKET_TOO_BIG, 0);
+ return;
+ }
+
+ send_packet(subnet->owner, packet);