FAR struct tcp_wrbuffer_s *tcp_wrbuffer_alloc(void)
{
FAR struct tcp_wrbuffer_s *wrb;
/* We need to allocate two things: (1) A write buffer structure and (2)
* at least one I/O buffer to start the chain.
*
* Allocate the write buffer structure first then the IOBG. In order to
* avoid deadlocks, we will need to free the IOB first, then the write
* buffer
*/
DEBUGVERIFY(net_lockedwait(&g_wrbuffer.sem));
/* Now, we are guaranteed to have a write buffer structure reserved
* for us in the free list.
*/
wrb = (FAR struct tcp_wrbuffer_s *)sq_remfirst(&g_wrbuffer.freebuffers);
DEBUGASSERT(wrb);
memset(wrb, 0, sizeof(struct tcp_wrbuffer_s));
/* Now get the first I/O buffer for the write buffer structure */
wrb->wb_iob = iob_alloc(false);
if (!wrb->wb_iob)
{
ndbg("ERROR: Failed to allocate I/O buffer\n");
tcp_wrbuffer_release(wrb);
return NULL;
}
return wrb;
}
static int iob_copyin_internal(FAR struct iob_s *iob, FAR const uint8_t *src,
unsigned int len, unsigned int offset,
bool throttled, bool can_block)
{
FAR struct iob_s *head = iob;
FAR struct iob_s *next;
FAR uint8_t *dest;
unsigned int ncopy;
unsigned int avail;
unsigned int total = len;
ninfo("iob=%p len=%u offset=%u\n", iob, len, offset);
DEBUGASSERT(iob && src);
/* The offset must applied to data that is already in the I/O buffer chain */
if (offset > iob->io_pktlen)
{
nerr("ERROR: offset is past the end of data: %u > %u\n",
offset, iob->io_pktlen);
return -ESPIPE;
}
/* Skip to the I/O buffer containing the data offset */
while (offset > iob->io_len)
{
offset -= iob->io_len;
iob = iob->io_flink;
}
/* Then loop until all of the I/O data is copied from the user buffer */
while (len > 0)
{
next = iob->io_flink;
/* Get the destination I/O buffer address and the amount of data
* available from that address.
*/
dest = &iob->io_data[iob->io_offset + offset];
avail = iob->io_len - offset;
ninfo("iob=%p avail=%u len=%u next=%p\n", iob, avail, len, next);
/* Will the rest of the copy fit into this buffer, overwriting
* existing data.
*/
if (len > avail)
{
/* No.. Is this the last buffer in the chain? */
if (next)
{
/* No.. clip to size that will overwrite. We cannot
* extend the length of an I/O block in mid-chain.
*/
ncopy = avail;
}
else
{
unsigned int maxlen;
unsigned int newlen;
/* Yes.. We can extend this buffer to the up to the very end. */
maxlen = CONFIG_IOB_BUFSIZE - iob->io_offset;
/* This is the new buffer length that we need. Of course,
* clipped to the maximum possible size in this buffer.
*/
newlen = len + offset;
if (newlen > maxlen)
{
newlen = maxlen;
}
/* Set the new length and increment the packet length */
head->io_pktlen += (newlen - iob->io_len);
iob->io_len = newlen;
/* Set the new number of bytes to copy */
ncopy = newlen - offset;
}
}
else
{
/* Yes.. Copy all of the remaining bytes */
ncopy = len;
}
/* Copy from the user buffer to the I/O buffer. */
memcpy(dest, src, ncopy);
ninfo("iob=%p Copy %u bytes new len=%u\n",
iob, ncopy, iob->io_len);
/* Adjust the total length of the copy and the destination address in
* the user buffer.
*/
len -= ncopy;
src += ncopy;
/* Skip to the next I/O buffer in the chain. First, check if we
* are at the end of the buffer chain.
*/
if (len > 0 && !next)
{
/* Yes.. allocate a new buffer.
*
* Copy as many bytes as possible. If we have successfully copied
* any already don't block, otherwise block if we're allowed.
*/
if (!can_block || len < total)
{
next = iob_tryalloc(throttled);
}
else
{
next = iob_alloc(throttled);
}
if (next == NULL)
{
nerr("ERROR: Failed to allocate I/O buffer\n");
return len;
}
/* Add the new, empty I/O buffer to the end of the buffer chain. */
iob->io_flink = next;
ninfo("iob=%p added to the chain\n", iob);
}
iob = next;
offset = 0;
}
return 0;
}
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;
}
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;
}
int iob_clone(FAR struct iob_s *iob1, FAR struct iob_s *iob2, bool throttled)
{
FAR uint8_t *src;
FAR uint8_t *dest;
unsigned int ncopy;
unsigned int avail1;
unsigned int avail2;
unsigned int offset1;
unsigned int offset2;
DEBUGASSERT(iob2->io_len == 0 && iob2->io_offset == 0 &&
iob2->io_pktlen == 0 && iob2->io_flink == NULL);
/* Copy the total packet size from the I/O buffer at the head of the chain */
iob2->io_pktlen = iob1->io_pktlen;
/* Handle special case where there are empty buffers at the head
* the the list.
*/
while (iob1->io_len <= 0)
{
iob1 = iob1->io_flink;
}
/* Pack each entry from iob1 to iob2 */
offset1 = 0;
offset2 = 0;
while (iob1)
{
/* Get the source I/O buffer pointer and the number of bytes to copy
* from this address.
*/
src = &iob1->io_data[iob1->io_offset + offset1];
avail1 = iob1->io_len - offset1;
/* Get the destination I/O buffer pointer and the number of bytes to
* copy to that address.
*/
dest = &iob2->io_data[offset2];
avail2 = CONFIG_IOB_BUFSIZE - offset2;
/* Copy the smaller of the two and update the srce and destination
* offsets.
*/
ncopy = MIN(avail1, avail2);
memcpy(dest, src, ncopy);
offset1 += ncopy;
offset2 += ncopy;
/* Have we taken all of the data from the source I/O buffer? */
if (offset1 >= iob1->io_len)
{
/* Skip over empty entries in the chain (there should not be any
* but just to be safe).
*/
do
{
/* Yes.. move to the next source I/O buffer */
iob1 = iob1->io_flink;
}
while (iob1->io_len <= 0);
/* Reset the offset to the beginning of the I/O buffer */
offset1 = 0;
}
/* Have we filled the destination I/O buffer? Is there more data to be
* transferred?
*/
if (offset2 >= CONFIG_IOB_BUFSIZE && iob1 != NULL)
{
FAR struct iob_s *next;
/* Allocate new destination I/O buffer and hook it into the
* destination I/O buffer chain.
*/
next = iob_alloc(throttled);
if (!next)
{
nerr("ERROR: Failed to allocate an I/O buffer/n");
return -ENOMEM;
}
iob2->io_flink = next;
iob2 = next;
offset2 = 0;
}
}
return 0;
}
