/*
* Diagnostic routine to check mbuf alignment as required by the
* crypto device drivers (that use DMA).
*/
void
m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
{
int roff;
struct mbuf *m = m_getptr(m0, off, &roff);
caddr_t addr;
if (m == NULL)
return;
kprintf("%s (off %u len %u): ", where, off, len);
addr = mtod(m, caddr_t) + roff;
do {
int mlen;
if (((uintptr_t) addr) & 3) {
kprintf("addr misaligned %p,", addr);
break;
}
mlen = m->m_len;
if (mlen > len)
mlen = len;
len -= mlen;
if (len && (mlen & 3)) {
kprintf("len mismatch %u,", mlen);
break;
}
m = m->m_next;
addr = m ? mtod(m, caddr_t) : NULL;
} while (m && len > 0);
for (m = m0; m; m = m->m_next)
kprintf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
kprintf("\n");
}
/*
* Remove hlen data at offset skip in the packet. This is used by
* the protocols strip protocol headers and associated data (e.g. IV,
* authenticator) on input.
*/
int
m_striphdr(struct mbuf *m, int skip, int hlen)
{
struct mbuf *m1;
int roff;
/* Find beginning of header */
m1 = m_getptr(m, skip, &roff);
if (m1 == NULL)
return (EINVAL);
/* Remove the header and associated data from the mbuf. */
if (roff == 0) {
/* The header was at the beginning of the mbuf */
newipsecstat.ips_input_front++;
m_adj(m1, hlen);
if ((m1->m_flags & M_PKTHDR) == 0)
m->m_pkthdr.len -= hlen;
} else if (roff + hlen >= m1->m_len) {
struct mbuf *mo;
/*
* Part or all of the header is at the end of this mbuf,
* so first let's remove the remainder of the header from
* the beginning of the remainder of the mbuf chain, if any.
*/
newipsecstat.ips_input_end++;
if (roff + hlen > m1->m_len) {
/* Adjust the next mbuf by the remainder */
m_adj(m1->m_next, roff + hlen - m1->m_len);
/* The second mbuf is guaranteed not to have a pkthdr... */
m->m_pkthdr.len -= (roff + hlen - m1->m_len);
}
/* Now, let's unlink the mbuf chain for a second...*/
mo = m1->m_next;
m1->m_next = NULL;
/* ...and trim the end of the first part of the chain...sick */
m_adj(m1, -(m1->m_len - roff));
if ((m1->m_flags & M_PKTHDR) == 0)
m->m_pkthdr.len -= (m1->m_len - roff);
/* Finally, let's relink */
m1->m_next = mo;
} else {
/*
* The header lies in the "middle" of the mbuf; copy
* the remainder of the mbuf down over the header.
*/
newipsecstat.ips_input_middle++;
bcopy(mtod(m1, u_char *) + roff + hlen,
mtod(m1, u_char *) + roff,
m1->m_len - (roff + hlen));
m1->m_len -= hlen;
m->m_pkthdr.len -= hlen;
}
return (0);
}
static void
m_trim(struct mbuf *m, int len)
{
struct mbuf *n;
int off;
n = m_getptr(m, len, &off);
if (n) {
n->m_len = off;
if (n->m_next) {
m_freem(n->m_next);
n->m_next = NULL;
}
}
}
/*
* Copy data from an mbuf chain starting "off" bytes from the beginning,
* continuing for "len" bytes, into the indicated buffer.
*/
void
m_copydata(struct mbuf *m, int off, int len, caddr_t cp)
{
unsigned count;
if (off < 0)
panic("m_copydata: off %d < 0", off);
if (len < 0)
panic("m_copydata: len %d < 0", len);
if ((m = m_getptr(m, off, &off)) == NULL)
panic("m_copydata: short mbuf chain");
while (len > 0) {
if (m == NULL)
panic("m_copydata: null mbuf");
count = min(m->m_len - off, len);
memmove(cp, mtod(m, caddr_t) + off, count);
len -= count;
cp += count;
off = 0;
m = m->m_next;
}
}
/*
* Apply a symmetric encryption/decryption algorithm.
*/
static int
swcr_encdec(struct cryptodesc *crd, struct swcr_data *sw, caddr_t buf,
int flags)
{
unsigned char iv[EALG_MAX_BLOCK_LEN], blk[EALG_MAX_BLOCK_LEN], *idat;
unsigned char *ivp, piv[EALG_MAX_BLOCK_LEN];
struct enc_xform *exf;
int i, k, j, blks;
exf = sw->sw_exf;
blks = exf->blocksize;
/* Check for non-padded data */
if (crd->crd_len % blks)
return EINVAL;
/* Initialize the IV */
if (crd->crd_flags & CRD_F_ENCRYPT) {
/* IV explicitly provided ? */
if (crd->crd_flags & CRD_F_IV_EXPLICIT)
bcopy(crd->crd_iv, iv, blks);
else
arc4rand(iv, blks, 0);
/* Do we need to write the IV */
if (!(crd->crd_flags & CRD_F_IV_PRESENT))
crypto_copyback(flags, buf, crd->crd_inject, blks, iv);
} else { /* Decryption */
/* IV explicitly provided ? */
if (crd->crd_flags & CRD_F_IV_EXPLICIT)
bcopy(crd->crd_iv, iv, blks);
else {
/* Get IV off buf */
crypto_copydata(flags, buf, crd->crd_inject, blks, iv);
}
}
if (crd->crd_flags & CRD_F_KEY_EXPLICIT) {
int error;
if (sw->sw_kschedule)
exf->zerokey(&(sw->sw_kschedule));
error = exf->setkey(&sw->sw_kschedule,
crd->crd_key, crd->crd_klen / 8);
if (error)
return (error);
}
ivp = iv;
/*
* xforms that provide a reinit method perform all IV
* handling themselves.
*/
if (exf->reinit)
exf->reinit(sw->sw_kschedule, iv);
if (flags & CRYPTO_F_IMBUF) {
struct mbuf *m = (struct mbuf *) buf;
/* Find beginning of data */
m = m_getptr(m, crd->crd_skip, &k);
if (m == NULL)
return EINVAL;
i = crd->crd_len;
while (i > 0) {
/*
* If there's insufficient data at the end of
* an mbuf, we have to do some copying.
*/
if (m->m_len < k + blks && m->m_len != k) {
m_copydata(m, k, blks, blk);
/* Actual encryption/decryption */
if (exf->reinit) {
if (crd->crd_flags & CRD_F_ENCRYPT) {
exf->encrypt(sw->sw_kschedule,
blk);
} else {
exf->decrypt(sw->sw_kschedule,
blk);
}
} else if (crd->crd_flags & CRD_F_ENCRYPT) {
/* XOR with previous block */
for (j = 0; j < blks; j++)
blk[j] ^= ivp[j];
exf->encrypt(sw->sw_kschedule, blk);
/*
* Keep encrypted block for XOR'ing
* with next block
*/
bcopy(blk, iv, blks);
ivp = iv;
} else { /* decrypt */
/*
* Keep encrypted block for XOR'ing
* with next block
*/
if (ivp == iv)
bcopy(blk, piv, blks);
else
bcopy(blk, iv, blks);
exf->decrypt(sw->sw_kschedule, blk);
/* XOR with previous block */
for (j = 0; j < blks; j++)
blk[j] ^= ivp[j];
if (ivp == iv)
bcopy(piv, iv, blks);
else
ivp = iv;
}
/* Copy back decrypted block */
m_copyback(m, k, blks, blk);
/* Advance pointer */
m = m_getptr(m, k + blks, &k);
if (m == NULL)
return EINVAL;
i -= blks;
/* Could be done... */
if (i == 0)
break;
}
/* Skip possibly empty mbufs */
if (k == m->m_len) {
for (m = m->m_next; m && m->m_len == 0;
m = m->m_next)
;
k = 0;
}
/* Sanity check */
if (m == NULL)
return EINVAL;
/*
* Warning: idat may point to garbage here, but
* we only use it in the while() loop, only if
* there are indeed enough data.
*/
idat = mtod(m, unsigned char *) + k;
while (m->m_len >= k + blks && i > 0) {
if (exf->reinit) {
if (crd->crd_flags & CRD_F_ENCRYPT) {
exf->encrypt(sw->sw_kschedule,
idat);
} else {
exf->decrypt(sw->sw_kschedule,
idat);
}
} else if (crd->crd_flags & CRD_F_ENCRYPT) {
/* XOR with previous block/IV */
for (j = 0; j < blks; j++)
idat[j] ^= ivp[j];
exf->encrypt(sw->sw_kschedule, idat);
ivp = idat;
} else { /* decrypt */
/*
* Keep encrypted block to be used
* in next block's processing.
*/
if (ivp == iv)
bcopy(idat, piv, blks);
else
bcopy(idat, iv, blks);
exf->decrypt(sw->sw_kschedule, idat);
/* XOR with previous block/IV */
for (j = 0; j < blks; j++)
idat[j] ^= ivp[j];
if (ivp == iv)
bcopy(piv, iv, blks);
else
ivp = iv;
}
idat += blks;
k += blks;
i -= blks;
}
}
return 0; /* Done with mbuf encryption/decryption */
} else if (flags & CRYPTO_F_IOV) {
struct mbuf *
m_copym0(struct mbuf *m0, int off, int len, int wait, int deep)
{
struct mbuf *m, *n, **np;
struct mbuf *top;
int copyhdr = 0;
if (off < 0 || len < 0)
panic("m_copym0: off %d, len %d", off, len);
if (off == 0 && m0->m_flags & M_PKTHDR)
copyhdr = 1;
if ((m = m_getptr(m0, off, &off)) == NULL)
panic("m_copym0: short mbuf chain");
np = ⊤
top = NULL;
while (len > 0) {
if (m == NULL) {
if (len != M_COPYALL)
panic("m_copym0: m == NULL and not COPYALL");
break;
}
MGET(n, wait, m->m_type);
*np = n;
if (n == NULL)
goto nospace;
if (copyhdr) {
if (m_dup_pkthdr(n, m0, wait))
goto nospace;
if (len != M_COPYALL)
n->m_pkthdr.len = len;
copyhdr = 0;
}
n->m_len = min(len, m->m_len - off);
if (m->m_flags & M_EXT) {
if (!deep) {
n->m_data = m->m_data + off;
n->m_ext = m->m_ext;
MCLADDREFERENCE(m, n);
} else {
/*
* we are unsure about the way m was allocated.
* copy into multiple MCLBYTES cluster mbufs.
*/
MCLGET(n, wait);
n->m_len = 0;
n->m_len = M_TRAILINGSPACE(n);
n->m_len = min(n->m_len, len);
n->m_len = min(n->m_len, m->m_len - off);
memcpy(mtod(n, caddr_t), mtod(m, caddr_t) + off,
n->m_len);
}
} else
memcpy(mtod(n, caddr_t), mtod(m, caddr_t) + off,
n->m_len);
if (len != M_COPYALL)
len -= n->m_len;
off += n->m_len;
#ifdef DIAGNOSTIC
if (off > m->m_len)
panic("m_copym0 overrun");
#endif
if (off == m->m_len) {
m = m->m_next;
off = 0;
}
np = &n->m_next;
}
return (top);
nospace:
m_freem(top);
return (NULL);
}
void
pflog_bpfcopy(const void *src_arg, void *dst_arg, size_t len)
{
struct mbuf *m, *mp, *mhdr, *mptr;
struct pfloghdr *pfloghdr;
u_int count;
u_char *dst, *mdst, *cp;
u_short reason;
int afto, hlen, mlen, off;
union pf_headers {
struct tcphdr tcp;
struct udphdr udp;
struct icmp icmp;
#ifdef INET6
struct icmp6_hdr icmp6;
struct mld_hdr mld;
struct nd_neighbor_solicit nd_ns;
#endif /* INET6 */
} pdhdrs;
struct pf_pdesc pd;
struct pf_addr osaddr, odaddr;
u_int16_t osport = 0, odport = 0;
u_int8_t proto = 0;
m = (struct mbuf *)src_arg;
dst = dst_arg;
mhdr = pflog_mhdr;
mptr = pflog_mptr;
if (m == NULL)
panic("pflog_bpfcopy got no mbuf");
/* first mbuf holds struct pfloghdr */
pfloghdr = mtod(m, struct pfloghdr *);
afto = pfloghdr->af != pfloghdr->naf;
count = min(m->m_len, len);
bcopy(pfloghdr, dst, count);
pfloghdr = (struct pfloghdr *)dst;
dst += count;
len -= count;
m = m->m_next;
if (len <= 0)
return;
/* second mbuf is pkthdr */
if (m == NULL)
panic("no second mbuf");
/*
* temporary mbuf will hold an ip/ip6 header and 8 bytes
* of the protocol header
*/
m_inithdr(mhdr);
mhdr->m_len = 0; /* XXX not done in m_inithdr() */
#if INET && INET6
/* offset for a new header */
if (afto && pfloghdr->af == AF_INET)
mhdr->m_data += sizeof(struct ip6_hdr) -
sizeof(struct ip);
#endif /* INET && INET6 */
mdst = mtod(mhdr, char *);
switch (pfloghdr->af) {
case AF_INET: {
struct ip *h;
if (m->m_pkthdr.len < sizeof(*h))
goto copy;
m_copydata(m, 0, sizeof(*h), mdst);
h = (struct ip *)mdst;
hlen = h->ip_hl << 2;
if (hlen > sizeof(*h) && (m->m_pkthdr.len >= hlen))
m_copydata(m, sizeof(*h), hlen - sizeof(*h),
mdst + sizeof(*h));
break;
}
#ifdef INET6
case AF_INET6: {
struct ip6_hdr *h;
if (m->m_pkthdr.len < sizeof(*h))
goto copy;
hlen = sizeof(struct ip6_hdr);
m_copydata(m, 0, hlen, mdst);
h = (struct ip6_hdr *)mdst;
proto = h->ip6_nxt;
break;
}
#endif /* INET6 */
default:
/* shouldn't happen ever :-) */
goto copy;
}
if (m->m_pkthdr.len < hlen + 8 && proto != IPPROTO_NONE)
goto copy;
else if (proto != IPPROTO_NONE) {
/* copy 8 bytes of the protocol header */
m_copydata(m, hlen, 8, mdst + hlen);
hlen += 8;
}
mhdr->m_len += hlen;
mhdr->m_pkthdr.len = mhdr->m_len;
/* create a chain mhdr -> mptr, mptr->m_data = (m->m_data+hlen) */
mp = m_getptr(m, hlen, &off);
if (mp != NULL) {
bcopy(mp, mptr, sizeof(*mptr));
cp = mtod(mp, char *);
mptr->m_data += off;
mptr->m_len -= off;
mptr->m_flags &= ~M_PKTHDR;
mhdr->m_next = mptr;
mhdr->m_pkthdr.len += m->m_pkthdr.len - hlen;
}
/*
* Apply a symmetric encryption/decryption algorithm.
*/
static int
swcr_encdec(struct cryptodesc *crd, struct swcr_data *sw, caddr_t buf,
int outtype)
{
unsigned char iv[EALG_MAX_BLOCK_LEN], blk[EALG_MAX_BLOCK_LEN], *idat;
unsigned char *ivp, piv[EALG_MAX_BLOCK_LEN];
struct enc_xform *exf;
int i, k, j, blks;
exf = sw->sw_exf;
blks = exf->blocksize;
/* Check for non-padded data */
if (crd->crd_len % blks)
return EINVAL;
/* Initialize the IV */
if (crd->crd_flags & CRD_F_ENCRYPT) {
/* IV explicitly provided ? */
if (crd->crd_flags & CRD_F_IV_EXPLICIT)
bcopy(crd->crd_iv, iv, blks);
else {
/* Get random IV */
for (i = 0;
i + sizeof (u_int32_t) < EALG_MAX_BLOCK_LEN;
i += sizeof (u_int32_t)) {
u_int32_t temp = arc4random();
bcopy(&temp, iv + i, sizeof(u_int32_t));
}
/*
* What if the block size is not a multiple
* of sizeof (u_int32_t), which is the size of
* what arc4random() returns ?
*/
if (EALG_MAX_BLOCK_LEN % sizeof (u_int32_t) != 0) {
u_int32_t temp = arc4random();
bcopy (&temp, iv + i,
EALG_MAX_BLOCK_LEN - i);
}
}
/* Do we need to write the IV */
if (!(crd->crd_flags & CRD_F_IV_PRESENT)) {
COPYBACK(outtype, buf, crd->crd_inject, blks, iv);
}
} else { /* Decryption */
/* IV explicitly provided ? */
if (crd->crd_flags & CRD_F_IV_EXPLICIT)
bcopy(crd->crd_iv, iv, blks);
else {
/* Get IV off buf */
COPYDATA(outtype, buf, crd->crd_inject, blks, iv);
}
}
ivp = iv;
if (outtype == CRYPTO_BUF_CONTIG) {
if (crd->crd_flags & CRD_F_ENCRYPT) {
for (i = crd->crd_skip;
i < crd->crd_skip + crd->crd_len; i += blks) {
/* XOR with the IV/previous block, as appropriate. */
if (i == crd->crd_skip)
for (k = 0; k < blks; k++)
buf[i + k] ^= ivp[k];
else
for (k = 0; k < blks; k++)
buf[i + k] ^= buf[i + k - blks];
exf->encrypt(sw->sw_kschedule, buf + i);
}
} else { /* Decrypt */
/*
* Start at the end, so we don't need to keep the encrypted
* block as the IV for the next block.
*/
for (i = crd->crd_skip + crd->crd_len - blks;
i >= crd->crd_skip; i -= blks) {
exf->decrypt(sw->sw_kschedule, buf + i);
/* XOR with the IV/previous block, as appropriate */
if (i == crd->crd_skip)
for (k = 0; k < blks; k++)
buf[i + k] ^= ivp[k];
else
for (k = 0; k < blks; k++)
buf[i + k] ^= buf[i + k - blks];
}
}
return 0;
} else if (outtype == CRYPTO_BUF_MBUF) {
struct mbuf *m = (struct mbuf *) buf;
/* Find beginning of data */
m = m_getptr(m, crd->crd_skip, &k);
if (m == NULL)
return EINVAL;
i = crd->crd_len;
while (i > 0) {
/*
* If there's insufficient data at the end of
* an mbuf, we have to do some copying.
*/
if (m->m_len < k + blks && m->m_len != k) {
m_copydata(m, k, blks, blk);
/* Actual encryption/decryption */
if (crd->crd_flags & CRD_F_ENCRYPT) {
/* XOR with previous block */
for (j = 0; j < blks; j++)
blk[j] ^= ivp[j];
exf->encrypt(sw->sw_kschedule, blk);
/*
* Keep encrypted block for XOR'ing
* with next block
*/
bcopy(blk, iv, blks);
ivp = iv;
} else { /* decrypt */
/*
* Keep encrypted block for XOR'ing
* with next block
*/
if (ivp == iv)
bcopy(blk, piv, blks);
else
bcopy(blk, iv, blks);
exf->decrypt(sw->sw_kschedule, blk);
/* XOR with previous block */
for (j = 0; j < blks; j++)
blk[j] ^= ivp[j];
if (ivp == iv)
bcopy(piv, iv, blks);
else
ivp = iv;
}
/* Copy back decrypted block */
m_copyback(m, k, blks, blk);
/* Advance pointer */
m = m_getptr(m, k + blks, &k);
if (m == NULL)
return EINVAL;
i -= blks;
/* Could be done... */
if (i == 0)
break;
}
/* Skip possibly empty mbufs */
if (k == m->m_len) {
for (m = m->m_next; m && m->m_len == 0;
m = m->m_next)
;
k = 0;
}
/* Sanity check */
if (m == NULL)
return EINVAL;
/*
* Warning: idat may point to garbage here, but
* we only use it in the while() loop, only if
* there are indeed enough data.
*/
idat = mtod(m, unsigned char *) + k;
while (m->m_len >= k + blks && i > 0) {
if (crd->crd_flags & CRD_F_ENCRYPT) {
/* XOR with previous block/IV */
for (j = 0; j < blks; j++)
idat[j] ^= ivp[j];
exf->encrypt(sw->sw_kschedule, idat);
ivp = idat;
} else { /* decrypt */
/*
* Keep encrypted block to be used
* in next block's processing.
*/
if (ivp == iv)
bcopy(idat, piv, blks);
else
bcopy(idat, iv, blks);
exf->decrypt(sw->sw_kschedule, idat);
/* XOR with previous block/IV */
for (j = 0; j < blks; j++)
idat[j] ^= ivp[j];
if (ivp == iv)
bcopy(piv, iv, blks);
else
ivp = iv;
}
idat += blks;
k += blks;
i -= blks;
}
}
return 0; /* Done with mbuf encryption/decryption */
} else if (outtype == CRYPTO_BUF_IOV) {
void
pflog_bpfcopy(const void *src_arg, void *dst_arg, size_t len)
{
struct mbuf *m, *mp, *mhdr, *mptr;
struct pfloghdr *pfloghdr;
u_int count;
u_char *dst, *mdst;
int afto, hlen, mlen, off;
union pf_headers {
struct tcphdr tcp;
struct udphdr udp;
struct icmp icmp;
#ifdef INET6
struct icmp6_hdr icmp6;
struct mld_hdr mld;
struct nd_neighbor_solicit nd_ns;
#endif /* INET6 */
} pdhdrs;
struct pf_pdesc pd;
struct pf_addr osaddr, odaddr;
u_int16_t osport = 0, odport = 0;
u_int8_t proto = 0;
m = (struct mbuf *)src_arg;
dst = dst_arg;
mhdr = pflog_mhdr;
mptr = pflog_mptr;
if (m == NULL)
panic("pflog_bpfcopy got no mbuf");
/* first mbuf holds struct pfloghdr */
pfloghdr = mtod(m, struct pfloghdr *);
afto = pfloghdr->af != pfloghdr->naf;
count = min(m->m_len, len);
bcopy(pfloghdr, dst, count);
pfloghdr = (struct pfloghdr *)dst;
dst += count;
len -= count;
m = m->m_next;
if (len <= 0)
return;
/* second mbuf is pkthdr */
if (m == NULL)
panic("no second mbuf");
/*
* temporary mbuf will hold an ip/ip6 header and 8 bytes
* of the protocol header
*/
m_inithdr(mhdr);
mhdr->m_len = 0; /* XXX not done in m_inithdr() */
#if INET && INET6
/* offset for a new header */
if (afto && pfloghdr->af == AF_INET)
mhdr->m_data += sizeof(struct ip6_hdr) -
sizeof(struct ip);
#endif /* INET && INET6 */
mdst = mtod(mhdr, char *);
switch (pfloghdr->af) {
case AF_INET: {
struct ip *h;
if (m->m_pkthdr.len < sizeof(*h))
goto copy;
m_copydata(m, 0, sizeof(*h), mdst);
h = (struct ip *)mdst;
hlen = h->ip_hl << 2;
if (hlen > sizeof(*h) && (m->m_pkthdr.len >= hlen))
m_copydata(m, sizeof(*h), hlen - sizeof(*h),
mdst + sizeof(*h));
break;
}
#ifdef INET6
case AF_INET6: {
struct ip6_hdr *h;
if (m->m_pkthdr.len < sizeof(*h))
goto copy;
hlen = sizeof(struct ip6_hdr);
m_copydata(m, 0, hlen, mdst);
h = (struct ip6_hdr *)mdst;
proto = h->ip6_nxt;
break;
}
#endif /* INET6 */
default:
/* shouldn't happen ever :-) */
goto copy;
}
if (m->m_pkthdr.len < hlen + 8 && proto != IPPROTO_NONE)
goto copy;
else if (proto != IPPROTO_NONE) {
/* copy 8 bytes of the protocol header */
m_copydata(m, hlen, 8, mdst + hlen);
hlen += 8;
}
mhdr->m_len += hlen;
mhdr->m_pkthdr.len = mhdr->m_len;
/* create a chain mhdr -> mptr, mptr->m_data = (m->m_data+hlen) */
mp = m_getptr(m, hlen, &off);
if (mp != NULL) {
bcopy(mp, mptr, sizeof(*mptr));
mptr->m_data += off;
mptr->m_len -= off;
mptr->m_flags &= ~M_PKTHDR;
mhdr->m_next = mptr;
mhdr->m_pkthdr.len += m->m_pkthdr.len - hlen;
}
/*
* Rewrite addresses if needed. Reason pointer must be NULL to avoid
* counting the packet here again.
*/
if (pf_setup_pdesc(&pd, &pdhdrs, pfloghdr->af, pfloghdr->dir, NULL,
mhdr, NULL) != PF_PASS)
goto copy;
pd.naf = pfloghdr->naf;
PF_ACPY(&osaddr, pd.src, pd.af);
PF_ACPY(&odaddr, pd.dst, pd.af);
if (pd.sport)
osport = *pd.sport;
if (pd.dport)
odport = *pd.dport;
if (pd.virtual_proto != PF_VPROTO_FRAGMENT &&
(pfloghdr->rewritten = pf_translate(&pd, &pfloghdr->saddr,
pfloghdr->sport, &pfloghdr->daddr, pfloghdr->dport, 0,
pfloghdr->dir))) {
m_copyback(pd.m, pd.off, min(pd.m->m_len - pd.off, pd.hdrlen),
pd.hdr.any, M_NOWAIT);
#if INET && INET6
if (afto) {
PF_ACPY(&pd.nsaddr, &pfloghdr->saddr, pd.naf);
PF_ACPY(&pd.ndaddr, &pfloghdr->daddr, pd.naf);
}
#endif /* INET && INET6 */
PF_ACPY(&pfloghdr->saddr, &osaddr, pd.af);
PF_ACPY(&pfloghdr->daddr, &odaddr, pd.af);
pfloghdr->sport = osport;
pfloghdr->dport = odport;
}
pd.tot_len = min(pd.tot_len, len);
pd.tot_len -= pd.m->m_data - pd.m->m_pktdat;
#if INET && INET6
if (afto && pfloghdr->rewritten)
pf_translate_af(&pd);
#endif /* INET && INET6 */
m = pd.m;
copy:
mlen = min(m->m_pkthdr.len, len);
m_copydata(m, 0, mlen, dst);
len -= mlen;
if (len > 0)
bzero(dst + mlen, len);
}
/*
* ESP input callback, called directly by the crypto driver.
*/
int
esp_input_cb(void *op)
{
u_int8_t lastthree[3], aalg[AH_HMAC_HASHLEN];
int s, hlen, roff, skip, protoff, error;
struct mbuf *m1, *mo, *m;
struct auth_hash *esph;
struct tdb_crypto *tc;
struct cryptop *crp;
struct m_tag *mtag;
struct tdb *tdb;
u_int32_t btsx;
caddr_t ptr;
crp = (struct cryptop *) op;
tc = (struct tdb_crypto *) crp->crp_opaque;
skip = tc->tc_skip;
protoff = tc->tc_protoff;
mtag = (struct m_tag *) tc->tc_ptr;
m = (struct mbuf *) crp->crp_buf;
if (m == NULL) {
/* Shouldn't happen... */
free(tc, M_XDATA);
crypto_freereq(crp);
espstat.esps_crypto++;
DPRINTF(("esp_input_cb(): bogus returned buffer from crypto\n"));
return (EINVAL);
}
s = spltdb();
tdb = gettdb(tc->tc_spi, &tc->tc_dst, tc->tc_proto);
if (tdb == NULL) {
free(tc, M_XDATA);
espstat.esps_notdb++;
DPRINTF(("esp_input_cb(): TDB is expired while in crypto"));
error = EPERM;
goto baddone;
}
esph = (struct auth_hash *) tdb->tdb_authalgxform;
/* Check for crypto errors */
if (crp->crp_etype) {
if (crp->crp_etype == EAGAIN) {
/* Reset the session ID */
if (tdb->tdb_cryptoid != 0)
tdb->tdb_cryptoid = crp->crp_sid;
splx(s);
return crypto_dispatch(crp);
}
free(tc, M_XDATA);
espstat.esps_noxform++;
DPRINTF(("esp_input_cb(): crypto error %d\n", crp->crp_etype));
error = crp->crp_etype;
goto baddone;
}
/* If authentication was performed, check now. */
if (esph != NULL) {
/*
* If we have a tag, it means an IPsec-aware NIC did the verification
* for us.
*/
if (mtag == NULL) {
/* Copy the authenticator from the packet */
m_copydata(m, m->m_pkthdr.len - esph->authsize,
esph->authsize, aalg);
ptr = (caddr_t) (tc + 1);
/* Verify authenticator */
if (bcmp(ptr, aalg, esph->authsize)) {
free(tc, M_XDATA);
DPRINTF(("esp_input_cb(): authentication failed for packet in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
espstat.esps_badauth++;
error = EACCES;
goto baddone;
}
}
/* Remove trailing authenticator */
m_adj(m, -(esph->authsize));
}
free(tc, M_XDATA);
/* Replay window checking, if appropriate */
if ((tdb->tdb_wnd > 0) && (!(tdb->tdb_flags & TDBF_NOREPLAY))) {
m_copydata(m, skip + sizeof(u_int32_t), sizeof(u_int32_t),
(unsigned char *) &btsx);
btsx = ntohl(btsx);
switch (checkreplaywindow32(btsx, 0, &(tdb->tdb_rpl),
tdb->tdb_wnd, &(tdb->tdb_bitmap), 1)) {
case 0: /* All's well */
#if NPFSYNC > 0
pfsync_update_tdb(tdb,0);
#endif
break;
case 1:
DPRINTF(("esp_input_cb(): replay counter wrapped for SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
espstat.esps_wrap++;
error = EACCES;
goto baddone;
case 2:
case 3:
DPRINTF(("esp_input_cb(): duplicate packet received in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
error = EACCES;
goto baddone;
default:
DPRINTF(("esp_input_cb(): bogus value from checkreplaywindow32() in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
espstat.esps_replay++;
error = EACCES;
goto baddone;
}
}
/* Release the crypto descriptors */
crypto_freereq(crp);
/* Determine the ESP header length */
if (tdb->tdb_flags & TDBF_NOREPLAY)
hlen = sizeof(u_int32_t) + tdb->tdb_ivlen; /* "old" ESP */
else
hlen = 2 * sizeof(u_int32_t) + tdb->tdb_ivlen; /* "new" ESP */
/* Find beginning of ESP header */
m1 = m_getptr(m, skip, &roff);
if (m1 == NULL) {
espstat.esps_hdrops++;
splx(s);
DPRINTF(("esp_input_cb(): bad mbuf chain, SA %s/%08x\n",
ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
m_freem(m);
return EINVAL;
}
/* Remove the ESP header and IV from the mbuf. */
if (roff == 0) {
/* The ESP header was conveniently at the beginning of the mbuf */
m_adj(m1, hlen);
if (!(m1->m_flags & M_PKTHDR))
m->m_pkthdr.len -= hlen;
} else if (roff + hlen >= m1->m_len) {
/*
* Part or all of the ESP header is at the end of this mbuf, so
* first let's remove the remainder of the ESP header from the
* beginning of the remainder of the mbuf chain, if any.
*/
if (roff + hlen > m1->m_len) {
/* Adjust the next mbuf by the remainder */
m_adj(m1->m_next, roff + hlen - m1->m_len);
/* The second mbuf is guaranteed not to have a pkthdr... */
m->m_pkthdr.len -= (roff + hlen - m1->m_len);
}
/* Now, let's unlink the mbuf chain for a second...*/
mo = m1->m_next;
m1->m_next = NULL;
/* ...and trim the end of the first part of the chain...sick */
m_adj(m1, -(m1->m_len - roff));
if (!(m1->m_flags & M_PKTHDR))
m->m_pkthdr.len -= (m1->m_len - roff);
/* Finally, let's relink */
m1->m_next = mo;
} else {
/*
* The ESP header lies in the "middle" of the mbuf...do an
* overlapping copy of the remainder of the mbuf over the ESP
* header.
*/
bcopy(mtod(m1, u_char *) + roff + hlen,
mtod(m1, u_char *) + roff, m1->m_len - (roff + hlen));
m1->m_len -= hlen;
m->m_pkthdr.len -= hlen;
}
/* Save the last three bytes of decrypted data */
m_copydata(m, m->m_pkthdr.len - 3, 3, lastthree);
/* Verify pad length */
if (lastthree[1] + 2 > m->m_pkthdr.len - skip) {
espstat.esps_badilen++;
splx(s);
DPRINTF(("esp_input_cb(): invalid padding length %d for packet in SA %s/%08x\n", lastthree[1], ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
m_freem(m);
return EINVAL;
}
/* Verify correct decryption by checking the last padding bytes */
if (!(tdb->tdb_flags & TDBF_RANDOMPADDING)) {
if ((lastthree[1] != lastthree[0]) && (lastthree[1] != 0)) {
espstat.esps_badenc++;
splx(s);
DPRINTF(("esp_input(): decryption failed for packet in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
m_freem(m);
return EINVAL;
}
}
/* Trim the mbuf chain to remove the trailing authenticator and padding */
m_adj(m, -(lastthree[1] + 2));
/* Restore the Next Protocol field */
m_copyback(m, protoff, sizeof(u_int8_t), lastthree + 2);
/* Back to generic IPsec input processing */
error = ipsec_common_input_cb(m, tdb, skip, protoff, mtag);
splx(s);
return (error);
baddone:
splx(s);
if (m != NULL)
m_freem(m);
crypto_freereq(crp);
return (error);
}
