asn1_dec.c

lm3s下lwip的udp

 * @note ASN coded integers are _always_ signed!
 */
err_t
snmp_asn1_dec_s32t(struct pbuf *p, u16_t ofs, u16_t len, s32_t *value)
{
  u16_t plen, base;
  u8_t *msg_ptr;
#if BYTE_ORDER == LITTLE_ENDIAN
  u8_t *lsb_ptr = (u8_t*)value;
#endif
#if BYTE_ORDER == BIG_ENDIAN
  u8_t *lsb_ptr = (u8_t*)value + sizeof(s32_t) - 1;
#endif
  u8_t sign;

  plen = 0;
  while (p != NULL)
  {
    base = plen;
    plen += p->len;
    if (ofs < plen)
    {
      msg_ptr = p->payload;
      msg_ptr += ofs - base;
      if ((len > 0) && (len < 5))
      {
        if (*msg_ptr & 0x80)
        {
          /* negative, start from -1 */
          *value = -1;
          sign = 1;
        }
        else
        {
          /* positive, start from 0 */
          *value = 0;
          sign = 0;
        }
        /* OR/AND octets with value */
        while (len > 1)
        {
          len--;
          if (sign)
          {
            *lsb_ptr &= *msg_ptr;
            *value <<= 8;
            *lsb_ptr |= 255;
          }
          else
          {
            *lsb_ptr |= *msg_ptr;
            *value <<= 8;
          }
          ofs += 1;
          if (ofs >= plen)
          {
            /* next octet in next pbuf */
            p = p->next;
            if (p == NULL) { return ERR_ARG; }
            msg_ptr = p->payload;
            plen += p->len;
          }
          else
          {
            /* next octet in same pbuf */
            msg_ptr++;
          }
        }
        if (sign)
        {
          *lsb_ptr &= *msg_ptr;
        }
        else
        {
          *lsb_ptr |= *msg_ptr;
        }
        return ERR_OK;
      }
      else
      {
        return ERR_ARG;
      }
    }
    p = p->next;
  }
  /* p == NULL, ofs >= plen */
  return ERR_ARG;
}

/**
 * Decodes object identifier from incoming message into array of s32_t.
 *
 * @param p points to a pbuf holding an ASN1 coded object identifier
 * @param ofs points to the offset within the pbuf chain of the ASN1 coded object identifier
 * @param len length of the coded object identifier
 * @param oid return object identifier struct
 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
 */
err_t
snmp_asn1_dec_oid(struct pbuf *p, u16_t ofs, u16_t len, struct snmp_obj_id *oid)
{
  u16_t plen, base;
  u8_t *msg_ptr;
  s32_t *oid_ptr;

  plen = 0;
  while (p != NULL)
  {
    base = plen;
    plen += p->len;
    if (ofs < plen)
    {
      msg_ptr = p->payload;
      msg_ptr += ofs - base;

      oid->len = 0;
      oid_ptr = &oid->id[0];
      if (len > 0)
      {
        /* first compressed octet */
        if (*msg_ptr == 0x2B)
        {
          /* (most) common case 1.3 (iso.org) */
          *oid_ptr = 1;
          oid_ptr++;
          *oid_ptr = 3;
          oid_ptr++;
        }
        else if (*msg_ptr < 40)
        {
          *oid_ptr = 0;
          oid_ptr++;
          *oid_ptr = *msg_ptr;
          oid_ptr++;
        }
        else if (*msg_ptr < 80)
        {
          *oid_ptr = 1;
          oid_ptr++;
          *oid_ptr = (*msg_ptr) - 40;
          oid_ptr++;
        }
        else
        {
          *oid_ptr = 2;
          oid_ptr++;
          *oid_ptr = (*msg_ptr) - 80;
          oid_ptr++;
        }
        oid->len = 2;
      }
      else
      {
        /* accepting zero length identifiers e.g. for
           getnext operation. uncommon but valid */
        return ERR_OK;
      }
      len--;
      if (len > 0)
      {
        ofs += 1;
        if (ofs >= plen)
        {
          /* next octet in next pbuf */
          p = p->next;
          if (p == NULL) { return ERR_ARG; }
          msg_ptr = p->payload;
          plen += p->len;
        }
        else
        {
          /* next octet in same pbuf */
          msg_ptr++;
        }
      }
      while ((len > 0) && (oid->len < LWIP_SNMP_OBJ_ID_LEN))
      {
        /* sub-identifier uses multiple octets */
        if (*msg_ptr & 0x80)
        {
          s32_t sub_id = 0;

          while ((*msg_ptr & 0x80) && (len > 1))
          {
            len--;
            sub_id = (sub_id << 7) + (*msg_ptr & ~0x80);
            ofs += 1;
            if (ofs >= plen)
            {
              /* next octet in next pbuf */
              p = p->next;
              if (p == NULL) { return ERR_ARG; }
              msg_ptr = p->payload;
              plen += p->len;
            }
            else
            {
              /* next octet in same pbuf */
              msg_ptr++;
            }
          }
          if (!(*msg_ptr & 0x80) && (len > 0))
          {
            /* last octet sub-identifier */
            len--;
            sub_id = (sub_id << 7) + *msg_ptr;
            *oid_ptr = sub_id;
          }
        }
        else
        {
          /* !(*msg_ptr & 0x80) sub-identifier uses single octet */
          len--;
          *oid_ptr = *msg_ptr;
        }
        if (len > 0)
        {
          /* remaining oid bytes available ... */
          ofs += 1;
          if (ofs >= plen)
          {
            /* next octet in next pbuf */
            p = p->next;
            if (p == NULL) { return ERR_ARG; }
            msg_ptr = p->payload;
            plen += p->len;
          }
          else
          {
            /* next octet in same pbuf */
            msg_ptr++;
          }
        }
        oid_ptr++;
        oid->len++;
      }
      if (len == 0)
      {
        /* len == 0, end of oid */
        return ERR_OK;
      }
      else
      {
        /* len > 0, oid->len == LWIP_SNMP_OBJ_ID_LEN or malformed encoding */
        return ERR_ARG;
      }

    }
    p = p->next;
  }
  /* p == NULL, ofs >= plen */
  return ERR_ARG;
}

/**
 * Decodes (copies) raw data (ip-addresses, octet strings, opaque encoding)
 * from incoming message into array.
 *
 * @param p points to a pbuf holding an ASN1 coded raw data
 * @param ofs points to the offset within the pbuf chain of the ASN1 coded raw data
 * @param len length of the coded raw data (zero is valid, e.g. empty string!)
 * @param raw_len length of the raw return value
 * @param raw return raw bytes
 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
 */
err_t
snmp_asn1_dec_raw(struct pbuf *p, u16_t ofs, u16_t len, u16_t raw_len, u8_t *raw)
{
  u16_t plen, base;
  u8_t *msg_ptr;

  if (len > 0)
  {
    plen = 0;
    while (p != NULL)
    {
      base = plen;
      plen += p->len;
      if (ofs < plen)
      {
        msg_ptr = p->payload;
        msg_ptr += ofs - base;
        if (raw_len >= len)
        {
          while (len > 1)
          {
            /* copy len - 1 octets */
            len--;
            *raw = *msg_ptr;
            raw++;
            ofs += 1;
            if (ofs >= plen)
            {
              /* next octet in next pbuf */
              p = p->next;
              if (p == NULL) { return ERR_ARG; }
              msg_ptr = p->payload;
              plen += p->len;
            }
            else
            {
              /* next octet in same pbuf */
              msg_ptr++;
            }
          }
          /* copy last octet */
          *raw = *msg_ptr;
          return ERR_OK;
        }
        else
        {
          /* raw_len < len, not enough dst space */
          return ERR_ARG;
        }
      }
      p = p->next;
    }
    /* p == NULL, ofs >= plen */
    return ERR_ARG;
  }
  else
  {
    /* len == 0, empty string */
    return ERR_OK;
  }
}

#endif /* LWIP_SNMP */