Beispiel #1
0
void devif_iob_send(FAR struct net_driver_s *dev, FAR struct iob_s *iob,
                    unsigned int len, unsigned int offset)
{
  DEBUGASSERT(dev && len > 0 && len < NET_DEV_MTU(dev));

  /* Copy the data from the I/O buffer chain to the device buffer */

  iob_copyout(dev->d_snddata, iob, len, offset);
  dev->d_sndlen = len;

#ifdef CONFIG_NET_TCP_WRBUFFER_DUMP
  /* Dump the outgoing device buffer */

  lib_dumpbuffer("devif_iob_send", dev->d_snddata, len);
#endif
}
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;
}