int rtwlan_rx(struct rtskb * rtskb, struct rtnet_device * rtnet_dev)
{
struct rtwlan_device * rtwlan = rtnetdev_priv(rtnet_dev);
struct ieee80211_hdr * hdr = (struct ieee80211_hdr *)rtskb->data;
u16 fc = le16_to_cpu(hdr->frame_ctl);
switch(rtwlan->mode) {
case RTWLAN_MODE_RAW:
case RTWLAN_MODE_ACK:
rtskb_pull(rtskb, ieee80211_get_hdrlen(fc));
rtskb->protocol = rt_eth_type_trans (rtskb, rtnet_dev);
break;
case RTWLAN_MODE_MON:
rtskb->mac.raw = rtskb->data;
rtcap_mark_incoming(rtskb);
break;
}
return 0;
}
struct iob_s *ieee80211_decrypt(FAR struct ieee80211_s *ic,
FAR struct iob_s *iob0,
struct ieee80211_node *ni)
{
FAR struct ieee80211_frame *wh;
FAR struct ieee80211_key *k;
FAR uint8_t *ivp;
FAR uint8_t *mmie;
uint16_t kid;
int hdrlen;
/* Find key for decryption */
wh = (FAR struct ieee80211_frame *)IOB_DATA(iob0);
if ((ic->ic_flags & IEEE80211_F_RSNON) &&
!IEEE80211_IS_MULTICAST(wh->i_addr1) &&
ni->ni_rsncipher != IEEE80211_CIPHER_USEGROUP)
{
k = &ni->ni_pairwise_key;
}
else if (!IEEE80211_IS_MULTICAST(wh->i_addr1) ||
(wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) != IEEE80211_FC0_TYPE_MGT)
{
/* Retrieve group data key id from IV field */
hdrlen = ieee80211_get_hdrlen(wh);
/* Check that IV field is present */
if (iob0->io_len < hdrlen + 4)
{
iob_free_chain(iob0);
return NULL;
}
ivp = (uint8_t *) wh + hdrlen;
kid = ivp[3] >> 6;
k = &ic->ic_nw_keys[kid];
}
int
parse_80211_header(const u_char* buf, int len, struct packet_info* p)
{
struct ieee80211_hdr* wh;
int hdrlen;
uint8_t *sa = NULL;
uint8_t *da = NULL;
u16 fc;
if (len < 2)
return -1;
wh = (struct ieee80211_hdr*)buf;
fc = le16toh(wh->frame_control);
hdrlen = ieee80211_get_hdrlen(fc);
DEBUG("len %d hdrlen %d\n", len, hdrlen);
if (len < hdrlen)
return -1;
p->wlan_len = len;
p->wlan_type = (fc & (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE));
switch (p->wlan_type & IEEE80211_FCTL_FTYPE) {
case IEEE80211_FTYPE_DATA:
p->pkt_types = PKT_TYPE_DATA;
switch (p->wlan_type & IEEE80211_FCTL_STYPE) {
case IEEE80211_STYPE_NULLFUNC:
p->pkt_types |= PKT_TYPE_NULL;
break;
case IEEE80211_STYPE_QOS_DATA:
/* TODO: ouch, should properly define a qos header */
p->wlan_qos_class = wh->addr4[0] & 0x7;
DEBUG("***QDATA %x\n", p->wlan_qos_class);
break;
}
sa = ieee80211_get_SA(wh);
da = ieee80211_get_DA(wh);
p->wlan_seqno = le16toh(wh->seq_ctrl) / 16;
if (fc & IEEE80211_FCTL_PROTECTED)
p->wlan_wep = 1;
if (fc & IEEE80211_FCTL_RETRY)
p->wlan_retry = 1;
break;
case IEEE80211_FTYPE_CTL:
p->pkt_types = PKT_TYPE_CTRL;
DEBUG("CTL\n");
switch (p->wlan_type & IEEE80211_FCTL_STYPE) {
case IEEE80211_STYPE_RTS:
p->pkt_types |= PKT_TYPE_RTS;
p->wlan_nav = le16toh(wh->duration_id);
DEBUG("RTS NAV %d\n", p->wlan_nav);
sa = wh->addr2;
da = wh->addr1;
break;
case IEEE80211_STYPE_CTS:
p->pkt_types |= PKT_TYPE_CTS;
p->wlan_nav = le16toh(wh->duration_id);
DEBUG("CTS NAV %d\n", p->wlan_nav);
da = wh->addr1;
break;
case IEEE80211_STYPE_ACK:
p->pkt_types |= PKT_TYPE_ACK;
p->wlan_nav = le16toh(wh->duration_id);
DEBUG("ACK NAV %d\n", p->wlan_nav);
da = wh->addr1;
break;
case IEEE80211_STYPE_PSPOLL:
sa = wh->addr2;
break;
case IEEE80211_STYPE_CFEND:
da = wh->addr1;
sa = wh->addr2;
break;
case IEEE80211_STYPE_CFENDACK:
/* dont know, dont care */
break;
}
break;
case IEEE80211_FTYPE_MGMT:
p->pkt_types = PKT_TYPE_MGMT;
DEBUG("MGMT\n");
break;
}
if (sa != NULL) {
memcpy(p->wlan_src, sa, 6);
DEBUG("SA %s\n", ether_sprintf(sa));
}
if (da != NULL) {
memcpy(p->wlan_dst, da, 6);
DEBUG("DA %s\n", ether_sprintf(da));
}
DEBUG("%s\n", get_packet_type_name(fc));
return 0;
}
int CVE_2007_4997_linux2_6_16_ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
struct CVE_2007_4997_linux2_6_16_ieee80211_rx_stats *rx_stats)
{
struct net_device *dev = ieee->dev;
struct ieee80211_hdr_4addr *hdr;
size_t hdrlen;
u16 fc, type, stype, sc;
struct net_device_stats *stats;
unsigned int frag;
u8 *payload;
u16 ethertype;
#ifdef NOT_YET
struct net_device *wds = NULL;
struct sk_buff *skb2 = NULL;
struct net_device *wds = NULL;
int frame_authorized = 0;
int from_assoc_ap = 0;
void *sta = NULL;
#endif
u8 dst[ETH_ALEN];
u8 src[ETH_ALEN];
struct ieee80211_crypt_data *crypt = NULL;
int keyidx = 0;
int can_be_decrypted = 0;
hdr = (struct ieee80211_hdr_4addr *)skb->data;
stats = &ieee->stats;
if (skb->len < 10) {
printk(KERN_INFO "%s: SKB length < 10\n", dev->name);
goto rx_dropped;
}
fc = le16_to_cpu(hdr->frame_ctl);
type = WLAN_FC_GET_TYPE(fc);
stype = WLAN_FC_GET_STYPE(fc);
sc = le16_to_cpu(hdr->seq_ctl);
frag = WLAN_GET_SEQ_FRAG(sc);
hdrlen = ieee80211_get_hdrlen(fc);
/* Put this code here so that we avoid duplicating it in all
* Rx paths. - Jean II */
#ifdef IW_WIRELESS_SPY /* defined in iw_handler.h */
#ifdef CONFIG_NET_RADIO
/* If spy monitoring on */
if (ieee->spy_data.spy_number > 0) {
struct iw_quality wstats;
wstats.updated = 0;
if (rx_stats->mask & IEEE80211_STATMASK_RSSI) {
wstats.level = rx_stats->rssi;
wstats.updated |= IW_QUAL_LEVEL_UPDATED;
} else
wstats.updated |= IW_QUAL_LEVEL_INVALID;
if (rx_stats->mask & IEEE80211_STATMASK_NOISE) {
wstats.noise = rx_stats->noise;
wstats.updated |= IW_QUAL_NOISE_UPDATED;
} else
wstats.updated |= IW_QUAL_NOISE_INVALID;
if (rx_stats->mask & IEEE80211_STATMASK_SIGNAL) {
wstats.qual = rx_stats->signal;
wstats.updated |= IW_QUAL_QUAL_UPDATED;
} else
wstats.updated |= IW_QUAL_QUAL_INVALID;
/* Update spy records */
wireless_spy_update(ieee->dev, hdr->addr2, &wstats);
}
#endif /* CONFIG_NET_RADIO */
#endif /* IW_WIRELESS_SPY */
#ifdef NOT_YET
hostap_update_rx_stats(local->ap, hdr, rx_stats);
#endif
if (ieee->iw_mode == IW_MODE_MONITOR) {
ieee80211_monitor_rx(ieee, skb, rx_stats);
stats->rx_packets++;
stats->rx_bytes += skb->len;
return 1;
}
can_be_decrypted = (is_multicast_ether_addr(hdr->addr1) ||
is_broadcast_ether_addr(hdr->addr2)) ?
ieee->host_mc_decrypt : ieee->host_decrypt;
if (can_be_decrypted) {
int idx = 0;
if (skb->len >= hdrlen + 3) {
/* Top two-bits of byte 3 are the key index */
idx = skb->data[hdrlen + 3] >> 6;
}
/* ieee->crypt[] is WEP_KEY (4) in length. Given that idx
* is only allowed 2-bits of storage, no value of idx can
* be provided via above code that would result in idx
* being out of range */
crypt = ieee->crypt[idx];
#ifdef NOT_YET
sta = NULL;
/* Use station specific key to override default keys if the
* receiver address is a unicast address ("individual RA"). If
* bcrx_sta_key parameter is set, station specific key is used
* even with broad/multicast targets (this is against IEEE
* 802.11, but makes it easier to use different keys with
* stations that do not support WEP key mapping). */
if (!(hdr->addr1[0] & 0x01) || local->bcrx_sta_key)
(void)hostap_handle_sta_crypto(local, hdr, &crypt,
&sta);
#endif
/* allow NULL decrypt to indicate an station specific override
* for default encryption */
if (crypt && (crypt->ops == NULL ||
crypt->ops->decrypt_mpdu == NULL))
crypt = NULL;
if (!crypt && (fc & IEEE80211_FCTL_PROTECTED)) {
/* This seems to be triggered by some (multicast?)
* frames from other than current BSS, so just drop the
* frames silently instead of filling system log with
* these reports. */
IEEE80211_DEBUG_DROP("Decryption failed (not set)"
" (SA=" MAC_FMT ")\n",
MAC_ARG(hdr->addr2));
ieee->ieee_stats.rx_discards_undecryptable++;
goto rx_dropped;
}
}
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;
}
struct iob_s *ieee80211_ccmp_decrypt(struct ieee80211_s *ic, struct iob_s *iob0,
struct ieee80211_key *k)
{
struct ieee80211_ccmp_ctx *ctx = k->k_priv;
struct ieee80211_frame *wh;
uint64_t pn, *prsc;
const uint8_t *ivp;
const uint8_t *src;
uint8_t *dst;
uint8_t mic0[IEEE80211_CCMP_MICLEN];
uint8_t a[16];
uint8_t b[16];
uint8_t s0[16];
uint8_t s[16];
struct iob_s *next0;
struct iob_s *iob;
struct iob_s *next;
int hdrlen;
int left;
int moff;
int noff;
int len;
uint16_t ctr;
int i;
int j;
wh = (FAR struct ieee80211_frame *)IOB_DATA(iob0);
hdrlen = ieee80211_get_hdrlen(wh);
ivp = (uint8_t *) wh + hdrlen;
if (iob0->io_pktlen < hdrlen + IEEE80211_CCMP_HDRLEN + IEEE80211_CCMP_MICLEN)
{
iob_free_chain(iob0);
return NULL;
}
/* Check that ExtIV bit is set */
if (!(ivp[3] & IEEE80211_WEP_EXTIV))
{
iob_free_chain(iob0);
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)
{
uint8_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 = (uint64_t) ivp[0] |
(uint64_t) ivp[1] << 8 |
(uint64_t) ivp[4] << 16 |
(uint64_t) ivp[5] << 24 | (uint64_t) ivp[6] << 32 | (uint64_t) ivp[7] << 40;
if (pn <= *prsc)
{
/* Replayed frame, discard */
iob_free_chain(iob0);
return NULL;
}
next0 = iob_alloc(false);
if (next0 == NULL)
{
goto nospace;
}
if (iob_clone(next0, iob0) < 0)
{
goto nospace;
}
next0->io_pktlen -= IEEE80211_CCMP_HDRLEN + IEEE80211_CCMP_MICLEN;
next0->io_len = CONFIG_IEEE80211_BUFSIZE;
if (next0->io_len > next0->io_pktlen)
{
next0->io_len = next0->io_pktlen;
}
/* Construct initial B, A and S_0 blocks */
ieee80211_ccmp_phase1(&ctx->rijndael, wh, pn,
next0->io_pktlen - hdrlen, b, a, s0);
/* Copy 802.11 header and clear protected bit */
memcpy(IOB_DATA(next0), wh, hdrlen);
wh = (FAR struct ieee80211_frame *)IOB_DATA(next0);
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;
iob = iob0;
next = next0;
moff = hdrlen + IEEE80211_CCMP_HDRLEN;
noff = hdrlen;
left = next0->io_pktlen - noff;
while (left > 0)
{
if (moff == iob->io_len)
{
/* Nothing left to copy from iob */
iob = iob->io_flink;
moff = 0;
}
if (noff == next->io_len)
{
struct iob_s *newbuf;
/* next is full and there's more data to copy */
newbuf = iob_alloc(false);
if (newbuf == NULL)
{
goto nospace;
}
next->io_flink = newbuf;
next = newbuf;
next->io_len = 0;
if (next->io_len > left)
{
next->io_len = left;
}
noff = 0;
}
len = MIN(iob->io_len - moff, next->io_len - noff);
src = (FAR uint8_t *) IOB_DATA(iob) + moff;
dst = (FAR uint8_t *) IOB_DATA(next) + 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 */
iob_copyout(mic0, iob, moff, IEEE80211_CCMP_MICLEN);
if (memcmp(mic0, b, IEEE80211_CCMP_MICLEN) != 0)
{
iob_free_chain(iob0);
iob_free_chain(next0);
return NULL;
}
/* update last seen packet number (MIC is validated) */
*prsc = pn;
iob_free_chain(iob0);
return next0;
nospace:
iob_free_chain(iob0);
if (next0 != NULL)
{
iob_free_chain(next0);
}
return NULL;
}
struct iob_s *ieee80211_ccmp_encrypt(struct ieee80211_s *ic, struct iob_s *iob0,
struct ieee80211_key *k)
{
struct ieee80211_ccmp_ctx *ctx = k->k_priv;
const struct ieee80211_frame *wh;
const uint8_t *src;
uint8_t *ivp, *mic, *dst;
uint8_t a[16], b[16], s0[16], s[16];
struct iob_s *next0, *iob, *next;
int hdrlen, left, moff, noff, len;
uint16_t ctr;
int i, j;
next0 = iob_alloc(false);
if (next0 == NULL)
{
goto nospace;
}
if (iob_clone(next0, iob0) < 0)
{
goto nospace;
}
next0->io_pktlen += IEEE80211_CCMP_HDRLEN;
next0->io_len = CONFIG_IEEE80211_BUFSIZE;
if (next0->io_len > next0->io_pktlen)
{
next0->io_len = next0->io_pktlen;
}
/* Copy 802.11 header */
wh = (FAR struct ieee80211_frame *)IOB_DATA(iob0);
hdrlen = ieee80211_get_hdrlen(wh);
memcpy(IOB_DATA(next0), wh, hdrlen);
k->k_tsc++; /* increment the 48-bit PN */
/* construct CCMP header */
ivp = (FAR uint8_t *) IOB_DATA(next0) + 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,
iob0->io_pktlen - 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;
iob = iob0;
next = next0;
moff = hdrlen;
noff = hdrlen + IEEE80211_CCMP_HDRLEN;
left = iob0->io_pktlen - moff;
while (left > 0)
{
if (moff == iob->io_len)
{
/* Nothing left to copy from iob */
iob = iob->io_flink;
moff = 0;
}
if (noff == next->io_len)
{
struct iob_s *newbuf;
/* next is full and there's more data to copy */
newbuf = iob_alloc(false);
if (newbuf == NULL)
{
goto nospace;
}
next->io_flink = newbuf;
next = newbuf;
next->io_len = 0;
if (next->io_len > left)
{
next->io_len = left;
}
noff = 0;
}
len = MIN(iob->io_len - moff, next->io_len - noff);
src = (FAR uint8_t *) IOB_DATA(iob) + moff;
dst = (FAR uint8_t *) IOB_DATA(next) + 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 (IOB_FREESPACE(next) < IEEE80211_CCMP_MICLEN)
{
struct iob_s *newbuf;
newbuf = iob_alloc(false);
if (newbuf == NULL)
{
goto nospace;
}
next->io_flink = newbuf;
next = newbuf;
next->io_len = 0;
}
/* Finalize MIC, U := T XOR first-M-bytes( S_0 ) */
mic = (FAR uint8_t *) IOB_DATA(next) + next->io_len;
for (i = 0; i < IEEE80211_CCMP_MICLEN; i++)
{
mic[i] = b[i] ^ s0[i];
}
next->io_len += IEEE80211_CCMP_MICLEN;
next0->io_pktlen += IEEE80211_CCMP_MICLEN;
iob_free_chain(iob0);
return next0;
nospace:
iob_free_chain(iob0);
if (next0 != NULL)
{
iob_free_chain(next0);
}
return NULL;
}
void ieee80211_input(struct ieee80211_s *ic, struct iob_s *iob,
struct ieee80211_node *ni, struct ieee80211_rxinfo *rxi)
{
struct ieee80211_frame *wh;
uint16_t *orxseq, nrxseq, qos;
uint8_t dir, type, subtype, tid;
int hdrlen, hasqos;
DEBUGASSERT(ni != NULL);
/* in monitor mode, send everything directly to bpf */
if (ic->ic_opmode == IEEE80211_M_MONITOR)
goto out;
/* Do not process frames without an Address 2 field any further.
* Only CTS and ACK control frames do not have this field.
*/
if (iob->io_len < sizeof(struct ieee80211_frame_min))
{
ndbg("ERROR: frame too short, len %u\n", iob->io_len);
goto out;
}
wh = (FAR struct ieee80211_frame *)IOB_DATA(iob);
if ((wh->i_fc[0] & IEEE80211_FC0_VERSION_MASK) != IEEE80211_FC0_VERSION_0)
{
ndbg("ERROR: frame with wrong version: %x\n", wh->i_fc[0]);
goto err;
}
dir = wh->i_fc[1] & IEEE80211_FC1_DIR_MASK;
type = wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK;
if (type != IEEE80211_FC0_TYPE_CTL)
{
hdrlen = ieee80211_get_hdrlen(wh);
if (iob->io_len < hdrlen)
{
ndbg("ERROR: frame too short, len %u\n", iob->io_len);
goto err;
}
}
if ((hasqos = ieee80211_has_qos(wh)))
{
qos = ieee80211_get_qos(wh);
tid = qos & IEEE80211_QOS_TID;
}
else
{
qos = 0;
tid = 0;
}
/* duplicate detection (see 9.2.9) */
if (ieee80211_has_seq(wh) && ic->ic_state != IEEE80211_S_SCAN)
{
nrxseq = letoh16(*(uint16_t *) wh->i_seq) >> IEEE80211_SEQ_SEQ_SHIFT;
if (hasqos)
orxseq = &ni->ni_qos_rxseqs[tid];
else
orxseq = &ni->ni_rxseq;
if ((wh->i_fc[1] & IEEE80211_FC1_RETRY) && nrxseq == *orxseq)
{
/* duplicate, silently discarded */
goto out;
}
*orxseq = nrxseq;
}
