struct iob_s *ieee80211_encrypt(struct ieee80211_s *ic, struct iob_s *iob0,
struct ieee80211_key *k)
{
switch (k->k_cipher)
{
case IEEE80211_CIPHER_WEP40:
case IEEE80211_CIPHER_WEP104:
iob0 = ieee80211_wep_encrypt(ic, iob0, k);
break;
case IEEE80211_CIPHER_TKIP:
iob0 = ieee80211_tkip_encrypt(ic, iob0, k);
break;
case IEEE80211_CIPHER_CCMP:
iob0 = ieee80211_ccmp_encrypt(ic, iob0, k);
break;
case IEEE80211_CIPHER_BIP:
iob0 = ieee80211_bip_encap(ic, iob0, k);
break;
default:
/* Should not get there */
iob_free_chain(iob0);
iob0 = NULL;
}
return iob0;
}
uint16_t tcp_datahandler(FAR struct tcp_conn_s *conn, FAR uint8_t *buffer,
uint16_t buflen)
{
FAR struct iob_s *iob;
int ret;
/* Try to allocate on I/O buffer to start the chain without waiting (and
* throttling as necessary). If we would have to wait, then drop the
* packet.
*/
iob = iob_tryalloc(true);
if (iob == NULL)
{
nlldbg("ERROR: Failed to create new I/O buffer chain\n");
return 0;
}
/* Copy the new appdata into the I/O buffer chain (without waiting) */
ret = iob_trycopyin(iob, buffer, buflen, 0, true);
if (ret < 0)
{
/* On a failure, iob_copyin return a negated error value but does
* not free any I/O buffers.
*/
nlldbg("ERROR: Failed to add data to the I/O buffer chain: %d\n", ret);
(void)iob_free_chain(iob);
return 0;
}
/* Add the new I/O buffer chain to the tail of the read-ahead queue (again
* without waiting).
*/
ret = iob_tryadd_queue(iob, &conn->readahead);
if (ret < 0)
{
nlldbg("ERROR: Failed to queue the I/O buffer chain: %d\n", ret);
(void)iob_free_chain(iob);
return 0;
}
nllvdbg("Buffered %d bytes\n", buflen);
return buflen;
}
void tcp_wrbuffer_release(FAR struct tcp_wrbuffer_s *wrb)
{
DEBUGASSERT(wrb && wrb->wb_iob);
/* To avoid deadlocks, we must following this ordering: Release the I/O
* buffer chain first, then the write buffer structure.
*/
iob_free_chain(wrb->wb_iob);
/* Then free the write buffer structure */
sq_addlast(&wrb->wb_node, &g_wrbuffer.freebuffers);
sem_post(&g_wrbuffer.sem);
}
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 ieee80211_output(FAR struct ieee80211_s *ic, FAR struct iob_s *iob,
FAR struct sockaddr *dst, uint8_t flags)
{
FAR struct uip_driver_s *dev;
FAR struct ieee80211_frame *wh;
FAR struct m_tag *mtag;
uip_lock_t flags;
int error = 0;
/* Get the driver structure */
dev = netdev_findbyaddr(ic->ic_ifname);
if (!dev)
{
error = -ENODEV;
goto bad;
}
/* Interface has to be up and running */
if ((dev->d_flags & (IFF_UP | IFF_RUNNING)) != (IFF_UP | IFF_RUNNING))
{
error = -ENETDOWN;
goto bad;
}
/* Try to get the DLT from a buffer tag */
if ((mtag = m_tag_find(iob, PACKET_TAG_DLT, NULL)) != NULL)
{
unsigned int dlt = *(unsigned int *)(mtag + 1);
/* Fallback to Ethernet for non-802.11 linktypes */
if (!(dlt == DLT_IEEE802_11 || dlt == DLT_IEEE802_11_RADIO))
{
goto fallback;
}
if (iob->io_pktlen < sizeof(struct ieee80211_frame_min))
{
return -EINVAL;
}
wh = (FAR struct ieee80211_frame *)IOB_DATA(iob);
if ((wh->i_fc[0] & IEEE80211_FC0_VERSION_MASK) != IEEE80211_FC0_VERSION_0)
{
return -EINVAL;
}
if (!(ic->ic_caps & IEEE80211_C_RAWCTL) &&
(wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) == IEEE80211_FC0_TYPE_CTL)
{
return -EINVAL;
}
/* Queue message on interface without adding any further headers, and
* start output if interface not yet active.
*/
flags = uip_lock();
error = ieee80211_ifsend(ic, iob, flags);
if (error)
{
/* buffer is already freed */
uip_unlock(flags);
ndbg("ERROR: %s: failed to queue raw tx frame\n", ic->ic_ifname);
return error;
}
uip_unlock(flags);
return error;
}
fallback:
return ether_output(ic, iob, dst);
bad:
if (iob)
{
iob_free_chain(iob);
}
return error;
}
static uint16_t udp_datahandler(FAR struct net_driver_s *dev, FAR struct udp_conn_s *conn,
FAR uint8_t *buffer, uint16_t buflen)
{
FAR struct iob_s *iob;
int ret;
#ifdef CONFIG_NET_IPv6
FAR struct sockaddr_in6 src_addr6;
#endif
#ifdef CONFIG_NET_IPv4
FAR struct sockaddr_in src_addr4;
#endif
FAR void *src_addr;
uint8_t src_addr_size;
/* Allocate on I/O buffer to start the chain (throttling as necessary).
* We will not wait for an I/O buffer to become available in this context.
*/
iob = iob_tryalloc(true);
if (iob == NULL)
{
nerr("ERROR: Failed to create new I/O buffer chain\n");
return 0;
}
#ifdef CONFIG_NET_IPv6
#ifdef CONFIG_NET_IPv4
if (IFF_IS_IPv6(dev->d_flags))
#endif
{
FAR struct udp_hdr_s *udp = UDPIPv6BUF;
FAR struct ipv6_hdr_s *ipv6 = IPv6BUF;
src_addr6.sin6_family = AF_INET6;
src_addr6.sin6_port = udp->srcport;
net_ipv6addr_copy(src_addr6.sin6_addr.s6_addr, ipv6->srcipaddr);
src_addr_size = sizeof(src_addr6);
src_addr = &src_addr6;
}
#endif /* CONFIG_NET_IPv6 */
#ifdef CONFIG_NET_IPv4
#ifdef CONFIG_NET_IPv6
else
#endif
{
#ifdef CONFIG_NET_IPv6
/* Hybrid dual-stack IPv6/IPv4 implementations recognize a special
* class of addresses, the IPv4-mapped IPv6 addresses.
*/
if (conn->domain == PF_INET6)
{
FAR struct udp_hdr_s *udp = UDPIPv6BUF;
FAR struct ipv6_hdr_s *ipv6 = IPv6BUF;
in_addr_t ipv4addr;
/* Encode the IPv4 address as an IPv-mapped IPv6 address */
src_addr6.sin6_family = AF_INET6;
src_addr6.sin6_port = udp->srcport;
ipv4addr = net_ip4addr_conv32(ipv6->srcipaddr);
ip6_map_ipv4addr(ipv4addr, src_addr6.sin6_addr.s6_addr16);
src_addr_size = sizeof(src_addr6);
src_addr = &src_addr6;
}
else
#endif
{
FAR struct udp_hdr_s *udp = UDPIPv4BUF;
FAR struct ipv4_hdr_s *ipv4 = IPv4BUF;
src_addr4.sin_family = AF_INET;
src_addr4.sin_port = udp->srcport;
net_ipv4addr_copy(src_addr4.sin_addr.s_addr,
net_ip4addr_conv32(ipv4->srcipaddr));
src_addr_size = sizeof(src_addr4);
src_addr = &src_addr4;
}
}
#endif /* CONFIG_NET_IPv4 */
/* Copy the src address info into the I/O buffer chain. We will not wait
* for an I/O buffer to become available in this context. It there is
* any failure to allocated, the entire I/O buffer chain will be discarded.
*/
ret = iob_trycopyin(iob, (FAR const uint8_t *)&src_addr_size,
sizeof(uint8_t), 0, true);
if (ret < 0)
{
/* On a failure, iob_trycopyin return a negated error value but does
* not free any I/O buffers.
*/
nerr("ERROR: Failed to add data to the I/O buffer chain: %d\n", ret);
(void)iob_free_chain(iob);
return 0;
}
ret = iob_trycopyin(iob, (FAR const uint8_t *)src_addr, src_addr_size,
sizeof(uint8_t), true);
if (ret < 0)
{
/* On a failure, iob_trycopyin return a negated error value but does
* not free any I/O buffers.
*/
nerr("ERROR: Failed to add data to the I/O buffer chain: %d\n", ret);
(void)iob_free_chain(iob);
return 0;
}
if (buflen > 0)
{
/* Copy the new appdata into the I/O buffer chain */
ret = iob_trycopyin(iob, buffer, buflen,
src_addr_size + sizeof(uint8_t), true);
if (ret < 0)
{
/* On a failure, iob_trycopyin return a negated error value but
* does not free any I/O buffers.
*/
nerr("ERROR: Failed to add data to the I/O buffer chain: %d\n",
ret);
(void)iob_free_chain(iob);
return 0;
}
}
/* Add the new I/O buffer chain to the tail of the read-ahead queue */
ret = iob_tryadd_queue(iob, &conn->readahead);
if (ret < 0)
{
nerr("ERROR: Failed to queue the I/O buffer chain: %d\n", ret);
(void)iob_free_chain(iob);
return 0;
}
#ifdef CONFIG_UDP_READAHEAD_NOTIFIER
/* Provided notification(s) that additional UDP read-ahead data is
* available.
*/
udp_notifier_signal(conn);
#endif
ninfo("Buffered %d bytes\n", buflen);
return buflen;
}
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;
}
