static int nptv6_getlasthdr(struct nptv6_cfg *cfg, struct mbuf *m, int *offset) { struct ip6_hdr *ip6; struct ip6_hbh *hbh; int proto, hlen; hlen = (offset == NULL) ? 0: *offset; if (m->m_len < hlen) return (-1); ip6 = mtodo(m, hlen); hlen += sizeof(*ip6); proto = ip6->ip6_nxt; while (proto == IPPROTO_HOPOPTS || proto == IPPROTO_ROUTING || proto == IPPROTO_DSTOPTS) { hbh = mtodo(m, hlen); if (m->m_len < hlen) return (-1); proto = hbh->ip6h_nxt; hlen += hbh->ip6h_len << 3; } if (offset != NULL) *offset = hlen; return (proto); }
static int nptv6_translate_icmpv6(struct nptv6_cfg *cfg, struct mbuf **mp, int offset) { struct icmp6_hdr *icmp6; struct ip6_hdr *ip6; struct mbuf *m; m = *mp; if (offset > m->m_len) return (-1); icmp6 = mtodo(m, offset); NPTV6_DEBUG("ICMPv6 type %d", icmp6->icmp6_type); switch (icmp6->icmp6_type) { case ICMP6_DST_UNREACH: case ICMP6_PACKET_TOO_BIG: case ICMP6_TIME_EXCEEDED: case ICMP6_PARAM_PROB: break; case ICMP6_ECHO_REQUEST: case ICMP6_ECHO_REPLY: /* nothing to translate */ return (0); default: /* * XXX: We can add some checks to not translate NDP and MLD * messages. Currently user must explicitly allow these message * types, otherwise packets will be dropped. */ return (-1); } offset += sizeof(*icmp6); if (offset + sizeof(*ip6) > m->m_pkthdr.len) return (-1); if (offset + sizeof(*ip6) > m->m_len) *mp = m = m_pullup(m, offset + sizeof(*ip6)); if (m == NULL) return (-1); ip6 = mtodo(m, offset); NPTV6_IPDEBUG("offset %d, %s -> %s %d", offset, inet_ntop(AF_INET6, &ip6->ip6_src, _s, sizeof(_s)), inet_ntop(AF_INET6, &ip6->ip6_dst, _d, sizeof(_d)), ip6->ip6_nxt); if (IN6_ARE_MASKED_ADDR_EQUAL(&ip6->ip6_src, &cfg->external, &cfg->mask)) return (nptv6_rewrite_external(cfg, mp, offset)); else if (IN6_ARE_MASKED_ADDR_EQUAL(&ip6->ip6_dst, &cfg->internal, &cfg->mask)) return (nptv6_rewrite_internal(cfg, mp, offset)); /* * Addresses in the inner IPv6 header doesn't matched to * our prefixes. */ return (-1); }
int udp_ipsec_output(struct mbuf *m, struct secasvar *sav) { struct udphdr *udp; struct mbuf *n; struct ip *ip; int hlen, off; IPSEC_ASSERT(sav->natt != NULL, ("UDP encapsulation isn't required.")); if (sav->sah->saidx.dst.sa.sa_family == AF_INET6) return (EAFNOSUPPORT); ip = mtod(m, struct ip *); hlen = ip->ip_hl << 2; n = m_makespace(m, hlen, sizeof(*udp), &off); if (n == NULL) { DPRINTF(("%s: m_makespace for udphdr failed\n", __func__)); return (ENOBUFS); } udp = mtodo(n, off); udp->uh_dport = sav->natt->dport; udp->uh_sport = sav->natt->sport; udp->uh_sum = 0; udp->uh_ulen = htons(m->m_pkthdr.len - hlen); ip = mtod(m, struct ip *); ip->ip_len = htons(m->m_pkthdr.len); ip->ip_p = IPPROTO_UDP; return (0); }
static int nptv6_rewrite_external(struct nptv6_cfg *cfg, struct mbuf **mp, int offset) { struct in6_addr *addr; struct ip6_hdr *ip6; int idx, proto; uint16_t adj; ip6 = mtodo(*mp, offset); NPTV6_IPDEBUG("offset %d, %s -> %s %d", offset, inet_ntop(AF_INET6, &ip6->ip6_src, _s, sizeof(_s)), inet_ntop(AF_INET6, &ip6->ip6_dst, _d, sizeof(_d)), ip6->ip6_nxt); if (offset == 0) addr = &ip6->ip6_dst; else { /* * When we rewriting inner IPv6 header, we need to rewrite * source address back to internal prefix. The datagram in * the ICMPv6 payload should looks like it was send from * internal prefix. */ addr = &ip6->ip6_src; } idx = nptv6_search_index(cfg, addr); if (idx < 0) { /* * Do not send ICMPv6 error when offset isn't zero. * This means we are rewriting inner IPv6 header in the * ICMPv6 error message. */ if (offset == 0) { icmp6_error2(*mp, ICMP6_DST_UNREACH, ICMP6_DST_UNREACH_ADDR, 0, (*mp)->m_pkthdr.rcvif); *mp = NULL; } return (IP_FW_DENY); } adj = addr->s6_addr16[idx]; nptv6_copy_addr(&cfg->internal, addr, &cfg->mask); adj = cksum_add(adj, ~cfg->adjustment); if (adj == 0xffff) adj = 0; addr->s6_addr16[idx] = adj; if (offset == 0) { /* * We may need to translate addresses in the inner IPv6 * header for ICMPv6 error messages. */ proto = nptv6_getlasthdr(cfg, *mp, &offset); if (proto < 0 || (proto == IPPROTO_ICMPV6 && nptv6_translate_icmpv6(cfg, mp, offset) != 0)) return (IP_FW_DENY); NPTV6STAT_INC(cfg, ex2in); } return (0); }
/* * Classify a packet to queue number using Jenkins hash function. * Return: queue number * the input of the hash are protocol no, perturbation, src IP, dst IP, * src port, dst port, */ static inline int fq_codel_classify_flow(struct mbuf *m, uint16_t fcount, struct fq_codel_si *si) { struct ip *ip; struct tcphdr *th; struct udphdr *uh; uint8_t tuple[41]; uint16_t hash=0; ip = (struct ip *)mtodo(m, dn_tag_get(m)->iphdr_off); //#ifdef INET6 struct ip6_hdr *ip6; int isip6; isip6 = (ip->ip_v == 6); if(isip6) { ip6 = (struct ip6_hdr *)ip; *((uint8_t *) &tuple[0]) = ip6->ip6_nxt; *((uint32_t *) &tuple[1]) = si->perturbation; memcpy(&tuple[5], ip6->ip6_src.s6_addr, 16); memcpy(&tuple[21], ip6->ip6_dst.s6_addr, 16); switch (ip6->ip6_nxt) { case IPPROTO_TCP: th = (struct tcphdr *)(ip6 + 1); *((uint16_t *) &tuple[37]) = th->th_dport; *((uint16_t *) &tuple[39]) = th->th_sport; break; case IPPROTO_UDP: uh = (struct udphdr *)(ip6 + 1); *((uint16_t *) &tuple[37]) = uh->uh_dport; *((uint16_t *) &tuple[39]) = uh->uh_sport; break; default: memset(&tuple[37], 0, 4); } hash = jenkins_hash(tuple, 41, HASHINIT) % fcount; return hash; } //#endif /* IPv4 */ *((uint8_t *) &tuple[0]) = ip->ip_p; *((uint32_t *) &tuple[1]) = si->perturbation; *((uint32_t *) &tuple[5]) = ip->ip_src.s_addr; *((uint32_t *) &tuple[9]) = ip->ip_dst.s_addr; switch (ip->ip_p) { case IPPROTO_TCP: th = (struct tcphdr *)(ip + 1); *((uint16_t *) &tuple[13]) = th->th_dport; *((uint16_t *) &tuple[15]) = th->th_sport; break; case IPPROTO_UDP: uh = (struct udphdr *)(ip + 1); *((uint16_t *) &tuple[13]) = uh->uh_dport; *((uint16_t *) &tuple[15]) = uh->uh_sport; break; default: memset(&tuple[13], 0, 4); } hash = jenkins_hash(tuple, 17, HASHINIT) % fcount; return hash; }
/* * Potentially decap ESP in UDP frame. Check for an ESP header. * If present, strip the UDP header and push the result through IPSec. * * Returns error if mbuf consumed and/or processed, otherwise 0. */ int udp_ipsec_input(struct mbuf *m, int off, int af) { union sockaddr_union dst; struct secasvar *sav; struct udphdr *udp; struct ip *ip; uint32_t spi; int hlen; /* * Just return if packet doesn't have enough data. * We need at least [IP header + UDP header + ESP header]. * NAT-Keepalive packet has only one byte of payload, so it * by default will not be processed. */ if (m->m_pkthdr.len < off + sizeof(struct esp)) return (0); m_copydata(m, off, sizeof(uint32_t), (caddr_t)&spi); if (spi == 0) /* Non-ESP marker. */ return (0); /* * Find SA and check that it is configured for UDP * encapsulation. */ bzero(&dst, sizeof(dst)); dst.sa.sa_family = af; switch (af) { #ifdef INET case AF_INET: dst.sin.sin_len = sizeof(struct sockaddr_in); ip = mtod(m, struct ip *); ip->ip_p = IPPROTO_ESP; off = offsetof(struct ip, ip_p); hlen = ip->ip_hl << 2; dst.sin.sin_addr = ip->ip_dst; break; #endif #ifdef INET6 case AF_INET6: /* Not yet */ /* FALLTHROUGH */ #endif default: ESPSTAT_INC(esps_nopf); m_freem(m); return (EPFNOSUPPORT); } sav = key_allocsa(&dst, IPPROTO_ESP, spi); if (sav == NULL) { ESPSTAT_INC(esps_notdb); m_freem(m); return (ENOENT); } udp = mtodo(m, hlen); if (sav->natt == NULL || sav->natt->sport != udp->uh_sport || sav->natt->dport != udp->uh_dport) { /* XXXAE: should we check source address? */ ESPSTAT_INC(esps_notdb); key_freesav(&sav); m_freem(m); return (ENOENT); } /* * Remove the UDP header * Before: * <--- off ---> * +----+------+-----+ * | IP | UDP | ESP | * +----+------+-----+ * <-skip-> * After: * +----+-----+ * | IP | ESP | * +----+-----+ * <-skip-> */ m_striphdr(m, hlen, sizeof(*udp)); /* * We cannot yet update the cksums so clear any h/w cksum flags * as they are no longer valid. */ if (m->m_pkthdr.csum_flags & CSUM_DATA_VALID) m->m_pkthdr.csum_flags &= ~(CSUM_DATA_VALID | CSUM_PSEUDO_HDR); /* * We can update ip_len and ip_sum here, but ipsec4_input_cb() * will do this anyway, so don't touch them here. */ ESPSTAT_INC(esps_input); (*sav->tdb_xform->xf_input)(m, sav, hlen, off); return (EINPROGRESS); /* Consumed by IPsec. */ }