/*
* Partition an mbuf chain in two pieces, returning the tail --
* all but the first len0 bytes. In case of failure, it returns NULL and
* attempts to restore the chain to its original state.
*/
struct mbuf *
m_split(struct mbuf *m0, int len0, int wait)
{
struct mbuf *m, *n;
unsigned len = len0, remain, olen;
for (m = m0; m && len > m->m_len; m = m->m_next)
len -= m->m_len;
if (m == NULL)
return (NULL);
remain = m->m_len - len;
if (m0->m_flags & M_PKTHDR) {
MGETHDR(n, wait, m0->m_type);
if (n == NULL)
return (NULL);
if (m_dup_pkthdr(n, m0, wait)) {
m_freem(n);
return (NULL);
}
n->m_pkthdr.len -= len0;
olen = m0->m_pkthdr.len;
m0->m_pkthdr.len = len0;
if (m->m_flags & M_EXT)
goto extpacket;
if (remain > MHLEN) {
/* m can't be the lead packet */
MH_ALIGN(n, 0);
n->m_next = m_split(m, len, wait);
if (n->m_next == NULL) {
(void) m_free(n);
m0->m_pkthdr.len = olen;
return (NULL);
} else
return (n);
} else
MH_ALIGN(n, remain);
} else if (remain == 0) {
n = m->m_next;
m->m_next = NULL;
return (n);
} else {
MGET(n, wait, m->m_type);
if (n == NULL)
return (NULL);
M_ALIGN(n, remain);
}
extpacket:
if (m->m_flags & M_EXT) {
n->m_ext = m->m_ext;
MCLADDREFERENCE(m, n);
n->m_data = m->m_data + len;
} else {
memcpy(mtod(n, caddr_t), mtod(m, caddr_t) + len, remain);
}
n->m_len = remain;
m->m_len = len;
n->m_next = m->m_next;
m->m_next = NULL;
return (n);
}
/*
* Copy an entire packet, including header (which must be present).
* An optimization of the common case `m_copym(m, 0, M_COPYALL, how)'.
* Note that the copy is read-only, because clusters are not copied,
* only their reference counts are incremented.
* Preserve alignment of the first mbuf so if the creator has left
* some room at the beginning (e.g. for inserting protocol headers)
* the copies still have the room available.
*/
struct mbuf *
m_copypacket(struct mbuf *m, int how)
{
struct mbuf *top, *n, *o;
MBUF_CHECKSLEEP(how);
n = m_get(how, m->m_type);
top = n;
if (n == NULL)
goto nospace;
if (!m_dup_pkthdr(n, m, how))
goto nospace;
n->m_len = m->m_len;
if (m->m_flags & M_EXT) {
n->m_data = m->m_data;
mb_dupcl(n, m);
} else {
n->m_data = n->m_pktdat + (m->m_data - m->m_pktdat );
bcopy(mtod(m, char *), mtod(n, char *), n->m_len);
}
m = m->m_next;
while (m) {
o = m_get(how, m->m_type);
if (o == NULL)
goto nospace;
n->m_next = o;
n = n->m_next;
n->m_len = m->m_len;
if (m->m_flags & M_EXT) {
n->m_data = m->m_data;
mb_dupcl(n, m);
} else {
bcopy(mtod(m, char *), mtod(n, char *), n->m_len);
}
m = m->m_next;
}
return top;
nospace:
m_freem(top);
return (NULL);
}
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);
}
/*
* Copy a packet header mbuf chain into a completely new chain, including
* copying any mbuf clusters. Use this instead of m_copypacket() when
* you need a writable copy of an mbuf chain.
*/
struct mbuf *
m_dup(const struct mbuf *m, int how)
{
struct mbuf **p, *top = NULL;
int remain, moff, nsize;
MBUF_CHECKSLEEP(how);
/* Sanity check */
if (m == NULL)
return (NULL);
M_ASSERTPKTHDR(m);
/* While there's more data, get a new mbuf, tack it on, and fill it */
remain = m->m_pkthdr.len;
moff = 0;
p = ⊤
while (remain > 0 || top == NULL) { /* allow m->m_pkthdr.len == 0 */
struct mbuf *n;
/* Get the next new mbuf */
if (remain >= MINCLSIZE) {
n = m_getcl(how, m->m_type, 0);
nsize = MCLBYTES;
} else {
n = m_get(how, m->m_type);
nsize = MLEN;
}
if (n == NULL)
goto nospace;
if (top == NULL) { /* First one, must be PKTHDR */
if (!m_dup_pkthdr(n, m, how)) {
m_free(n);
goto nospace;
}
if ((n->m_flags & M_EXT) == 0)
nsize = MHLEN;
n->m_flags &= ~M_RDONLY;
}
n->m_len = 0;
/* Link it into the new chain */
*p = n;
p = &n->m_next;
/* Copy data from original mbuf(s) into new mbuf */
while (n->m_len < nsize && m != NULL) {
int chunk = min(nsize - n->m_len, m->m_len - moff);
bcopy(m->m_data + moff, n->m_data + n->m_len, chunk);
moff += chunk;
n->m_len += chunk;
remain -= chunk;
if (moff == m->m_len) {
m = m->m_next;
moff = 0;
}
}
/* Check correct total mbuf length */
KASSERT((remain > 0 && m != NULL) || (remain == 0 && m == NULL),
("%s: bogus m_pkthdr.len", __func__));
}
return (top);
nospace:
m_freem(top);
return (NULL);
}
/*
* Make a copy of an mbuf chain starting "off0" bytes from the beginning,
* continuing for "len" bytes. If len is M_COPYALL, copy to end of mbuf.
* The wait parameter is a choice of M_WAITOK/M_NOWAIT from caller.
* Note that the copy is read-only, because clusters are not copied,
* only their reference counts are incremented.
*/
struct mbuf *
m_copym(struct mbuf *m, int off0, int len, int wait)
{
struct mbuf *n, **np;
int off = off0;
struct mbuf *top;
int copyhdr = 0;
KASSERT(off >= 0, ("m_copym, negative off %d", off));
KASSERT(len >= 0, ("m_copym, negative len %d", len));
MBUF_CHECKSLEEP(wait);
if (off == 0 && m->m_flags & M_PKTHDR)
copyhdr = 1;
while (off > 0) {
KASSERT(m != NULL, ("m_copym, offset > size of mbuf chain"));
if (off < m->m_len)
break;
off -= m->m_len;
m = m->m_next;
}
np = ⊤
top = NULL;
while (len > 0) {
if (m == NULL) {
KASSERT(len == M_COPYALL,
("m_copym, length > size of mbuf chain"));
break;
}
if (copyhdr)
n = m_gethdr(wait, m->m_type);
else
n = m_get(wait, m->m_type);
*np = n;
if (n == NULL)
goto nospace;
if (copyhdr) {
if (!m_dup_pkthdr(n, m, wait))
goto nospace;
if (len == M_COPYALL)
n->m_pkthdr.len -= off0;
else
n->m_pkthdr.len = len;
copyhdr = 0;
}
n->m_len = min(len, m->m_len - off);
if (m->m_flags & M_EXT) {
n->m_data = m->m_data + off;
mb_dupcl(n, m);
} else
bcopy(mtod(m, caddr_t)+off, mtod(n, caddr_t),
(u_int)n->m_len);
if (len != M_COPYALL)
len -= n->m_len;
off = 0;
m = m->m_next;
np = &n->m_next;
}
return (top);
nospace:
m_freem(top);
return (NULL);
}
/*
* Defragment a mbuf chain, returning the shortest possible
* chain of mbufs and clusters. If allocation fails and
* this cannot be completed, NULL will be returned, but
* the passed in chain will be unchanged. Upon success,
* the original chain will be freed, and the new chain
* will be returned.
*
* If a non-packet header is passed in, the original
* mbuf (chain?) will be returned unharmed.
*/
struct mbuf *
m_defrag(struct mbuf *m0, int how)
{
struct mbuf *m_new = NULL, *m_final = NULL;
int progress = 0, length;
MBUF_CHECKSLEEP(how);
if (!(m0->m_flags & M_PKTHDR))
return (m0);
m_fixhdr(m0); /* Needed sanity check */
#ifdef MBUF_STRESS_TEST
if (m_defragrandomfailures) {
int temp = arc4random() & 0xff;
if (temp == 0xba)
goto nospace;
}
#endif
if (m0->m_pkthdr.len > MHLEN)
m_final = m_getcl(how, MT_DATA, M_PKTHDR);
else
m_final = m_gethdr(how, MT_DATA);
if (m_final == NULL)
goto nospace;
if (m_dup_pkthdr(m_final, m0, how) == 0)
goto nospace;
m_new = m_final;
while (progress < m0->m_pkthdr.len) {
length = m0->m_pkthdr.len - progress;
if (length > MCLBYTES)
length = MCLBYTES;
if (m_new == NULL) {
if (length > MLEN)
m_new = m_getcl(how, MT_DATA, 0);
else
m_new = m_get(how, MT_DATA);
if (m_new == NULL)
goto nospace;
}
m_copydata(m0, progress, length, mtod(m_new, caddr_t));
progress += length;
m_new->m_len = length;
if (m_new != m_final)
m_cat(m_final, m_new);
m_new = NULL;
}
#ifdef MBUF_STRESS_TEST
if (m0->m_next == NULL)
m_defraguseless++;
#endif
m_freem(m0);
m0 = m_final;
#ifdef MBUF_STRESS_TEST
m_defragpackets++;
m_defragbytes += m0->m_pkthdr.len;
#endif
return (m0);
nospace:
#ifdef MBUF_STRESS_TEST
m_defragfailure++;
#endif
if (m_final)
m_freem(m_final);
return (NULL);
}
struct mbuf *
ieee80211_ccmp_decrypt(struct ieee80211com *ic, struct mbuf *m0,
struct ieee80211_key *k)
{
struct ieee80211_ccmp_ctx *ctx = k->k_priv;
struct ieee80211_frame *wh;
u_int64_t pn, *prsc;
const u_int8_t *ivp, *src;
u_int8_t *dst;
u_int8_t mic0[IEEE80211_CCMP_MICLEN];
u_int8_t a[16], b[16], s0[16], s[16];
struct mbuf *n0, *m, *n;
int hdrlen, left, moff, noff, len;
u_int16_t ctr;
int i, j;
wh = mtod(m0, struct ieee80211_frame *);
hdrlen = ieee80211_get_hdrlen(wh);
ivp = (u_int8_t *)wh + hdrlen;
if (m0->m_pkthdr.len < hdrlen + IEEE80211_CCMP_HDRLEN +
IEEE80211_CCMP_MICLEN) {
m_freem(m0);
return NULL;
}
/* check that ExtIV bit is set */
if (!(ivp[3] & IEEE80211_WEP_EXTIV)) {
m_freem(m0);
return NULL;
}
/* retrieve last seen packet number for this frame type/priority */
if ((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) ==
IEEE80211_FC0_TYPE_DATA) {
u_int8_t tid = ieee80211_has_qos(wh) ?
ieee80211_get_qos(wh) & IEEE80211_QOS_TID : 0;
prsc = &k->k_rsc[tid];
} else /* 11w: management frames have their own counters */
prsc = &k->k_mgmt_rsc;
/* extract the 48-bit PN from the CCMP header */
pn = (u_int64_t)ivp[0] |
(u_int64_t)ivp[1] << 8 |
(u_int64_t)ivp[4] << 16 |
(u_int64_t)ivp[5] << 24 |
(u_int64_t)ivp[6] << 32 |
(u_int64_t)ivp[7] << 40;
if (pn <= *prsc) {
/* replayed frame, discard */
ic->ic_stats.is_ccmp_replays++;
m_freem(m0);
return NULL;
}
MGET(n0, M_DONTWAIT, m0->m_type);
if (n0 == NULL)
goto nospace;
if (m_dup_pkthdr(n0, m0, M_DONTWAIT))
goto nospace;
n0->m_pkthdr.len -= IEEE80211_CCMP_HDRLEN + IEEE80211_CCMP_MICLEN;
n0->m_len = MHLEN;
if (n0->m_pkthdr.len >= MINCLSIZE) {
MCLGET(n0, M_DONTWAIT);
if (n0->m_flags & M_EXT)
n0->m_len = n0->m_ext.ext_size;
}
if (n0->m_len > n0->m_pkthdr.len)
n0->m_len = n0->m_pkthdr.len;
/* construct initial B, A and S_0 blocks */
ieee80211_ccmp_phase1(&ctx->rijndael, wh, pn,
n0->m_pkthdr.len - hdrlen, b, a, s0);
/* copy 802.11 header and clear protected bit */
memcpy(mtod(n0, caddr_t), wh, hdrlen);
wh = mtod(n0, struct ieee80211_frame *);
wh->i_fc[1] &= ~IEEE80211_FC1_PROTECTED;
/* construct S_1 */
ctr = 1;
a[14] = ctr >> 8;
a[15] = ctr & 0xff;
rijndael_encrypt(&ctx->rijndael, a, s);
/* decrypt frame body and compute MIC */
j = 0;
m = m0;
n = n0;
moff = hdrlen + IEEE80211_CCMP_HDRLEN;
noff = hdrlen;
left = n0->m_pkthdr.len - noff;
while (left > 0) {
if (moff == m->m_len) {
/* nothing left to copy from m */
m = m->m_next;
moff = 0;
}
if (noff == n->m_len) {
/* n is full and there's more data to copy */
MGET(n->m_next, M_DONTWAIT, n->m_type);
if (n->m_next == NULL)
goto nospace;
n = n->m_next;
n->m_len = MLEN;
if (left >= MINCLSIZE) {
MCLGET(n, M_DONTWAIT);
if (n->m_flags & M_EXT)
n->m_len = n->m_ext.ext_size;
}
if (n->m_len > left)
n->m_len = left;
noff = 0;
}
len = min(m->m_len - moff, n->m_len - noff);
src = mtod(m, u_int8_t *) + moff;
dst = mtod(n, u_int8_t *) + noff;
for (i = 0; i < len; i++) {
/* decrypt message */
dst[i] = src[i] ^ s[j];
/* update MIC with clear text */
b[j] ^= dst[i];
if (++j < 16)
continue;
/* we have a full block, encrypt MIC */
rijndael_encrypt(&ctx->rijndael, b, b);
/* construct a new S_ctr block */
ctr++;
a[14] = ctr >> 8;
a[15] = ctr & 0xff;
rijndael_encrypt(&ctx->rijndael, a, s);
j = 0;
}
moff += len;
noff += len;
left -= len;
}
if (j != 0) /* partial block, encrypt MIC */
rijndael_encrypt(&ctx->rijndael, b, b);
/* finalize MIC, U := T XOR first-M-bytes( S_0 ) */
for (i = 0; i < IEEE80211_CCMP_MICLEN; i++)
b[i] ^= s0[i];
/* check that it matches the MIC in received frame */
m_copydata(m, moff, IEEE80211_CCMP_MICLEN, mic0);
if (timingsafe_bcmp(mic0, b, IEEE80211_CCMP_MICLEN) != 0) {
ic->ic_stats.is_ccmp_dec_errs++;
m_freem(m0);
m_freem(n0);
return NULL;
}
/* update last seen packet number (MIC is validated) */
*prsc = pn;
m_freem(m0);
return n0;
nospace:
ic->ic_stats.is_rx_nombuf++;
m_freem(m0);
if (n0 != NULL)
m_freem(n0);
return NULL;
}
struct mbuf *
ieee80211_ccmp_encrypt(struct ieee80211com *ic, struct mbuf *m0,
struct ieee80211_key *k)
{
struct ieee80211_ccmp_ctx *ctx = k->k_priv;
const struct ieee80211_frame *wh;
const u_int8_t *src;
u_int8_t *ivp, *mic, *dst;
u_int8_t a[16], b[16], s0[16], s[16];
struct mbuf *n0, *m, *n;
int hdrlen, left, moff, noff, len;
u_int16_t ctr;
int i, j;
MGET(n0, M_DONTWAIT, m0->m_type);
if (n0 == NULL)
goto nospace;
if (m_dup_pkthdr(n0, m0, M_DONTWAIT))
goto nospace;
n0->m_pkthdr.len += IEEE80211_CCMP_HDRLEN;
n0->m_len = MHLEN;
if (n0->m_pkthdr.len >= MINCLSIZE - IEEE80211_CCMP_MICLEN) {
MCLGET(n0, M_DONTWAIT);
if (n0->m_flags & M_EXT)
n0->m_len = n0->m_ext.ext_size;
}
if (n0->m_len > n0->m_pkthdr.len)
n0->m_len = n0->m_pkthdr.len;
/* copy 802.11 header */
wh = mtod(m0, struct ieee80211_frame *);
hdrlen = ieee80211_get_hdrlen(wh);
memcpy(mtod(n0, caddr_t), wh, hdrlen);
k->k_tsc++; /* increment the 48-bit PN */
/* construct CCMP header */
ivp = mtod(n0, u_int8_t *) + hdrlen;
ivp[0] = k->k_tsc; /* PN0 */
ivp[1] = k->k_tsc >> 8; /* PN1 */
ivp[2] = 0; /* Rsvd */
ivp[3] = k->k_id << 6 | IEEE80211_WEP_EXTIV; /* KeyID | ExtIV */
ivp[4] = k->k_tsc >> 16; /* PN2 */
ivp[5] = k->k_tsc >> 24; /* PN3 */
ivp[6] = k->k_tsc >> 32; /* PN4 */
ivp[7] = k->k_tsc >> 40; /* PN5 */
/* construct initial B, A and S_0 blocks */
ieee80211_ccmp_phase1(&ctx->rijndael, wh, k->k_tsc,
m0->m_pkthdr.len - hdrlen, b, a, s0);
/* construct S_1 */
ctr = 1;
a[14] = ctr >> 8;
a[15] = ctr & 0xff;
rijndael_encrypt(&ctx->rijndael, a, s);
/* encrypt frame body and compute MIC */
j = 0;
m = m0;
n = n0;
moff = hdrlen;
noff = hdrlen + IEEE80211_CCMP_HDRLEN;
left = m0->m_pkthdr.len - moff;
while (left > 0) {
if (moff == m->m_len) {
/* nothing left to copy from m */
m = m->m_next;
moff = 0;
}
if (noff == n->m_len) {
/* n is full and there's more data to copy */
MGET(n->m_next, M_DONTWAIT, n->m_type);
if (n->m_next == NULL)
goto nospace;
n = n->m_next;
n->m_len = MLEN;
if (left >= MINCLSIZE - IEEE80211_CCMP_MICLEN) {
MCLGET(n, M_DONTWAIT);
if (n->m_flags & M_EXT)
n->m_len = n->m_ext.ext_size;
}
if (n->m_len > left)
n->m_len = left;
noff = 0;
}
len = min(m->m_len - moff, n->m_len - noff);
src = mtod(m, u_int8_t *) + moff;
dst = mtod(n, u_int8_t *) + noff;
for (i = 0; i < len; i++) {
/* update MIC with clear text */
b[j] ^= src[i];
/* encrypt message */
dst[i] = src[i] ^ s[j];
if (++j < 16)
continue;
/* we have a full block, encrypt MIC */
rijndael_encrypt(&ctx->rijndael, b, b);
/* construct a new S_ctr block */
ctr++;
a[14] = ctr >> 8;
a[15] = ctr & 0xff;
rijndael_encrypt(&ctx->rijndael, a, s);
j = 0;
}
moff += len;
noff += len;
left -= len;
}
if (j != 0) /* partial block, encrypt MIC */
rijndael_encrypt(&ctx->rijndael, b, b);
/* reserve trailing space for MIC */
if (M_TRAILINGSPACE(n) < IEEE80211_CCMP_MICLEN) {
MGET(n->m_next, M_DONTWAIT, n->m_type);
if (n->m_next == NULL)
goto nospace;
n = n->m_next;
n->m_len = 0;
}
/* finalize MIC, U := T XOR first-M-bytes( S_0 ) */
mic = mtod(n, u_int8_t *) + n->m_len;
for (i = 0; i < IEEE80211_CCMP_MICLEN; i++)
mic[i] = b[i] ^ s0[i];
n->m_len += IEEE80211_CCMP_MICLEN;
n0->m_pkthdr.len += IEEE80211_CCMP_MICLEN;
m_freem(m0);
return n0;
nospace:
ic->ic_stats.is_tx_nombuf++;
m_freem(m0);
if (n0 != NULL)
m_freem(n0);
return NULL;
}
