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. */
}
