added support for lwip

lwip/core/exclude/snmp/asn1_dec.c

@@ -0,0 +1,653 @@
+/**
+ * @file
+ * Abstract Syntax Notation One (ISO 8824, 8825) decoding
+ *
+ * @todo not optimised (yet), favor correctness over speed, favor speed over size
+ */
+#ifdef EXCLUDE
+/*
+ * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * Author: Christiaan Simons <christiaan.simons@axon.tv>
+ */
+
+#include "lwip/opt.h"
+
+#if LWIP_SNMP
+#include "lwip/snmp_asn1.h"
+
+/**
+ * Retrieves type field from incoming pbuf chain.
+ *
+ * @param p points to a pbuf holding an ASN1 coded type field
+ * @param ofs points to the offset within the pbuf chain of the ASN1 coded type field
+ * @param type return ASN1 type
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
+ */
+err_t
+snmp_asn1_dec_type(struct pbuf *p, u16_t ofs, u8_t *type)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+      *type = *msg_ptr;
+      return ERR_OK;
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Decodes length field from incoming pbuf chain into host length.
+ *
+ * @param p points to a pbuf holding an ASN1 coded length
+ * @param ofs points to the offset within the pbuf chain of the ASN1 coded length
+ * @param octets_used returns number of octets used by the length code
+ * @param length return host order length, upto 64k
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
+ */
+err_t
+snmp_asn1_dec_length(struct pbuf *p, u16_t ofs, u8_t *octets_used, u16_t *length)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+
+      if (*msg_ptr < 0x80)
+      {
+        /* primitive definite length format */
+        *octets_used = 1;
+        *length = *msg_ptr;
+        return ERR_OK;
+      }
+      else if (*msg_ptr == 0x80)
+      {
+        /* constructed indefinite length format, termination with two zero octets */
+        u8_t zeros;
+        u8_t i;
+
+        *length = 0;
+        zeros = 0;
+        while (zeros != 2)
+        {
+          i = 2;
+          while (i > 0)
+          {
+            i--;
+            (*length) += 1;
+            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 == 0)
+            {
+              zeros++;
+              if (zeros == 2)
+              {
+                /* stop while (i > 0) */
+                i = 0;
+              }
+            }
+            else
+            {
+              zeros = 0;
+            }
+          }
+        }
+        *octets_used = 1;
+        return ERR_OK;
+      }
+      else if (*msg_ptr == 0x81)
+      {
+        /* constructed definite length format, one octet */
+        ofs += 1;
+        if (ofs >= plen)
+        {
+          /* next octet in next pbuf */
+          p = p->next;
+          if (p == NULL) { return ERR_ARG; }
+          msg_ptr = p->payload;
+        }
+        else
+        {
+          /* next octet in same pbuf */
+          msg_ptr++;
+        }
+        *length = *msg_ptr;
+        *octets_used = 2;
+        return ERR_OK;
+      }
+      else if (*msg_ptr == 0x82)
+      {
+        u8_t i;
+
+        /* constructed definite length format, two octets */
+        i = 2;
+        while (i > 0)
+        {
+          i--;
+          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 (i == 0)
+          {
+            /* least significant length octet */
+            *length |= *msg_ptr;
+          }
+          else
+          {
+            /* most significant length octet */
+            *length = (*msg_ptr) << 8;
+          }
+        }
+        *octets_used = 3;
+        return ERR_OK;
+      }
+      else
+      {
+        /* constructed definite length format 3..127 octets, this is too big (>64k) */
+        /**  @todo: do we need to accept inefficient codings with many leading zero's? */
+        *octets_used = 1 + ((*msg_ptr) & 0x7f);
+        return ERR_ARG;
+      }
+    }
+    p = p->next;
+  }
+
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Decodes positive integer (counter, gauge, timeticks) into u32_t.
+ *
+ * @param p points to a pbuf holding an ASN1 coded integer
+ * @param ofs points to the offset within the pbuf chain of the ASN1 coded integer
+ * @param len length of the coded integer field
+ * @param value return host order integer
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
+ *
+ * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
+ * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
+ * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
+ */
+err_t
+snmp_asn1_dec_u32t(struct pbuf *p, u16_t ofs, u16_t len, u32_t *value)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  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 < 6))
+      {
+        /* start from zero */
+        *value = 0;
+        if (*msg_ptr & 0x80)
+        {
+          /* negative, expecting zero sign bit! */
+          return ERR_ARG;
+        }
+        else
+        {
+          /* positive */
+          if ((len > 1) && (*msg_ptr == 0))
+          {
+            /* skip leading "sign byte" octet 0x00 */
+            len--;
+            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++;
+            }
+          }
+        }
+        /* OR octets with value */
+        while (len > 1)
+        {
+          len--;
+          *value |= *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++;
+          }
+        }
+        *value |= *msg_ptr;
+        return ERR_OK;
+      }
+      else
+      {
+        return ERR_ARG;
+      }
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Decodes integer into s32_t.
+ *
+ * @param p points to a pbuf holding an ASN1 coded integer
+ * @param ofs points to the offset within the pbuf chain of the ASN1 coded integer
+ * @param len length of the coded integer field
+ * @param value return host order integer
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
+ *
+ * @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;
+  u8_t *lsb_ptr = (u8_t*)value;
+  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 */
+
+#endif

lwip/core/exclude/snmp/asn1_enc.c

@@ -0,0 +1,612 @@
+/**
+ * @file
+ * Abstract Syntax Notation One (ISO 8824, 8825) encoding
+ *
+ * @todo not optimised (yet), favor correctness over speed, favor speed over size
+ */
+#ifdef EXCLUDE
+/*
+ * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * Author: Christiaan Simons <christiaan.simons@axon.tv>
+ */
+
+#include "lwip/opt.h"
+
+#if LWIP_SNMP
+#include "lwip/snmp_asn1.h"
+
+/**
+ * Returns octet count for length.
+ *
+ * @param length
+ * @param octets_needed points to the return value
+ */
+void
+snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed)
+{
+  if (length < 0x80U)
+  {
+    *octets_needed = 1;
+  }
+  else if (length < 0x100U)
+  {
+    *octets_needed = 2;
+  }
+  else
+  {
+    *octets_needed = 3;
+  }
+}
+
+/**
+ * Returns octet count for an u32_t.
+ *
+ * @param value
+ * @param octets_needed points to the return value
+ *
+ * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
+ * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
+ * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
+ */
+void
+snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed)
+{
+  if (value < 0x80UL)
+  {
+    *octets_needed = 1;
+  }
+  else if (value < 0x8000UL)
+  {
+    *octets_needed = 2;
+  }
+  else if (value < 0x800000UL)
+  {
+    *octets_needed = 3;
+  }
+  else if (value < 0x80000000UL)
+  {
+    *octets_needed = 4;
+  }
+  else
+  {
+    *octets_needed = 5;
+  }
+}
+
+/**
+ * Returns octet count for an s32_t.
+ *
+ * @param value
+ * @param octets_needed points to the return value
+ *
+ * @note ASN coded integers are _always_ signed.
+ */
+void
+snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed)
+{
+  if (value < 0)
+  {
+    value = ~value;
+  }
+  if (value < 0x80L)
+  {
+    *octets_needed = 1;
+  }
+  else if (value < 0x8000L)
+  {
+    *octets_needed = 2;
+  }
+  else if (value < 0x800000L)
+  {
+    *octets_needed = 3;
+  }
+  else
+  {
+    *octets_needed = 4;
+  }
+}
+
+/**
+ * Returns octet count for an object identifier.
+ *
+ * @param ident_len object identifier array length
+ * @param ident points to object identifier array
+ * @param octets_needed points to the return value
+ */
+void
+snmp_asn1_enc_oid_cnt(u8_t ident_len, s32_t *ident, u16_t *octets_needed)
+{
+  s32_t sub_id;
+  u8_t cnt;
+
+  cnt = 0;
+  if (ident_len > 1)
+  {
+    /* compressed prefix in one octet */
+    cnt++;
+    ident_len -= 2;
+    ident += 2;
+  }
+  while(ident_len > 0)
+  {
+    ident_len--;
+    sub_id = *ident;
+
+    sub_id >>= 7;
+    cnt++;
+    while(sub_id > 0)
+    {
+      sub_id >>= 7;
+      cnt++;
+    }
+    ident++;
+  }
+  *octets_needed = cnt;
+}
+
+/**
+ * Encodes ASN type field into a pbuf chained ASN1 msg.
+ *
+ * @param p points to output pbuf to encode value into
+ * @param ofs points to the offset within the pbuf chain
+ * @param type input ASN1 type
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode
+ */
+err_t
+snmp_asn1_enc_type(struct pbuf *p, u16_t ofs, u8_t type)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+      *msg_ptr = type;
+      return ERR_OK;
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Encodes host order length field into a pbuf chained ASN1 msg.
+ *
+ * @param p points to output pbuf to encode length into
+ * @param ofs points to the offset within the pbuf chain
+ * @param length is the host order length to be encoded
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode
+ */
+err_t
+snmp_asn1_enc_length(struct pbuf *p, u16_t ofs, u16_t length)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+
+      if (length < 0x80)
+      {
+        *msg_ptr = length;
+        return ERR_OK;
+      }
+      else if (length < 0x100)
+      {
+        *msg_ptr = 0x81;
+        ofs += 1;
+        if (ofs >= plen)
+        {
+          /* next octet in next pbuf */
+          p = p->next;
+          if (p == NULL) { return ERR_ARG; }
+          msg_ptr = p->payload;
+        }
+        else
+        {
+          /* next octet in same pbuf */
+          msg_ptr++;
+        }
+        *msg_ptr = length;
+        return ERR_OK;
+      }
+      else
+      {
+        u8_t i;
+
+        /* length >= 0x100 && length <= 0xFFFF */
+        *msg_ptr = 0x82;
+        i = 2;
+        while (i > 0)
+        {
+          i--;
+          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 (i == 0)
+          {
+            /* least significant length octet */
+            *msg_ptr = length;
+          }
+          else
+          {
+            /* most significant length octet */
+            *msg_ptr = length >> 8;
+          }
+        }
+        return ERR_OK;
+      }
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Encodes u32_t (counter, gauge, timeticks) into a pbuf chained ASN1 msg.
+ *
+ * @param p points to output pbuf to encode value into
+ * @param ofs points to the offset within the pbuf chain
+ * @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt())
+ * @param value is the host order u32_t value to be encoded
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode
+ *
+ * @see snmp_asn1_enc_u32t_cnt()
+ */
+err_t
+snmp_asn1_enc_u32t(struct pbuf *p, u16_t ofs, u8_t octets_needed, u32_t value)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+
+      if (octets_needed == 5)
+      {
+        /* not enough bits in 'value' add leading 0x00 */
+        octets_needed--;
+        *msg_ptr = 0x00;
+        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 (octets_needed > 1)
+      {
+        octets_needed--;
+        *msg_ptr = value >> (octets_needed << 3);
+        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++;
+        }
+      }
+      /* (only) one least significant octet */
+      *msg_ptr = value;
+      return ERR_OK;
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Encodes s32_t integer into a pbuf chained ASN1 msg.
+ *
+ * @param p points to output pbuf to encode value into
+ * @param ofs points to the offset within the pbuf chain
+ * @param octets_needed encoding length (from snmp_asn1_enc_s32t_cnt())
+ * @param value is the host order s32_t value to be encoded
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode
+ *
+ * @see snmp_asn1_enc_s32t_cnt()
+ */
+err_t
+snmp_asn1_enc_s32t(struct pbuf *p, u16_t ofs, u8_t octets_needed, s32_t value)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+
+      while (octets_needed > 1)
+      {
+        octets_needed--;
+        *msg_ptr = value >> (octets_needed << 3);
+        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++;
+        }
+      }
+      /* (only) one least significant octet */
+      *msg_ptr = value;
+      return ERR_OK;
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Encodes object identifier into a pbuf chained ASN1 msg.
+ *
+ * @param p points to output pbuf to encode oid into
+ * @param ofs points to the offset within the pbuf chain
+ * @param ident_len object identifier array length
+ * @param ident points to object identifier array
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode
+ */
+err_t
+snmp_asn1_enc_oid(struct pbuf *p, u16_t ofs, u8_t ident_len, s32_t *ident)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+
+      if (ident_len > 1)
+      {
+        if ((ident[0] == 1) && (ident[1] == 3))
+        {
+          /* compressed (most common) prefix .iso.org */
+          *msg_ptr = 0x2b;
+        }
+        else
+        {
+          /* calculate prefix */
+          *msg_ptr = (ident[0] * 40) + ident[1];
+        }
+        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++;
+        }
+        ident_len -= 2;
+        ident += 2;
+      }
+      else
+      {
+/* @bug:  allow empty varbinds for symmetry (we must decode them for getnext), allow partial compression??  */
+        /* ident_len <= 1, at least we need zeroDotZero (0.0) (ident_len == 2) */
+        return ERR_ARG;
+      }
+      while (ident_len > 0)
+      {
+        s32_t sub_id;
+        u8_t shift, tail;
+
+        ident_len--;
+        sub_id = *ident;
+        tail = 0;
+        shift = 28;
+        while(shift > 0)
+        {
+          u8_t code;
+
+          code = sub_id >> shift;
+          if ((code != 0) || (tail != 0))
+          {
+            tail = 1;
+            *msg_ptr = code | 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++;
+            }
+          }
+          shift -= 7;
+        }
+        *msg_ptr = (u8_t)sub_id & 0x7F;
+        if (ident_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++;
+          }
+        }
+        /* proceed to next sub-identifier */
+        ident++;
+      }
+      return ERR_OK;
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+/**
+ * Encodes raw data (octet string, opaque) into a pbuf chained ASN1 msg.
+ *
+ * @param p points to output pbuf to encode raw data into
+ * @param ofs points to the offset within the pbuf chain
+ * @param raw_len raw data length
+ * @param raw points raw data
+ * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode
+ */
+err_t
+snmp_asn1_enc_raw(struct pbuf *p, u16_t ofs, u8_t raw_len, u8_t *raw)
+{
+  u16_t plen, base;
+  u8_t *msg_ptr;
+
+  plen = 0;
+  while (p != NULL)
+  {
+    base = plen;
+    plen += p->len;
+    if (ofs < plen)
+    {
+      msg_ptr = p->payload;
+      msg_ptr += ofs - base;
+
+      while (raw_len > 1)
+      {
+        /* copy raw_len - 1 octets */
+        raw_len--;
+        *msg_ptr = *raw;
+        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++;
+        }
+      }
+      if (raw_len > 0)
+      {
+        /* copy last or single octet */
+        *msg_ptr = *raw;
+      }
+      return ERR_OK;
+    }
+    p = p->next;
+  }
+  /* p == NULL, ofs >= plen */
+  return ERR_ARG;
+}
+
+#endif /* LWIP_SNMP */
+
+#endif

lwip/core/exclude/snmp/msg_out.c

@@ -0,0 +1,691 @@
+/**
+ * @file
+ * SNMP output message processing (RFC1157).
+ *
+ * Output responses and traps are build in two passes:
+ *
+ * Pass 0: iterate over the output message backwards to determine encoding lengths
+ * Pass 1: the actual forward encoding of internal form into ASN1
+ *
+ * The single-pass encoding method described by Comer & Stevens
+ * requires extra buffer space and copying for reversal of the packet.
+ * The buffer requirement can be prohibitively large for big payloads
+ * (>= 484) therefore we use the two encoding passes.
+ */
+
+#ifdef EXCLUDE
+
+/*
+ * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * Author: Christiaan Simons <christiaan.simons@axon.tv>
+ */
+
+#include "lwip/opt.h"
+
+#if LWIP_SNMP
+#include "arch/cc.h"
+#include "lwip/udp.h"
+#include "lwip/netif.h"
+
+#include "lwip/snmp.h"
+#include "lwip/snmp_asn1.h"
+#include "lwip/snmp_msg.h"
+
+struct snmp_trap_dst
+{
+  /* destination IP address in network order */
+  struct ip_addr dip;
+  /* set to 0 when disabled, >0 when enabled */
+  u8_t enable;
+};
+#if (SNMP_TRAP_DESTINATIONS == 0)
+#error "need at least one trap destination"
+#endif
+struct snmp_trap_dst trap_dst[SNMP_TRAP_DESTINATIONS];
+
+/** TRAP message structure */
+struct snmp_msg_trap trap_msg;
+
+static u16_t snmp_resp_header_sum(struct snmp_msg_pstat *m_stat, u16_t vb_len);
+static u16_t snmp_trap_header_sum(struct snmp_msg_trap *m_trap, u16_t vb_len);
+static u16_t snmp_varbind_list_sum(struct snmp_varbind_root *root);
+
+static u16_t snmp_resp_header_enc(struct snmp_msg_pstat *m_stat, struct pbuf *p);
+static u16_t snmp_trap_header_enc(struct snmp_msg_trap *m_trap, struct pbuf *p);
+static u16_t snmp_varbind_list_enc(struct snmp_varbind_root *root, struct pbuf *p, u16_t ofs);
+
+/**
+ * Sets enable switch for this trap destination.
+ * @param dst_idx index in 0 .. SNMP_TRAP_DESTINATIONS-1
+ * @param enable switch if 0 destination is disabled >0 enabled.
+ */
+void
+snmp_trap_dst_enable(u8_t dst_idx, u8_t enable)
+{
+  if (dst_idx < SNMP_TRAP_DESTINATIONS)
+  {
+    trap_dst[dst_idx].enable = enable;
+  }
+}
+
+/**
+ * Sets IPv4 address for this trap destination.
+ * @param dst_idx index in 0 .. SNMP_TRAP_DESTINATIONS-1
+ * @param dst IPv4 address in host order.
+ */
+void
+snmp_trap_dst_ip_set(u8_t dst_idx, struct ip_addr *dst)
+{
+  if (dst_idx < SNMP_TRAP_DESTINATIONS)
+  {
+    trap_dst[dst_idx].dip.addr = htonl(dst->addr);
+  }
+}
+
+/**
+ * Sends a 'getresponse' message to the request originator.
+ *
+ * @param m_stat points to the current message request state source
+ * @return ERR_OK when success, ERR_MEM if we're out of memory
+ *
+ * @note the caller is responsible for filling in outvb in the m_stat
+ * and provide error-status and index (except for tooBig errors) ...
+ */
+err_t
+snmp_send_response(struct snmp_msg_pstat *m_stat)
+{
+  struct snmp_varbind_root emptyvb = {NULL, NULL, 0, 0, 0};
+  struct pbuf *p;
+  u16_t tot_len;
+  err_t err;
+
+  /* pass 0, calculate length fields */
+  tot_len = snmp_varbind_list_sum(&m_stat->outvb);
+  tot_len = snmp_resp_header_sum(m_stat, tot_len);
+
+  /* try allocating pbuf(s) for complete response */
+  p = pbuf_alloc(PBUF_TRANSPORT, tot_len, PBUF_POOL);
+  if (p == NULL)
+  {
+    LWIP_DEBUGF(SNMP_MSG_DEBUG, ("snmp_snd_response() tooBig\n"));
+
+    /* can't construct reply, return error-status tooBig */
+    m_stat->error_status = SNMP_ES_TOOBIG;
+    m_stat->error_index = 0;
+    /* pass 0, recalculate lengths, for empty varbind-list */
+    tot_len = snmp_varbind_list_sum(&emptyvb);
+    tot_len = snmp_resp_header_sum(m_stat, tot_len);
+    /* retry allocation once for header and empty varbind-list */
+    p = pbuf_alloc(PBUF_TRANSPORT, tot_len, PBUF_POOL);
+  }
+  if (p != NULL)
+  {
+    /* first pbuf alloc try or retry alloc success */
+    u16_t ofs;
+
+    LWIP_DEBUGF(SNMP_MSG_DEBUG, ("snmp_snd_response() p != NULL\n"));
+
+    /* pass 1, size error, encode packet ino the pbuf(s) */
+    ofs = snmp_resp_header_enc(m_stat, p);
+    if (m_stat->error_status == SNMP_ES_TOOBIG)
+    {
+      snmp_varbind_list_enc(&emptyvb, p, ofs);
+    }
+    else
+    {
+      snmp_varbind_list_enc(&m_stat->outvb, p, ofs);
+    }
+
+    switch (m_stat->error_status)
+    {
+      case SNMP_ES_TOOBIG:
+        snmp_inc_snmpouttoobigs();
+        break;
+      case SNMP_ES_NOSUCHNAME:
+        snmp_inc_snmpoutnosuchnames();
+        break;
+      case SNMP_ES_BADVALUE:
+        snmp_inc_snmpoutbadvalues();
+        break;
+      case SNMP_ES_GENERROR:
+        snmp_inc_snmpoutgenerrs();
+        break;
+    }
+    snmp_inc_snmpoutgetresponses();
+    snmp_inc_snmpoutpkts();
+
+    /** @todo do we need separate rx and tx pcbs for threaded case? */
+    /** connect to the originating source */
+    udp_connect(m_stat->pcb, &m_stat->sip, m_stat->sp);
+    err = udp_send(m_stat->pcb, p);
+    if (err == ERR_MEM)
+    {
+      /** @todo release some memory, retry and return tooBig? tooMuchHassle? */
+      err = ERR_MEM;
+    }
+    else
+    {
+      err = ERR_OK;
+    }
+    /** disassociate remote address and port with this pcb */
+    udp_disconnect(m_stat->pcb);
+
+    pbuf_free(p);
+    LWIP_DEBUGF(SNMP_MSG_DEBUG, ("snmp_snd_response() done\n"));
+    return err;
+  }
+  else
+  {
+    /* first pbuf alloc try or retry alloc failed
+       very low on memory, couldn't return tooBig */
+    return ERR_MEM;
+  }
+}
+
+
+/**
+ * Sends an generic or enterprise specific trap message.
+ *
+ * @param generic_trap is the trap code
+ * @param eoid points to enterprise object identifier
+ * @param specific_trap used for enterprise traps when generic_trap == 6
+ * @return ERR_OK when success, ERR_MEM if we're out of memory
+ *
+ * @note the caller is responsible for filling in outvb in the trap_msg
+ * @note the use of the enterpise identifier field
+ * is per RFC1215.
+ * Use .iso.org.dod.internet.mgmt.mib-2.snmp for generic traps
+ * and .iso.org.dod.internet.private.enterprises.yourenterprise
+ * (sysObjectID) for specific traps.
+ */
+err_t
+snmp_send_trap(s8_t generic_trap, struct snmp_obj_id *eoid, s32_t specific_trap)
+{
+  struct snmp_trap_dst *td;
+  struct netif *dst_if;
+  struct ip_addr dst_ip;
+  struct pbuf *p;
+  u16_t i,tot_len;
+
+  for (i=0, td = &trap_dst[0]; i<SNMP_TRAP_DESTINATIONS; i++, td++)
+  {
+    if ((td->enable != 0) && (td->dip.addr != 0))
+    {
+      /* network order trap destination */
+      trap_msg.dip.addr = td->dip.addr;
+      /* lookup current source address for this dst */
+      dst_if = ip_route(&td->dip);
+      dst_ip.addr = ntohl(dst_if->ip_addr.addr);
+      trap_msg.sip_raw[0] = dst_ip.addr >> 24;
+      trap_msg.sip_raw[1] = dst_ip.addr >> 16;
+      trap_msg.sip_raw[2] = dst_ip.addr >> 8;
+      trap_msg.sip_raw[3] = dst_ip.addr;
+      trap_msg.gen_trap = generic_trap;
+      trap_msg.spc_trap = specific_trap;
+      if (generic_trap == SNMP_GENTRAP_ENTERPRISESPC)
+      {
+        /* enterprise-Specific trap */
+        trap_msg.enterprise = eoid;
+      }
+      else
+      {
+        /* generic (MIB-II) trap */
+        snmp_get_snmpgrpid_ptr(&trap_msg.enterprise);
+      }
+      snmp_get_sysuptime(&trap_msg.ts);
+
+      /* pass 0, calculate length fields */
+      tot_len = snmp_varbind_list_sum(&trap_msg.outvb);
+      tot_len = snmp_trap_header_sum(&trap_msg, tot_len);
+
+      /* allocate pbuf(s) */
+      p = pbuf_alloc(PBUF_TRANSPORT, tot_len, PBUF_POOL);
+      if (p != NULL)
+      {
+        u16_t ofs;
+
+        /* pass 1, encode packet ino the pbuf(s) */
+        ofs = snmp_trap_header_enc(&trap_msg, p);
+        snmp_varbind_list_enc(&trap_msg.outvb, p, ofs);
+
+        snmp_inc_snmpouttraps();
+        snmp_inc_snmpoutpkts();
+
+        /** connect to the TRAP destination */
+        udp_connect(trap_msg.pcb, &trap_msg.dip, SNMP_TRAP_PORT);
+        udp_send(trap_msg.pcb, p);
+        /** disassociate remote address and port with this pcb */
+        udp_disconnect(trap_msg.pcb);
+
+        pbuf_free(p);
+      }
+      else
+      {
+        return ERR_MEM;
+      }
+    }
+  }
+  return ERR_OK;
+}
+
+void
+snmp_coldstart_trap(void)
+{
+  trap_msg.outvb.head = NULL;
+  trap_msg.outvb.tail = NULL;
+  trap_msg.outvb.count = 0;
+  snmp_send_trap(SNMP_GENTRAP_COLDSTART, NULL, 0);
+}
+
+void
+snmp_authfail_trap(void)
+{
+  u8_t enable;
+  snmp_get_snmpenableauthentraps(&enable);
+  if (enable == 1)
+  {
+    trap_msg.outvb.head = NULL;
+    trap_msg.outvb.tail = NULL;
+    trap_msg.outvb.count = 0;
+    snmp_send_trap(SNMP_GENTRAP_AUTHFAIL, NULL, 0);
+  }
+}
+
+/**
+ * Sums response header field lengths from tail to head and
+ * returns resp_header_lengths for second encoding pass.
+ *
+ * @param vb_len varbind-list length
+ * @param rhl points to returned header lengths
+ * @return the required lenght for encoding the response header
+ */
+static u16_t
+snmp_resp_header_sum(struct snmp_msg_pstat *m_stat, u16_t vb_len)
+{
+  u16_t tot_len;
+  struct snmp_resp_header_lengths *rhl;
+
+  rhl = &m_stat->rhl;
+  tot_len = vb_len;
+  snmp_asn1_enc_s32t_cnt(m_stat->error_index, &rhl->erridxlen);
+  snmp_asn1_enc_length_cnt(rhl->erridxlen, &rhl->erridxlenlen);
+  tot_len += 1 + rhl->erridxlenlen + rhl->erridxlen;
+
+  snmp_asn1_enc_s32t_cnt(m_stat->error_status, &rhl->errstatlen);
+  snmp_asn1_enc_length_cnt(rhl->errstatlen, &rhl->errstatlenlen);
+  tot_len += 1 + rhl->errstatlenlen + rhl->errstatlen;
+
+  snmp_asn1_enc_s32t_cnt(m_stat->rid, &rhl->ridlen);
+  snmp_asn1_enc_length_cnt(rhl->ridlen, &rhl->ridlenlen);
+  tot_len += 1 + rhl->ridlenlen + rhl->ridlen;
+
+  rhl->pdulen = tot_len;
+  snmp_asn1_enc_length_cnt(rhl->pdulen, &rhl->pdulenlen);
+  tot_len += 1 + rhl->pdulenlen;
+
+  rhl->comlen = m_stat->com_strlen;
+  snmp_asn1_enc_length_cnt(rhl->comlen, &rhl->comlenlen);
+  tot_len += 1 + rhl->comlenlen + rhl->comlen;
+
+  snmp_asn1_enc_s32t_cnt(snmp_version, &rhl->verlen);
+  snmp_asn1_enc_length_cnt(rhl->verlen, &rhl->verlenlen);
+  tot_len += 1 + rhl->verlen + rhl->verlenlen;
+
+  rhl->seqlen = tot_len;
+  snmp_asn1_enc_length_cnt(rhl->seqlen, &rhl->seqlenlen);
+  tot_len += 1 + rhl->seqlenlen;
+
+  return tot_len;
+}
+
+/**
+ * Sums trap header field lengths from tail to head and
+ * returns trap_header_lengths for second encoding pass.
+ *
+ * @param vb_len varbind-list length
+ * @param thl points to returned header lengths
+ * @return the required lenght for encoding the trap header
+ */
+static u16_t
+snmp_trap_header_sum(struct snmp_msg_trap *m_trap, u16_t vb_len)
+{
+  u16_t tot_len;
+  struct snmp_trap_header_lengths *thl;
+
+  thl = &m_trap->thl;
+  tot_len = vb_len;
+
+  snmp_asn1_enc_u32t_cnt(m_trap->ts, &thl->tslen);
+  snmp_asn1_enc_length_cnt(thl->tslen, &thl->tslenlen);
+  tot_len += 1 + thl->tslen + thl->tslenlen;
+
+  snmp_asn1_enc_s32t_cnt(m_trap->spc_trap, &thl->strplen);
+  snmp_asn1_enc_length_cnt(thl->strplen, &thl->strplenlen);
+  tot_len += 1 + thl->strplen + thl->strplenlen;
+
+  snmp_asn1_enc_s32t_cnt(m_trap->gen_trap, &thl->gtrplen);
+  snmp_asn1_enc_length_cnt(thl->gtrplen, &thl->gtrplenlen);
+  tot_len += 1 + thl->gtrplen + thl->gtrplenlen;
+
+  thl->aaddrlen = 4;
+  snmp_asn1_enc_length_cnt(thl->aaddrlen, &thl->aaddrlenlen);
+  tot_len += 1 + thl->aaddrlen + thl->aaddrlenlen;
+
+  snmp_asn1_enc_oid_cnt(m_trap->enterprise->len, &m_trap->enterprise->id[0], &thl->eidlen);
+  snmp_asn1_enc_length_cnt(thl->eidlen, &thl->eidlenlen);
+  tot_len += 1 + thl->eidlen + thl->eidlenlen;
+
+  thl->pdulen = tot_len;
+  snmp_asn1_enc_length_cnt(thl->pdulen, &thl->pdulenlen);
+  tot_len += 1 + thl->pdulenlen;
+
+  thl->comlen = sizeof(snmp_publiccommunity) - 1;
+  snmp_asn1_enc_length_cnt(thl->comlen, &thl->comlenlen);
+  tot_len += 1 + thl->comlenlen + thl->comlen;
+
+  snmp_asn1_enc_s32t_cnt(snmp_version, &thl->verlen);
+  snmp_asn1_enc_length_cnt(thl->verlen, &thl->verlenlen);
+  tot_len += 1 + thl->verlen + thl->verlenlen;
+
+  thl->seqlen = tot_len;
+  snmp_asn1_enc_length_cnt(thl->seqlen, &thl->seqlenlen);
+  tot_len += 1 + thl->seqlenlen;
+
+  return tot_len;
+}
+
+/**
+ * Sums varbind lengths from tail to head and
+ * annotates lengths in varbind for second encoding pass.
+ *
+ * @param root points to the root of the variable binding list
+ * @return the required lenght for encoding the variable bindings
+ */
+static u16_t
+snmp_varbind_list_sum(struct snmp_varbind_root *root)
+{
+  struct snmp_varbind *vb;
+  u32_t *uint_ptr;
+  s32_t *sint_ptr;
+  u16_t tot_len;
+
+  tot_len = 0;
+  vb = root->tail;
+  while ( vb != NULL )
+  {
+    /* encoded value lenght depends on type */
+    switch (vb->value_type)
+    {
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG):
+        sint_ptr = vb->value;
+        snmp_asn1_enc_s32t_cnt(*sint_ptr, &vb->vlen);
+        break;
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_COUNTER):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_GAUGE):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_TIMETICKS):
+        uint_ptr = vb->value;
+        snmp_asn1_enc_u32t_cnt(*uint_ptr, &vb->vlen);
+        break;
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OC_STR):
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_NUL):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_IPADDR):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_OPAQUE):
+        vb->vlen = vb->value_len;
+        break;
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OBJ_ID):
+        sint_ptr = vb->value;
+        snmp_asn1_enc_oid_cnt(vb->value_len / sizeof(s32_t), sint_ptr, &vb->vlen);
+        break;
+      default:
+        /* unsupported type */
+        vb->vlen = 0;
+        break;
+    };
+    /* encoding length of value length field */
+    snmp_asn1_enc_length_cnt(vb->vlen, &vb->vlenlen);
+    snmp_asn1_enc_oid_cnt(vb->ident_len, vb->ident, &vb->olen);
+    snmp_asn1_enc_length_cnt(vb->olen, &vb->olenlen);
+
+    vb->seqlen = 1 + vb->vlenlen + vb->vlen;
+    vb->seqlen += 1 + vb->olenlen + vb->olen;
+    snmp_asn1_enc_length_cnt(vb->seqlen, &vb->seqlenlen);
+
+    /* varbind seq */
+    tot_len += 1 + vb->seqlenlen + vb->seqlen;
+
+    vb = vb->prev;
+  }
+
+  /* varbind-list seq */
+  root->seqlen = tot_len;
+  snmp_asn1_enc_length_cnt(root->seqlen, &root->seqlenlen);
+  tot_len += 1 + root->seqlenlen;
+
+  return tot_len;
+}
+
+/**
+ * Encodes response header from head to tail.
+ */
+static u16_t
+snmp_resp_header_enc(struct snmp_msg_pstat *m_stat, struct pbuf *p)
+{
+  u16_t ofs;
+
+  ofs = 0;
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_CONSTR | SNMP_ASN1_SEQ));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.seqlen);
+  ofs += m_stat->rhl.seqlenlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.verlen);
+  ofs += m_stat->rhl.verlenlen;
+  snmp_asn1_enc_s32t(p, ofs, m_stat->rhl.verlen, snmp_version);
+  ofs += m_stat->rhl.verlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OC_STR));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.comlen);
+  ofs += m_stat->rhl.comlenlen;
+  snmp_asn1_enc_raw(p, ofs, m_stat->rhl.comlen, m_stat->community);
+  ofs += m_stat->rhl.comlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_CONTXT | SNMP_ASN1_CONSTR | SNMP_ASN1_PDU_GET_RESP));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.pdulen);
+  ofs += m_stat->rhl.pdulenlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.ridlen);
+  ofs += m_stat->rhl.ridlenlen;
+  snmp_asn1_enc_s32t(p, ofs, m_stat->rhl.ridlen, m_stat->rid);
+  ofs += m_stat->rhl.ridlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.errstatlen);
+  ofs += m_stat->rhl.errstatlenlen;
+  snmp_asn1_enc_s32t(p, ofs, m_stat->rhl.errstatlen, m_stat->error_status);
+  ofs += m_stat->rhl.errstatlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_stat->rhl.erridxlen);
+  ofs += m_stat->rhl.erridxlenlen;
+  snmp_asn1_enc_s32t(p, ofs, m_stat->rhl.erridxlen, m_stat->error_index);
+  ofs += m_stat->rhl.erridxlen;
+
+  return ofs;
+}
+
+/**
+ * Encodes trap header from head to tail.
+ */
+static u16_t
+snmp_trap_header_enc(struct snmp_msg_trap *m_trap, struct pbuf *p)
+{
+  u16_t ofs;
+
+  ofs = 0;
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_CONSTR | SNMP_ASN1_SEQ));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.seqlen);
+  ofs += m_trap->thl.seqlenlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.verlen);
+  ofs += m_trap->thl.verlenlen;
+  snmp_asn1_enc_s32t(p, ofs, m_trap->thl.verlen, snmp_version);
+  ofs += m_trap->thl.verlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OC_STR));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.comlen);
+  ofs += m_trap->thl.comlenlen;
+  snmp_asn1_enc_raw(p, ofs, m_trap->thl.comlen, (u8_t *)&snmp_publiccommunity[0]);
+  ofs += m_trap->thl.comlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_CONTXT | SNMP_ASN1_CONSTR | SNMP_ASN1_PDU_TRAP));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.pdulen);
+  ofs += m_trap->thl.pdulenlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OBJ_ID));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.eidlen);
+  ofs += m_trap->thl.eidlenlen;
+  snmp_asn1_enc_oid(p, ofs, m_trap->enterprise->len, &m_trap->enterprise->id[0]);
+  ofs += m_trap->thl.eidlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_IPADDR));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.aaddrlen);
+  ofs += m_trap->thl.aaddrlenlen;
+  snmp_asn1_enc_raw(p, ofs, m_trap->thl.aaddrlen, &m_trap->sip_raw[0]);
+  ofs += m_trap->thl.aaddrlen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.gtrplen);
+  ofs += m_trap->thl.gtrplenlen;
+  snmp_asn1_enc_u32t(p, ofs, m_trap->thl.gtrplen, m_trap->gen_trap);
+  ofs += m_trap->thl.gtrplen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.strplen);
+  ofs += m_trap->thl.strplenlen;
+  snmp_asn1_enc_u32t(p, ofs, m_trap->thl.strplen, m_trap->spc_trap);
+  ofs += m_trap->thl.strplen;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_TIMETICKS));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, m_trap->thl.tslen);
+  ofs += m_trap->thl.tslenlen;
+  snmp_asn1_enc_u32t(p, ofs, m_trap->thl.tslen, m_trap->ts);
+  ofs += m_trap->thl.tslen;
+
+  return ofs;
+}
+
+/**
+ * Encodes varbind list from head to tail.
+ */
+static u16_t
+snmp_varbind_list_enc(struct snmp_varbind_root *root, struct pbuf *p, u16_t ofs)
+{
+  struct snmp_varbind *vb;
+  s32_t *sint_ptr;
+  u32_t *uint_ptr;
+  u8_t *raw_ptr;
+
+  snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_CONSTR | SNMP_ASN1_SEQ));
+  ofs += 1;
+  snmp_asn1_enc_length(p, ofs, root->seqlen);
+  ofs += root->seqlenlen;
+
+  vb = root->head;
+  while ( vb != NULL )
+  {
+    snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_CONSTR | SNMP_ASN1_SEQ));
+    ofs += 1;
+    snmp_asn1_enc_length(p, ofs, vb->seqlen);
+    ofs += vb->seqlenlen;
+
+    snmp_asn1_enc_type(p, ofs, (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OBJ_ID));
+    ofs += 1;
+    snmp_asn1_enc_length(p, ofs, vb->olen);
+    ofs += vb->olenlen;
+    snmp_asn1_enc_oid(p, ofs, vb->ident_len, &vb->ident[0]);
+    ofs += vb->olen;
+
+    snmp_asn1_enc_type(p, ofs, vb->value_type);
+    ofs += 1;
+    snmp_asn1_enc_length(p, ofs, vb->vlen);
+    ofs += vb->vlenlen;
+
+    switch (vb->value_type)
+    {
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_INTEG):
+        sint_ptr = vb->value;
+        snmp_asn1_enc_s32t(p, ofs, vb->vlen, *sint_ptr);
+        break;
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_COUNTER):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_GAUGE):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_TIMETICKS):
+        uint_ptr = vb->value;
+        snmp_asn1_enc_u32t(p, ofs, vb->vlen, *uint_ptr);
+        break;
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OC_STR):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_IPADDR):
+      case (SNMP_ASN1_APPLIC | SNMP_ASN1_PRIMIT | SNMP_ASN1_OPAQUE):
+        raw_ptr = vb->value;
+        snmp_asn1_enc_raw(p, ofs, vb->vlen, raw_ptr);
+        break;
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_NUL):
+        break;
+      case (SNMP_ASN1_UNIV | SNMP_ASN1_PRIMIT | SNMP_ASN1_OBJ_ID):
+        sint_ptr = vb->value;
+        snmp_asn1_enc_oid(p, ofs, vb->value_len / sizeof(s32_t), sint_ptr);
+        break;
+      default:
+        /* unsupported type */
+        break;
+    };
+    ofs += vb->vlen;
+    vb = vb->next;
+  }
+  return ofs;
+}
+
+#endif /* LWIP_SNMP */
+
+#endif

lwip/core/inet.c

@@ -0,0 +1,537 @@
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+
+/* inet.c
+ *
+ * Functions common to all TCP/IP modules, such as the Internet checksum and the
+ * byte order functions.
+ *
+ */
+
+
+#include "lwip/opt.h"
+
+#include "lwip/arch.h"
+
+#include "lwip/def.h"
+#include "lwip/inet.h"
+
+#include "lwip/sys.h"
+
+/* These are some reference implementations of the checksum algorithm, with the
+ * aim of being simple, correct and fully portable. Checksumming is the
+ * first thing you would want to optimize for your platform. If you create
+ * your own version, link it in and in your sys_arch.h put:
+ * 
+ * #define LWIP_CHKSUM <your_checksum_routine> 
+*/
+#ifndef LWIP_CHKSUM
+#define LWIP_CHKSUM lwip_standard_chksum
+
+#if 1 /* Version A */
+/**
+ * lwip checksum
+ *
+ * @param dataptr points to start of data to be summed at any boundary
+ * @param len length of data to be summed
+ * @return host order (!) lwip checksum (non-inverted Internet sum) 
+ *
+ * @note accumulator size limits summable length to 64k
+ * @note host endianess is irrelevant (p3 RFC1071)
+ */
+static u16_t
+lwip_standard_chksum(void *dataptr, u16_t len)
+{
+  u32_t acc;
+  u16_t src;
+  u8_t *octetptr;
+
+  acc = 0;
+  /* dataptr may be at odd or even addresses */
+  octetptr = (u8_t*)dataptr;
+  while (len > 1)
+  {
+    /* declare first octet as most significant
+       thus assume network order, ignoring host order */
+    src = (*octetptr) << 8;
+    octetptr++;
+    /* declare second octet as least significant */
+    src |= (*octetptr);
+    octetptr++;
+    acc += src;
+    len -= 2;
+  }
+  if (len > 0)
+  {
+    /* accumulate remaining octet */
+    src = (*octetptr) << 8;
+    acc += src;
+  }
+  /* add deferred carry bits */
+  acc = (acc >> 16) + (acc & 0x0000ffffUL);
+  if ((acc & 0xffff0000) != 0) {
+    acc = (acc >> 16) + (acc & 0x0000ffffUL);
+  }
+  /* This maybe a little confusing: reorder sum using htons()
+     instead of ntohs() since it has a little less call overhead.
+     The caller must invert bits for Internet sum ! */
+  return htons((u16_t)acc);
+}
+#endif
+
+#if 0 /* Version B */
+/*
+ * Curt McDowell
+ * Broadcom Corp.
+ * csm@broadcom.com
+ *
+ * IP checksum two bytes at a time with support for
+ * unaligned buffer.
+ * Works for len up to and including 0x20000.
+ * by Curt McDowell, Broadcom Corp. 12/08/2005
+ */
+
+static u16_t
+lwip_standard_chksum(void *dataptr, int len)
+{
+  u8_t *pb = dataptr;
+  u16_t *ps, t = 0;
+  u32_t sum = 0;
+  int odd = ((u32_t)pb & 1);
+
+  /* Get aligned to u16_t */
+  if (odd && len > 0) {
+    ((u8_t *)&t)[1] = *pb++;
+    len--;
+  }
+
+  /* Add the bulk of the data */
+  ps = (u16_t *)pb;
+  while (len > 1) {
+    sum += *ps++;
+    len -= 2;
+  }
+
+  /* Consume left-over byte, if any */
+  if (len > 0)
+    ((u8_t *)&t)[0] = *(u8_t *)ps;;
+
+  /* Add end bytes */
+  sum += t;
+
+  /*  Fold 32-bit sum to 16 bits */
+  while (sum >> 16)
+    sum = (sum & 0xffff) + (sum >> 16);
+
+  /* Swap if alignment was odd */
+  if (odd)
+    sum = ((sum & 0xff) << 8) | ((sum & 0xff00) >> 8);
+
+  return sum;
+}
+#endif
+
+#if 0 /* Version C */
+/**
+ * An optimized checksum routine. Basically, it uses loop-unrolling on
+ * the checksum loop, treating the head and tail bytes specially, whereas
+ * the inner loop acts on 8 bytes at a time. 
+ *
+ * @arg start of buffer to be checksummed. May be an odd byte address.
+ * @len number of bytes in the buffer to be checksummed.
+ * 
+ * by Curt McDowell, Broadcom Corp. December 8th, 2005
+ */
+
+static u16_t
+lwip_standard_chksum(void *dataptr, int len)
+{
+  u8_t *pb = dataptr;
+  u16_t *ps, t = 0;
+  u32_t *pl;
+  u32_t sum = 0, tmp;
+  /* starts at odd byte address? */
+  int odd = ((u32_t)pb & 1);
+
+  if (odd && len > 0) {
+    ((u8_t *)&t)[1] = *pb++;
+    len--;
+  }
+
+  ps = (u16_t *)pb;
+
+  if (((u32_t)ps & 3) && len > 1) {
+    sum += *ps++;
+    len -= 2;
+  }
+
+  pl = (u32_t *)ps;
+
+  while (len > 7)  {
+    tmp = sum + *pl++;          /* ping */
+    if (tmp < sum)
+      tmp++;                    /* add back carry */
+
+    sum = tmp + *pl++;          /* pong */
+    if (sum < tmp)
+      sum++;                    /* add back carry */
+
+    len -= 8;
+  }
+
+  /* make room in upper bits */
+  sum = (sum >> 16) + (sum & 0xffff);
+
+  ps = (u16_t *)pl;
+
+  /* 16-bit aligned word remaining? */
+  while (len > 1) {
+    sum += *ps++;
+    len -= 2;
+  }
+
+  /* dangling tail byte remaining? */
+  if (len > 0)                  /* include odd byte */
+    ((u8_t *)&t)[0] = *(u8_t *)ps;
+
+  sum += t;                     /* add end bytes */
+
+  while (sum >> 16)             /* combine halves */
+    sum = (sum >> 16) + (sum & 0xffff);
+
+  if (odd)
+    sum = ((sum & 0xff) << 8) | ((sum & 0xff00) >> 8);
+
+  return sum;
+}
+#endif
+
+#endif /* LWIP_CHKSUM */
+
+/* inet_chksum_pseudo:
+ *
+ * Calculates the pseudo Internet checksum used by TCP and UDP for a pbuf chain.
+ */
+
+u16_t
+inet_chksum_pseudo(struct pbuf *p,
+       struct ip_addr *src, struct ip_addr *dest,
+       u8_t proto, u16_t proto_len)
+{
+  u32_t acc;
+  struct pbuf *q;
+  u8_t swapped;
+
+  acc = 0;
+  swapped = 0;
+  /* iterate through all pbuf in chain */
+  for(q = p; q != NULL; q = q->next) {
+    LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): checksumming pbuf %p (has next %p) \n",
+      (void *)q, (void *)q->next));
+    acc += LWIP_CHKSUM(q->payload, q->len);
+    /*LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): unwrapped lwip_chksum()=%"X32_F" \n", acc));*/
+    while (acc >> 16) {
+      acc = (acc & 0xffffUL) + (acc >> 16);
+    }
+    if (q->len % 2 != 0) {
+      swapped = 1 - swapped;
+      acc = ((acc & 0xff) << 8) | ((acc & 0xff00UL) >> 8);
+    }
+    /*LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): wrapped lwip_chksum()=%"X32_F" \n", acc));*/
+  }
+
+  if (swapped) {
+    acc = ((acc & 0xff) << 8) | ((acc & 0xff00UL) >> 8);
+  }
+  acc += (src->addr & 0xffffUL);
+  acc += ((src->addr >> 16) & 0xffffUL);
+  acc += (dest->addr & 0xffffUL);
+  acc += ((dest->addr >> 16) & 0xffffUL);
+  acc += (u32_t)htons((u16_t)proto);
+  acc += (u32_t)htons(proto_len);
+
+  while (acc >> 16) {
+    acc = (acc & 0xffffUL) + (acc >> 16);
+  }
+  LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): pbuf chain lwip_chksum()=%"X32_F"\n", acc));
+  return (u16_t)~(acc & 0xffffUL);
+}
+
+/* inet_chksum:
+ *
+ * Calculates the Internet checksum over a portion of memory. Used primarily for IP
+ * and ICMP.
+ */
+
+u16_t
+inet_chksum(void *dataptr, u16_t len)
+{
+  u32_t acc;
+
+  acc = LWIP_CHKSUM(dataptr, len);
+  while (acc >> 16) {
+    acc = (acc & 0xffff) + (acc >> 16);
+  }
+  return (u16_t)~(acc & 0xffff);
+}
+
+u16_t
+inet_chksum_pbuf(struct pbuf *p)
+{
+  u32_t acc;
+  struct pbuf *q;
+  u8_t swapped;
+
+  acc = 0;
+  swapped = 0;
+  for(q = p; q != NULL; q = q->next) {
+    acc += LWIP_CHKSUM(q->payload, q->len);
+    while (acc >> 16) {
+      acc = (acc & 0xffffUL) + (acc >> 16);
+    }
+    if (q->len % 2 != 0) {
+      swapped = 1 - swapped;
+      acc = (acc & 0x00ffUL << 8) | (acc & 0xff00UL >> 8);
+    }
+  }
+
+  if (swapped) {
+    acc = ((acc & 0x00ffUL) << 8) | ((acc & 0xff00UL) >> 8);
+  }
+  return (u16_t)~(acc & 0xffffUL);
+}
+
+/* Here for now until needed in other places in lwIP */
+#ifndef isprint
+#define in_range(c, lo, up)  ((u8_t)c >= lo && (u8_t)c <= up)
+#define isprint(c)           in_range(c, 0x20, 0x7f)
+#define isdigit(c)           in_range(c, '0', '9')
+#define isxdigit(c)          (isdigit(c) || in_range(c, 'a', 'f') || in_range(c, 'A', 'F'))
+#define islower(c)           in_range(c, 'a', 'z')
+#define isspace(c)           (c == ' ' || c == '\f' || c == '\n' || c == '\r' || c == '\t' || c == '\v')
+#endif    
+    
+/*
+ * Ascii internet address interpretation routine.
+ * The value returned is in network order.
+ */
+
+u32_t
+inet_addr(const char *cp)
+{
+  struct in_addr val;
+
+  if (inet_aton(cp, &val)) {
+    return (val.s_addr);
+  }
+  return (INADDR_NONE);
+}
+
+/*
+ * Check whether "cp" is a valid ascii representation
+ * of an Internet address and convert to a binary address.
+ * Returns 1 if the address is valid, 0 if not.
+ * This replaces inet_addr, the return value from which
+ * cannot distinguish between failure and a local broadcast address.
+ */
+int
+inet_aton(const char *cp, struct in_addr *addr)
+{
+  u32_t val;
+  int base, n, c;
+  u32_t parts[4];
+  u32_t *pp = parts;
+
+  c = *cp;
+  for (;;) {
+    /*
+     * Collect number up to ``.''.
+     * Values are specified as for C:
+     * 0x=hex, 0=octal, 1-9=decimal.
+     */
+    if (!isdigit(c))
+      return (0);
+    val = 0;
+    base = 10;
+    if (c == '0') {
+      c = *++cp;
+      if (c == 'x' || c == 'X') {
+        base = 16;
+        c = *++cp;
+      } else
+        base = 8;
+    }
+    for (;;) {
+      if (isdigit(c)) {
+        val = (val * base) + (int)(c - '0');
+        c = *++cp;
+      } else if (base == 16 && isxdigit(c)) {
+        val = (val << 4) | (int)(c + 10 - (islower(c) ? 'a' : 'A'));
+        c = *++cp;
+      } else
+        break;
+    }
+    if (c == '.') {
+      /*
+       * Internet format:
+       *  a.b.c.d
+       *  a.b.c   (with c treated as 16 bits)
+       *  a.b (with b treated as 24 bits)
+       */
+      if (pp >= parts + 3)
+        return (0);
+      *pp++ = val;
+      c = *++cp;
+    } else
+      break;
+  }
+  /*
+   * Check for trailing characters.
+   */
+  if (c != '\0' && (!isprint(c) || !isspace(c)))
+    return (0);
+  /*
+   * Concoct the address according to
+   * the number of parts specified.
+   */
+  n = pp - parts + 1;
+  switch (n) {
+
+  case 0:
+    return (0);       /* initial nondigit */
+
+  case 1:             /* a -- 32 bits */
+    break;
+
+  case 2:             /* a.b -- 8.24 bits */
+    if (val > 0xffffff)
+      return (0);
+    val |= parts[0] << 24;
+    break;
+
+  case 3:             /* a.b.c -- 8.8.16 bits */
+    if (val > 0xffff)
+      return (0);
+    val |= (parts[0] << 24) | (parts[1] << 16);
+    break;
+
+  case 4:             /* a.b.c.d -- 8.8.8.8 bits */
+    if (val > 0xff)
+      return (0);
+    val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
+    break;
+  }
+  if (addr)
+    addr->s_addr = htonl(val);
+  return (1);
+}
+
+/* Convert numeric IP address into decimal dotted ASCII representation.
+ * returns ptr to static buffer; not reentrant!
+ */
+char *
+inet_ntoa(struct in_addr addr)
+{
+  static char str[16];
+  u32_t s_addr = addr.s_addr;
+  char inv[3];
+  char *rp;
+  u8_t *ap;
+  u8_t rem;
+  u8_t n;
+  u8_t i;
+
+  rp = str;
+  ap = (u8_t *)&s_addr;
+  for(n = 0; n < 4; n++) {
+    i = 0;
+    do {
+      rem = *ap % (u8_t)10;
+      *ap /= (u8_t)10;
+      inv[i++] = '0' + rem;
+    } while(*ap);
+    while(i--)
+      *rp++ = inv[i];
+    *rp++ = '.';
+    ap++;
+  }
+  *--rp = 0;
+  return str;
+}
+
+/*
+ * These are reference implementations of the byte swapping functions.
+ * Again with the aim of being simple, correct and fully portable.
+ * Byte swapping is the second thing you would want to optimize. You will
+ * need to port it to your architecture and in your cc.h:
+ * 
+ * #define LWIP_PLATFORM_BYTESWAP 1
+ * #define LWIP_PLATFORM_HTONS(x) <your_htons>
+ * #define LWIP_PLATFORM_HTONL(x) <your_htonl>
+ *
+ * Note ntohs() and ntohl() are merely references to the htonx counterparts.
+ */
+
+#ifndef BYTE_ORDER
+#error BYTE_ORDER is not defined
+#endif
+#if (LWIP_PLATFORM_BYTESWAP == 0) && (BYTE_ORDER == LITTLE_ENDIAN)
+
+u16_t
+htons(u16_t n)
+{
+  return ((n & 0xff) << 8) | ((n & 0xff00) >> 8);
+}
+
+u16_t
+ntohs(u16_t n)
+{
+  return htons(n);
+}
+
+u32_t
+htonl(u32_t n)
+{
+  return ((n & 0xff) << 24) |
+    ((n & 0xff00) << 8) |
+    ((n & 0xff0000) >> 8) |
+    ((n & 0xff000000) >> 24);
+}
+
+u32_t
+ntohl(u32_t n)
+{
+  return htonl(n);
+}
+
+#endif /* (LWIP_PLATFORM_BYTESWAP == 0) && (BYTE_ORDER == LITTLE_ENDIAN) */

lwip/core/ipv4/icmp.c

@@ -0,0 +1,202 @@
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+/* Some ICMP messages should be passed to the transport protocols. This
+   is not implemented. */
+
+#include <string.h>
+
+#include "lwip/opt.h"
+#include "lwip/icmp.h"
+#include "lwip/inet.h"
+#include "lwip/ip.h"
+#include "lwip/def.h"
+#include "lwip/stats.h"
+#include "lwip/snmp.h"
+
+void
+icmp_input(struct pbuf *p, struct netif *inp)
+{
+  u8_t type;
+  u8_t code;
+  struct icmp_echo_hdr *iecho;
+  struct ip_hdr *iphdr;
+  struct ip_addr tmpaddr;
+  u16_t hlen;
+
+  ICMP_STATS_INC(icmp.recv);
+  snmp_inc_icmpinmsgs();
+
+
+  iphdr = p->payload;
+  hlen = IPH_HL(iphdr) * 4;
+  if (pbuf_header(p, -((s16_t)hlen)) || (p->tot_len < sizeof(u16_t)*2)) {
+    LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short ICMP (%"U16_F" bytes) received\n", p->tot_len));
+    pbuf_free(p);
+    ICMP_STATS_INC(icmp.lenerr);
+    snmp_inc_icmpinerrors();
+    return;
+  }
+
+  type = *((u8_t *)p->payload);
+  code = *(((u8_t *)p->payload)+1);
+  switch (type) {
+  case ICMP_ECHO:
+    /* broadcast or multicast destination address? */
+    if (ip_addr_isbroadcast(&iphdr->dest, inp) || ip_addr_ismulticast(&iphdr->dest)) {
+      LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to multicast or broadcast pings\n"));
+      ICMP_STATS_INC(icmp.err);
+      pbuf_free(p);
+      return;
+    }
+    LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n"));
+    if (p->tot_len < sizeof(struct icmp_echo_hdr)) {
+      LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n"));
+      pbuf_free(p);
+      ICMP_STATS_INC(icmp.lenerr);
+      snmp_inc_icmpinerrors();
+
+      return;
+    }
+    iecho = p->payload;
+    if (inet_chksum_pbuf(p) != 0) {
+      LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n"));
+      pbuf_free(p);
+      ICMP_STATS_INC(icmp.chkerr);
+      snmp_inc_icmpinerrors();
+      return;
+    }
+    tmpaddr.addr = iphdr->src.addr;
+    iphdr->src.addr = iphdr->dest.addr;
+    iphdr->dest.addr = tmpaddr.addr;
+    ICMPH_TYPE_SET(iecho, ICMP_ER);
+    /* adjust the checksum */
+    if (iecho->chksum >= htons(0xffff - (ICMP_ECHO << 8))) {
+      iecho->chksum += htons(ICMP_ECHO << 8) + 1;
+    } else {
+      iecho->chksum += htons(ICMP_ECHO << 8);
+    }
+    ICMP_STATS_INC(icmp.xmit);
+    /* increase number of messages attempted to send */
+    snmp_inc_icmpoutmsgs();
+    /* increase number of echo replies attempted to send */
+    snmp_inc_icmpoutechoreps();
+
+    pbuf_header(p, hlen);
+    ip_output_if(p, &(iphdr->src), IP_HDRINCL,
+     IPH_TTL(iphdr), 0, IP_PROTO_ICMP, inp);
+    break;
+  default:
+  LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ICMP type %"S16_F" code %"S16_F" not supported.\n", (s16_t)type, (s16_t)code));
+    ICMP_STATS_INC(icmp.proterr);
+    ICMP_STATS_INC(icmp.drop);
+  }
+  pbuf_free(p);
+}
+
+void
+icmp_dest_unreach(struct pbuf *p, enum icmp_dur_type t)
+{
+  struct pbuf *q;
+  struct ip_hdr *iphdr;
+  struct icmp_dur_hdr *idur;
+
+  q = pbuf_alloc(PBUF_IP, 8 + IP_HLEN + 8, PBUF_RAM);
+  /* ICMP header + IP header + 8 bytes of data */
+
+  iphdr = p->payload;
+
+  idur = q->payload;
+  ICMPH_TYPE_SET(idur, ICMP_DUR);
+  ICMPH_CODE_SET(idur, t);
+
+  memcpy((u8_t *)q->payload + 8, p->payload, IP_HLEN + 8);
+
+  /* calculate checksum */
+  idur->chksum = 0;
+  idur->chksum = inet_chksum(idur, q->len);
+  ICMP_STATS_INC(icmp.xmit);
+  /* increase number of messages attempted to send */
+  snmp_inc_icmpoutmsgs();
+  /* increase number of destination unreachable messages attempted to send */
+  snmp_inc_icmpoutdestunreachs();
+
+  ip_output(q, NULL, &(iphdr->src),
+      ICMP_TTL, 0, IP_PROTO_ICMP);
+  pbuf_free(q);
+}
+
+#if IP_FORWARD
+void
+icmp_time_exceeded(struct pbuf *p, enum icmp_te_type t)
+{
+  struct pbuf *q;
+  struct ip_hdr *iphdr;
+  struct icmp_te_hdr *tehdr;
+
+  q = pbuf_alloc(PBUF_IP, 8 + IP_HLEN + 8, PBUF_RAM);
+
+  iphdr = p->payload;
+  LWIP_DEBUGF(ICMP_DEBUG, ("icmp_time_exceeded from "));
+  ip_addr_debug_print(ICMP_DEBUG, &(iphdr->src));
+  LWIP_DEBUGF(ICMP_DEBUG, (" to "));
+  ip_addr_debug_print(ICMP_DEBUG, &(iphdr->dest));
+  LWIP_DEBUGF(ICMP_DEBUG, ("\n"));
+
+  tehdr = q->payload;
+  ICMPH_TYPE_SET(tehdr, ICMP_TE);
+  ICMPH_CODE_SET(tehdr, t);
+
+  /* copy fields from original packet */
+  memcpy((u8_t *)q->payload + 8, (u8_t *)p->payload, IP_HLEN + 8);
+
+  /* calculate checksum */
+  tehdr->chksum = 0;
+  tehdr->chksum = inet_chksum(tehdr, q->len);
+  ICMP_STATS_INC(icmp.xmit);
+  /* increase number of messages attempted to send */
+  snmp_inc_icmpoutmsgs();
+  /* increase number of destination unreachable messages attempted to send */
+  snmp_inc_icmpouttimeexcds();
+  ip_output(q, NULL, &(iphdr->src),
+      ICMP_TTL, 0, IP_PROTO_ICMP);
+  pbuf_free(q);
+}
+
+#endif /* IP_FORWARD */
+
+
+
+
+
+
+

lwip/core/ipv4/ip.c

@@ -0,0 +1,515 @@
+/* @file
+ *
+ * This is the IP layer implementation for incoming and outgoing IP traffic.
+ * 
+ * @see ip_frag.c
+ *
+ */
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include "lwip/opt.h"
+
+#include "lwip/def.h"
+#include "lwip/mem.h"
+#include "lwip/ip.h"
+#include "lwip/ip_frag.h"
+#include "lwip/inet.h"
+#include "lwip/netif.h"
+#include "lwip/icmp.h"
+#include "lwip/raw.h"
+#include "lwip/udp.h"
+#include "lwip/tcp.h"
+
+#include "lwip/stats.h"
+
+#include "arch/perf.h"
+
+#include "lwip/snmp.h"
+#if LWIP_DHCP
+#  include "lwip/dhcp.h"
+#endif /* LWIP_DHCP */
+
+/**
+ * Initializes the IP layer.
+ */
+
+void
+ip_init(void)
+{
+#if IP_FRAG
+  ip_frag_init();
+#endif
+}
+
+/**
+ * Finds the appropriate network interface for a given IP address. It
+ * searches the list of network interfaces linearly. A match is found
+ * if the masked IP address of the network interface equals the masked
+ * IP address given to the function.
+ */
+
+struct netif *
+ip_route(struct ip_addr *dest)
+{
+  struct netif *netif;
+
+  /* iterate through netifs */
+  for(netif = netif_list; netif != NULL; netif = netif->next) {
+    /* network mask matches? */
+    if (ip_addr_netcmp(dest, &(netif->ip_addr), &(netif->netmask))) {
+      /* return netif on which to forward IP packet */
+      return netif;
+    }
+  }
+  /* no matching netif found, use default netif */
+  return netif_default;
+}
+#if IP_FORWARD
+
+/**
+ * Forwards an IP packet. It finds an appropriate route for the
+ * packet, decrements the TTL value of the packet, adjusts the
+ * checksum and outputs the packet on the appropriate interface.
+ */
+
+static struct netif *
+ip_forward(struct pbuf *p, struct ip_hdr *iphdr, struct netif *inp)
+{
+  struct netif *netif;
+
+  PERF_START;
+  /* Find network interface where to forward this IP packet to. */
+  netif = ip_route((struct ip_addr *)&(iphdr->dest));
+  if (netif == NULL) {
+    LWIP_DEBUGF(IP_DEBUG, ("ip_forward: no forwarding route for 0x%"X32_F" found\n",
+                      iphdr->dest.addr));
+    snmp_inc_ipoutnoroutes();
+    return (struct netif *)NULL;
+  }
+  /* Do not forward packets onto the same network interface on which
+   * they arrived. */
+  if (netif == inp) {
+    LWIP_DEBUGF(IP_DEBUG, ("ip_forward: not bouncing packets back on incoming interface.\n"));
+    snmp_inc_ipoutnoroutes();
+    return (struct netif *)NULL;
+  }
+
+  /* decrement TTL */
+  IPH_TTL_SET(iphdr, IPH_TTL(iphdr) - 1);
+  /* send ICMP if TTL == 0 */
+  if (IPH_TTL(iphdr) == 0) {
+    snmp_inc_ipinhdrerrors();
+    /* Don't send ICMP messages in response to ICMP messages */
+    if (IPH_PROTO(iphdr) != IP_PROTO_ICMP) {
+      icmp_time_exceeded(p, ICMP_TE_TTL);
+    }
+    return (struct netif *)NULL;
+  }
+
+  /* Incrementally update the IP checksum. */
+  if (IPH_CHKSUM(iphdr) >= htons(0xffff - 0x100)) {
+    IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + htons(0x100) + 1);
+  } else {
+    IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + htons(0x100));
+  }
+
+  LWIP_DEBUGF(IP_DEBUG, ("ip_forward: forwarding packet to 0x%"X32_F"\n",
+                    iphdr->dest.addr));
+
+  IP_STATS_INC(ip.fw);
+  IP_STATS_INC(ip.xmit);
+  snmp_inc_ipforwdatagrams();
+
+  PERF_STOP("ip_forward");
+  /* transmit pbuf on chosen interface */
+  netif->output(netif, p, (struct ip_addr *)&(iphdr->dest));
+  return netif;
+}
+#endif /* IP_FORWARD */
+
+/**
+ * This function is called by the network interface device driver when
+ * an IP packet is received. The function does the basic checks of the
+ * IP header such as packet size being at least larger than the header
+ * size etc. If the packet was not destined for us, the packet is
+ * forwarded (using ip_forward). The IP checksum is always checked.
+ *
+ * Finally, the packet is sent to the upper layer protocol input function.
+ * 
+ * 
+ * 
+ */
+
+err_t
+ip_input(struct pbuf *p, struct netif *inp) {
+  struct ip_hdr *iphdr;
+  struct netif *netif;
+  u16_t iphdrlen;
+
+  IP_STATS_INC(ip.recv);
+  snmp_inc_ipinreceives();
+
+  /* identify the IP header */
+  iphdr = p->payload;
+  if (IPH_V(iphdr) != 4) {
+    LWIP_DEBUGF(IP_DEBUG | 1, ("IP packet dropped due to bad version number %"U16_F"\n", IPH_V(iphdr)));
+    ip_debug_print(p);
+    pbuf_free(p);
+    IP_STATS_INC(ip.err);
+    IP_STATS_INC(ip.drop);
+    snmp_inc_ipinhdrerrors();
+    return ERR_OK;
+  }
+  /* obtain IP header length in number of 32-bit words */
+  iphdrlen = IPH_HL(iphdr);
+  /* calculate IP header length in bytes */
+  iphdrlen *= 4;
+
+  /* header length exceeds first pbuf length? */
+  if (iphdrlen > p->len) {
+    LWIP_DEBUGF(IP_DEBUG | 2, ("IP header (len %"U16_F") does not fit in first pbuf (len %"U16_F"), IP packet droppped.\n",
+      iphdrlen, p->len));
+    /* free (drop) packet pbufs */
+    pbuf_free(p);
+    IP_STATS_INC(ip.lenerr);
+    IP_STATS_INC(ip.drop);
+    snmp_inc_ipindiscards();
+    return ERR_OK;
+  }
+
+  /* verify checksum */
+#if CHECKSUM_CHECK_IP
+  if (inet_chksum(iphdr, iphdrlen) != 0) {
+
+    LWIP_DEBUGF(IP_DEBUG | 2, ("Checksum (0x%"X16_F") failed, IP packet dropped.\n", inet_chksum(iphdr, iphdrlen)));
+    ip_debug_print(p);
+    pbuf_free(p);
+    IP_STATS_INC(ip.chkerr);
+    IP_STATS_INC(ip.drop);
+    snmp_inc_ipinhdrerrors();
+    return ERR_OK;
+  }
+#endif
+
+  /* Trim pbuf. This should have been done at the netif layer,
+   * but we'll do it anyway just to be sure that its done. */
+  pbuf_realloc(p, ntohs(IPH_LEN(iphdr)));
+
+  /* match packet against an interface, i.e. is this packet for us? */
+  for (netif = netif_list; netif != NULL; netif = netif->next) {
+
+    LWIP_DEBUGF(IP_DEBUG, ("ip_input: iphdr->dest 0x%"X32_F" netif->ip_addr 0x%"X32_F" (0x%"X32_F", 0x%"X32_F", 0x%"X32_F")\n",
+      iphdr->dest.addr, netif->ip_addr.addr,
+      iphdr->dest.addr & netif->netmask.addr,
+      netif->ip_addr.addr & netif->netmask.addr,
+      iphdr->dest.addr & ~(netif->netmask.addr)));
+
+    /* interface is up and configured? */
+    if ((netif_is_up(netif)) && (!ip_addr_isany(&(netif->ip_addr))))
+    {
+      /* unicast to this interface address? */
+      if (ip_addr_cmp(&(iphdr->dest), &(netif->ip_addr)) ||
+         /* or broadcast on this interface network address? */
+         ip_addr_isbroadcast(&(iphdr->dest), netif)) {
+        LWIP_DEBUGF(IP_DEBUG, ("ip_input: packet accepted on interface %c%c\n",
+          netif->name[0], netif->name[1]));
+        /* break out of for loop */
+        break;
+      }
+    }
+  }
+#if LWIP_DHCP
+  /* Pass DHCP messages regardless of destination address. DHCP traffic is addressed
+   * using link layer addressing (such as Ethernet MAC) so we must not filter on IP.
+   * According to RFC 1542 section 3.1.1, referred by RFC 2131).
+   */
+  if (netif == NULL) {
+    /* remote port is DHCP server? */
+    if (IPH_PROTO(iphdr) == IP_PROTO_UDP) {
+      LWIP_DEBUGF(IP_DEBUG | DBG_TRACE | 1, ("ip_input: UDP packet to DHCP client port %"U16_F"\n",
+        ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdrlen))->dest)));
+      if (ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdrlen))->dest) == DHCP_CLIENT_PORT) {
+        LWIP_DEBUGF(IP_DEBUG | DBG_TRACE | 1, ("ip_input: DHCP packet accepted.\n"));
+        netif = inp;
+      }
+    }
+  }
+#endif /* LWIP_DHCP */
+  /* packet not for us? */
+  if (netif == NULL) {
+    /* packet not for us, route or discard */
+    LWIP_DEBUGF(IP_DEBUG | DBG_TRACE | 1, ("ip_input: packet not for us.\n"));
+#if IP_FORWARD
+    /* non-broadcast packet? */
+    if (!ip_addr_isbroadcast(&(iphdr->dest), inp)) {
+      /* try to forward IP packet on (other) interfaces */
+      ip_forward(p, iphdr, inp);
+    }
+    else
+#endif /* IP_FORWARD */
+    {
+      snmp_inc_ipinaddrerrors();
+      snmp_inc_ipindiscards();
+    }
+    pbuf_free(p);
+    return ERR_OK;
+  }
+  /* packet consists of multiple fragments? */
+  if ((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0) {
+#if IP_REASSEMBLY /* packet fragment reassembly code present? */
+    LWIP_DEBUGF(IP_DEBUG, ("IP packet is a fragment (id=0x%04"X16_F" tot_len=%"U16_F" len=%"U16_F" MF=%"U16_F" offset=%"U16_F"), calling ip_reass()\n",
+      ntohs(IPH_ID(iphdr)), p->tot_len, ntohs(IPH_LEN(iphdr)), !!(IPH_OFFSET(iphdr) & htons(IP_MF)), (ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK)*8));
+    /* reassemble the packet*/
+    p = ip_reass(p);
+    /* packet not fully reassembled yet? */
+    if (p == NULL) {
+      return ERR_OK;
+    }
+    iphdr = p->payload;
+#else /* IP_REASSEMBLY == 0, no packet fragment reassembly code present */
+    pbuf_free(p);
+    LWIP_DEBUGF(IP_DEBUG | 2, ("IP packet dropped since it was fragmented (0x%"X16_F") (while IP_REASSEMBLY == 0).\n",
+      ntohs(IPH_OFFSET(iphdr))));
+    IP_STATS_INC(ip.opterr);
+    IP_STATS_INC(ip.drop);
+    /* unsupported protocol feature */
+    snmp_inc_ipinunknownprotos();
+    return ERR_OK;
+#endif /* IP_REASSEMBLY */
+  }
+
+#if IP_OPTIONS == 0 /* no support for IP options in the IP header? */
+  if (iphdrlen > IP_HLEN) {
+    LWIP_DEBUGF(IP_DEBUG | 2, ("IP packet dropped since there were IP options (while IP_OPTIONS == 0).\n"));
+    pbuf_free(p);
+    IP_STATS_INC(ip.opterr);
+    IP_STATS_INC(ip.drop);
+    /* unsupported protocol feature */
+    snmp_inc_ipinunknownprotos();
+    return ERR_OK;
+  }
+#endif /* IP_OPTIONS == 0 */
+
+  /* send to upper layers */
+  LWIP_DEBUGF(IP_DEBUG, ("ip_input: \n"));
+  ip_debug_print(p);
+  LWIP_DEBUGF(IP_DEBUG, ("ip_input: p->len %"U16_F" p->tot_len %"U16_F"\n", p->len, p->tot_len));
+
+#if LWIP_RAW
+  /* raw input did not eat the packet? */
+  if (raw_input(p, inp) == 0) {
+#endif /* LWIP_RAW */
+
+  switch (IPH_PROTO(iphdr)) {
+#if LWIP_UDP
+  case IP_PROTO_UDP:
+  case IP_PROTO_UDPLITE:
+    snmp_inc_ipindelivers();
+    udp_input(p, inp);
+    break;
+#endif /* LWIP_UDP */
+#if LWIP_TCP
+  case IP_PROTO_TCP:
+    snmp_inc_ipindelivers();
+    tcp_input(p, inp);
+    break;
+#endif /* LWIP_TCP */
+  case IP_PROTO_ICMP:
+    snmp_inc_ipindelivers();
+    icmp_input(p, inp);
+    break;
+  default:
+    /* send ICMP destination protocol unreachable unless is was a broadcast */
+    if (!ip_addr_isbroadcast(&(iphdr->dest), inp) &&
+        !ip_addr_ismulticast(&(iphdr->dest))) {
+      p->payload = iphdr;
+      icmp_dest_unreach(p, ICMP_DUR_PROTO);
+    }
+    pbuf_free(p);
+
+    LWIP_DEBUGF(IP_DEBUG | 2, ("Unsupported transport protocol %"U16_F"\n", IPH_PROTO(iphdr)));
+
+    IP_STATS_INC(ip.proterr);
+    IP_STATS_INC(ip.drop);
+    snmp_inc_ipinunknownprotos();
+  }
+#if LWIP_RAW
+  } /* LWIP_RAW */
+#endif
+  return ERR_OK;
+}
+
+/**
+ * Sends an IP packet on a network interface. This function constructs
+ * the IP header and calculates the IP header checksum. If the source
+ * IP address is NULL, the IP address of the outgoing network
+ * interface is filled in as source address.
+ *
+ * @note ip_id: RFC791 "some host may be able to simply use
+ *  unique identifiers independent of destination"
+ */
+
+err_t
+ip_output_if(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
+             u8_t ttl, u8_t tos,
+             u8_t proto, struct netif *netif)
+{
+  struct ip_hdr *iphdr;
+  static u16_t ip_id = 0;
+
+  snmp_inc_ipoutrequests();
+
+  if (dest != IP_HDRINCL) {
+    if (pbuf_header(p, IP_HLEN)) {
+      LWIP_DEBUGF(IP_DEBUG | 2, ("ip_output: not enough room for IP header in pbuf\n"));
+
+      IP_STATS_INC(ip.err);
+      snmp_inc_ipoutdiscards();
+      return ERR_BUF;
+    }
+
+    iphdr = p->payload;
+
+    IPH_TTL_SET(iphdr, ttl);
+    IPH_PROTO_SET(iphdr, proto);
+
+    ip_addr_set(&(iphdr->dest), dest);
+
+    IPH_VHLTOS_SET(iphdr, 4, IP_HLEN / 4, tos);
+    IPH_LEN_SET(iphdr, htons(p->tot_len));
+    IPH_OFFSET_SET(iphdr, htons(IP_DF));
+    IPH_ID_SET(iphdr, htons(ip_id));
+    ++ip_id;
+
+    if (ip_addr_isany(src)) {
+      ip_addr_set(&(iphdr->src), &(netif->ip_addr));
+    } else {
+      ip_addr_set(&(iphdr->src), src);
+    }
+
+    IPH_CHKSUM_SET(iphdr, 0);
+#if CHECKSUM_GEN_IP
+    IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN));
+#endif
+  } else {
+    iphdr = p->payload;
+    dest = &(iphdr->dest);
+  }
+
+#if IP_FRAG
+  /* don't fragment if interface has mtu set to 0 [loopif] */
+  if (netif->mtu && (p->tot_len > netif->mtu))
+    return ip_frag(p,netif,dest);
+#endif
+
+  IP_STATS_INC(ip.xmit);
+
+  LWIP_DEBUGF(IP_DEBUG, ("ip_output_if: %c%c%"U16_F"\n", netif->name[0], netif->name[1], netif->num));
+  ip_debug_print(p);
+
+  LWIP_DEBUGF(IP_DEBUG, ("netif->output()"));
+
+  return netif->output(netif, p, dest);
+}
+
+/**
+ * Simple interface to ip_output_if. It finds the outgoing network
+ * interface and calls upon ip_output_if to do the actual work.
+ */
+
+err_t
+ip_output(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
+          u8_t ttl, u8_t tos, u8_t proto)
+{
+  struct netif *netif;
+
+  if ((netif = ip_route(dest)) == NULL) {
+    LWIP_DEBUGF(IP_DEBUG | 2, ("ip_output: No route to 0x%"X32_F"\n", dest->addr));
+
+    IP_STATS_INC(ip.rterr);
+    snmp_inc_ipoutnoroutes();
+    return ERR_RTE;
+  }
+
+  return ip_output_if(p, src, dest, ttl, tos, proto, netif);
+}
+
+#if IP_DEBUG
+void
+ip_debug_print(struct pbuf *p)
+{
+  struct ip_hdr *iphdr = p->payload;
+  u8_t *payload;
+
+  payload = (u8_t *)iphdr + IP_HLEN;
+
+  LWIP_DEBUGF(IP_DEBUG, ("IP header:\n"));
+  LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(IP_DEBUG, ("|%2"S16_F" |%2"S16_F" |  0x%02"X16_F" |     %5"U16_F"     | (v, hl, tos, len)\n",
+                    IPH_V(iphdr),
+                    IPH_HL(iphdr),
+                    IPH_TOS(iphdr),
+                    ntohs(IPH_LEN(iphdr))));
+  LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(IP_DEBUG, ("|    %5"U16_F"      |%"U16_F"%"U16_F"%"U16_F"|    %4"U16_F"   | (id, flags, offset)\n",
+                    ntohs(IPH_ID(iphdr)),
+                    ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
+                    ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
+                    ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
+                    ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
+  LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(IP_DEBUG, ("|  %3"U16_F"  |  %3"U16_F"  |    0x%04"X16_F"     | (ttl, proto, chksum)\n",
+                    IPH_TTL(iphdr),
+                    IPH_PROTO(iphdr),
+                    ntohs(IPH_CHKSUM(iphdr))));
+  LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(IP_DEBUG, ("|  %3"U16_F"  |  %3"U16_F"  |  %3"U16_F"  |  %3"U16_F"  | (src)\n",
+                    ip4_addr1(&iphdr->src),
+                    ip4_addr2(&iphdr->src),
+                    ip4_addr3(&iphdr->src),
+                    ip4_addr4(&iphdr->src)));
+  LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(IP_DEBUG, ("|  %3"U16_F"  |  %3"U16_F"  |  %3"U16_F"  |  %3"U16_F"  | (dest)\n",
+                    ip4_addr1(&iphdr->dest),
+                    ip4_addr2(&iphdr->dest),
+                    ip4_addr3(&iphdr->dest),
+                    ip4_addr4(&iphdr->dest)));
+  LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
+}
+#endif /* IP_DEBUG */
+
+
+
+
+
+

lwip/core/ipv4/ip_addr.c

@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved. 
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, 
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission. 
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ * 
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include "lwip/ip_addr.h"
+#include "lwip/inet.h"
+#include "lwip/netif.h"
+
+#define IP_ADDR_ANY_VALUE 0x00000000UL
+#define IP_ADDR_BROADCAST_VALUE 0xffffffffUL
+
+/* used by IP_ADDR_ANY and IP_ADDR_BROADCAST in ip_addr.h */
+const struct ip_addr ip_addr_any = { IP_ADDR_ANY_VALUE };
+const struct ip_addr ip_addr_broadcast = { IP_ADDR_BROADCAST_VALUE };
+
+/* Determine if an address is a broadcast address on a network interface 
+ * 
+ * @param addr address to be checked
+ * @param netif the network interface against which the address is checked
+ * @return returns non-zero if the address is a broadcast address
+ *
+ */
+
+u8_t ip_addr_isbroadcast(struct ip_addr *addr, struct netif *netif)
+{
+  u32_t addr2test;
+
+  addr2test = addr->addr;
+  /* all ones (broadcast) or all zeroes (old skool broadcast) */
+  if ((~addr2test == IP_ADDR_ANY_VALUE) ||
+      (addr2test == IP_ADDR_ANY_VALUE))
+    return 1;
+  /* no broadcast support on this network interface? */
+  else if ((netif->flags & NETIF_FLAG_BROADCAST) == 0)
+    /* the given address cannot be a broadcast address
+     * nor can we check against any broadcast addresses */
+    return 0;
+  /* address matches network interface address exactly? => no broadcast */
+  else if (addr2test == netif->ip_addr.addr)
+    return 0;
+  /*  on the same (sub) network... */
+  else if (ip_addr_netcmp(addr, &(netif->ip_addr), &(netif->netmask))
+         /* ...and host identifier bits are all ones? =>... */
+          && ((addr2test & ~netif->netmask.addr) ==
+           (IP_ADDR_BROADCAST_VALUE & ~netif->netmask.addr)))
+    /* => network broadcast address */
+    return 1;
+  else
+    return 0;
+}

lwip/core/ipv4/ip_frag.c

@@ -0,0 +1,388 @@
+/* @file
+ * 
+ * This is the IP packet segmentation and reassembly implementation.
+ *
+ */
+
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved. 
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, 
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission. 
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ * 
+ * Author: Jani Monoses <jani@iv.ro> 
+ * original reassembly code by Adam Dunkels <adam@sics.se>
+ * 
+ */
+
+#include <string.h>
+
+#include "lwip/opt.h"
+#include "lwip/ip.h"
+#include "lwip/ip_frag.h"
+#include "lwip/netif.h"
+#include "lwip/snmp.h"
+#include "lwip/stats.h"
+
+static u8_t ip_reassbuf[IP_HLEN + IP_REASS_BUFSIZE];
+static u8_t ip_reassbitmap[IP_REASS_BUFSIZE / (8 * 8) + 1];
+static const u8_t bitmap_bits[8] = { 0xff, 0x7f, 0x3f, 0x1f,
+  0x0f, 0x07, 0x03, 0x01
+};
+static u16_t ip_reasslen;
+static u8_t ip_reassflags;
+#define IP_REASS_FLAG_LASTFRAG 0x01
+
+static u8_t ip_reasstmr;
+
+/*
+ * Copy len bytes from offset in pbuf to buffer 
+ *
+ * helper used by both ip_reass and ip_frag
+ */
+static struct pbuf *
+copy_from_pbuf(struct pbuf *p, u16_t * offset,
+           u8_t * buffer, u16_t len)
+{
+  u16_t l;
+
+  p->payload = (u8_t *)p->payload + *offset;
+  p->len -= *offset;
+  while (len) {
+    l = len < p->len ? len : p->len;
+    memcpy(buffer, p->payload, l);
+    buffer += l;
+    len -= l;
+    if (len)
+      p = p->next;
+    else
+      *offset = l;
+  }
+  return p;
+}
+
+
+/**
+ * Initializes IP reassembly and fragmentation states.
+ */
+void
+ip_frag_init(void)
+{
+  ip_reasstmr = 0;
+  ip_reassflags = 0;
+  ip_reasslen = 0;
+  memset(ip_reassbitmap, 0, sizeof(ip_reassbitmap));
+}
+
+/**
+ * Reassembly timer base function
+ * for both NO_SYS == 0 and 1 (!).
+ *
+ * Should be called every 1000 msec.
+ */
+void
+ip_reass_tmr(void)
+{
+  if (ip_reasstmr > 0) {
+    ip_reasstmr--;
+    LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass_tmr: timer dec %"U16_F"\n",(u16_t)ip_reasstmr));
+    if (ip_reasstmr == 0) {
+      /* reassembly timed out */
+      snmp_inc_ipreasmfails();
+    }
+  }
+}
+
+/**
+ * Reassembles incoming IP fragments into an IP datagram.
+ *
+ * @param p points to a pbuf chain of the fragment
+ * @return NULL if reassembly is incomplete, ? otherwise
+ */
+struct pbuf *
+ip_reass(struct pbuf *p)
+{
+  struct pbuf *q;
+  struct ip_hdr *fraghdr, *iphdr;
+  u16_t offset, len;
+  u16_t i;
+
+  IPFRAG_STATS_INC(ip_frag.recv);
+  snmp_inc_ipreasmreqds();
+
+  iphdr = (struct ip_hdr *) ip_reassbuf;
+  fraghdr = (struct ip_hdr *) p->payload;
+  /* If ip_reasstmr is zero, no packet is present in the buffer, so we
+     write the IP header of the fragment into the reassembly
+     buffer. The timer is updated with the maximum age. */
+  if (ip_reasstmr == 0) {
+    LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: new packet\n"));
+    memcpy(iphdr, fraghdr, IP_HLEN);
+    ip_reasstmr = IP_REASS_MAXAGE;
+    ip_reassflags = 0;
+    /* Clear the bitmap. */
+    memset(ip_reassbitmap, 0, sizeof(ip_reassbitmap));
+  }
+
+  /* Check if the incoming fragment matches the one currently present
+     in the reasembly buffer. If so, we proceed with copying the
+     fragment into the buffer. */
+  if (ip_addr_cmp(&iphdr->src, &fraghdr->src) &&
+      ip_addr_cmp(&iphdr->dest, &fraghdr->dest) &&
+      IPH_ID(iphdr) == IPH_ID(fraghdr)) {
+    LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: matching previous fragment ID=%"X16_F"\n",
+      ntohs(IPH_ID(fraghdr))));
+    IPFRAG_STATS_INC(ip_frag.cachehit);
+    /* Find out the offset in the reassembly buffer where we should
+       copy the fragment. */
+    len = ntohs(IPH_LEN(fraghdr)) - IPH_HL(fraghdr) * 4;
+    offset = (ntohs(IPH_OFFSET(fraghdr)) & IP_OFFMASK) * 8;
+
+    /* If the offset or the offset + fragment length overflows the
+       reassembly buffer, we discard the entire packet. */
+    if ((offset > IP_REASS_BUFSIZE) || ((offset + len) > IP_REASS_BUFSIZE)) {
+      LWIP_DEBUGF(IP_REASS_DEBUG,
+       ("ip_reass: fragment outside of buffer (%"S16_F":%"S16_F"/%"S16_F").\n", offset,
+        offset + len, IP_REASS_BUFSIZE));
+      ip_reasstmr = 0;
+      snmp_inc_ipreasmfails();
+      goto nullreturn;
+    }
+
+    /* Copy the fragment into the reassembly buffer, at the right
+       offset. */
+    LWIP_DEBUGF(IP_REASS_DEBUG,
+     ("ip_reass: copying with offset %"S16_F" into %"S16_F":%"S16_F"\n", offset,
+      IP_HLEN + offset, IP_HLEN + offset + len));
+    i = IPH_HL(fraghdr) * 4;
+    copy_from_pbuf(p, &i, &ip_reassbuf[IP_HLEN + offset], len);
+
+    /* Update the bitmap. */
+    if (offset / (8 * 8) == (offset + len) / (8 * 8)) {
+      LWIP_DEBUGF(IP_REASS_DEBUG,
+       ("ip_reass: updating single byte in bitmap.\n"));
+      /* If the two endpoints are in the same byte, we only update that byte. */
+      LWIP_ASSERT("offset / (8 * 8) < sizeof(ip_reassbitmap)",
+                   offset / (8 * 8) < sizeof(ip_reassbitmap));
+      ip_reassbitmap[offset / (8 * 8)] |=
+        bitmap_bits[(offset / 8) & 7] &
+        ~bitmap_bits[((offset + len) / 8) & 7];
+    } else {
+      /* If the two endpoints are in different bytes, we update the
+         bytes in the endpoints and fill the stuff inbetween with
+         0xff. */
+      LWIP_ASSERT("offset / (8 * 8) < sizeof(ip_reassbitmap)",
+                   offset / (8 * 8) < sizeof(ip_reassbitmap));
+      ip_reassbitmap[offset / (8 * 8)] |= bitmap_bits[(offset / 8) & 7];
+      LWIP_DEBUGF(IP_REASS_DEBUG,
+       ("ip_reass: updating many bytes in bitmap (%"S16_F":%"S16_F").\n",
+        1 + offset / (8 * 8), (offset + len) / (8 * 8)));
+      for (i = 1 + offset / (8 * 8); i < (offset + len) / (8 * 8); ++i) {
+        ip_reassbitmap[i] = 0xff;
+      }
+      LWIP_ASSERT("(offset + len) / (8 * 8) < sizeof(ip_reassbitmap)",
+                   (offset + len) / (8 * 8) < sizeof(ip_reassbitmap));
+      ip_reassbitmap[(offset + len) / (8 * 8)] |=
+        ~bitmap_bits[((offset + len) / 8) & 7];
+    }
+
+    /* If this fragment has the More Fragments flag set to zero, we
+       know that this is the last fragment, so we can calculate the
+       size of the entire packet. We also set the
+       IP_REASS_FLAG_LASTFRAG flag to indicate that we have received
+       the final fragment. */
+
+    if ((ntohs(IPH_OFFSET(fraghdr)) & IP_MF) == 0) {
+      ip_reassflags |= IP_REASS_FLAG_LASTFRAG;
+      ip_reasslen = offset + len;
+      LWIP_DEBUGF(IP_REASS_DEBUG,
+       ("ip_reass: last fragment seen, total len %"S16_F"\n",
+        ip_reasslen));
+    }
+
+    /* Finally, we check if we have a full packet in the buffer. We do
+       this by checking if we have the last fragment and if all bits
+       in the bitmap are set. */
+    if (ip_reassflags & IP_REASS_FLAG_LASTFRAG) {
+      /* Check all bytes up to and including all but the last byte in
+         the bitmap. */
+      LWIP_ASSERT("ip_reasslen / (8 * 8) - 1 < sizeof(ip_reassbitmap)",
+                   ip_reasslen / (8 * 8) - 1 < ((u16_t) sizeof(ip_reassbitmap)));
+      for (i = 0; i < ip_reasslen / (8 * 8) - 1; ++i) {
+        if (ip_reassbitmap[i] != 0xff) {
+          LWIP_DEBUGF(IP_REASS_DEBUG,
+           ("ip_reass: last fragment seen, bitmap %"S16_F"/%"S16_F" failed (%"X16_F")\n",
+            i, ip_reasslen / (8 * 8) - 1, ip_reassbitmap[i]));
+          goto nullreturn;
+        }
+      }
+      /* Check the last byte in the bitmap. It should contain just the
+         right amount of bits. */
+      LWIP_ASSERT("ip_reasslen / (8 * 8) < sizeof(ip_reassbitmap)",
+                   ip_reasslen / (8 * 8) < sizeof(ip_reassbitmap));
+      if (ip_reassbitmap[ip_reasslen / (8 * 8)] !=
+        (u8_t) ~ bitmap_bits[ip_reasslen / 8 & 7]) {
+         LWIP_DEBUGF(IP_REASS_DEBUG,
+          ("ip_reass: last fragment seen, bitmap %"S16_F" didn't contain %"X16_F" (%"X16_F")\n",
+        ip_reasslen / (8 * 8), ~bitmap_bits[ip_reasslen / 8 & 7],
+        ip_reassbitmap[ip_reasslen / (8 * 8)]));
+        goto nullreturn;
+      }
+
+      /* Pretend to be a "normal" (i.e., not fragmented) IP packet
+         from now on. */
+      ip_reasslen += IP_HLEN;
+
+      IPH_LEN_SET(iphdr, htons(ip_reasslen));
+      IPH_OFFSET_SET(iphdr, 0);
+      IPH_CHKSUM_SET(iphdr, 0);
+      IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN));
+
+      /* If we have come this far, we have a full packet in the
+         buffer, so we allocate a pbuf and copy the packet into it. We
+         also reset the timer. */
+      ip_reasstmr = 0;
+      pbuf_free(p);
+      p = pbuf_alloc(PBUF_LINK, ip_reasslen, PBUF_POOL);
+      if (p != NULL) {
+        i = 0;
+        for (q = p; q != NULL; q = q->next) {
+          /* Copy enough bytes to fill this pbuf in the chain. The
+             available data in the pbuf is given by the q->len variable. */
+          LWIP_DEBUGF(IP_REASS_DEBUG,
+           ("ip_reass: memcpy from %p (%"S16_F") to %p, %"S16_F" bytes\n",
+            (void *)&ip_reassbuf[i], i, q->payload,
+            q->len > ip_reasslen - i ? ip_reasslen - i : q->len));
+          memcpy(q->payload, &ip_reassbuf[i],
+            q->len > ip_reasslen - i ? ip_reasslen - i : q->len);
+          i += q->len;
+        }
+        IPFRAG_STATS_INC(ip_frag.fw);
+        snmp_inc_ipreasmoks();
+      } else {
+        LWIP_DEBUGF(IP_REASS_DEBUG,
+          ("ip_reass: pbuf_alloc(PBUF_LINK, ip_reasslen=%"U16_F", PBUF_POOL) failed\n", ip_reasslen));
+        IPFRAG_STATS_INC(ip_frag.memerr);
+        snmp_inc_ipreasmfails();
+      }
+      LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: p %p\n", (void*)p));
+      return p;
+    }
+  }
+
+nullreturn:
+  IPFRAG_STATS_INC(ip_frag.drop);
+  pbuf_free(p);
+  return NULL;
+}
+
+static u8_t buf[MEM_ALIGN_SIZE(IP_FRAG_MAX_MTU)];
+
+/**
+ * Fragment an IP datagram if too large for the netif.
+ *
+ * Chop the datagram in MTU sized chunks and send them in order
+ * by using a fixed size static memory buffer (PBUF_ROM)
+ */
+err_t 
+ip_frag(struct pbuf *p, struct netif *netif, struct ip_addr *dest)
+{
+  struct pbuf *rambuf;
+  struct pbuf *header;
+  struct ip_hdr *iphdr;
+  u16_t nfb = 0;
+  u16_t left, cop;
+  u16_t mtu = netif->mtu;
+  u16_t ofo, omf;
+  u16_t last;
+  u16_t poff = IP_HLEN;
+  u16_t tmp;
+
+  /* Get a RAM based MTU sized pbuf */
+  rambuf = pbuf_alloc(PBUF_LINK, 0, PBUF_REF);
+  if (rambuf == NULL) {
+    LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_frag: pbuf_alloc(PBUF_LINK, 0, PBUF_REF) failed\n"));
+    return ERR_MEM;
+  }
+  rambuf->tot_len = rambuf->len = mtu;
+  rambuf->payload = MEM_ALIGN((void *)buf);
+
+  /* Copy the IP header in it */
+  iphdr = rambuf->payload;
+  memcpy(iphdr, p->payload, IP_HLEN);
+
+  /* Save original offset */
+  tmp = ntohs(IPH_OFFSET(iphdr));
+  ofo = tmp & IP_OFFMASK;
+  omf = tmp & IP_MF;
+
+  left = p->tot_len - IP_HLEN;
+
+  while (left) {
+    last = (left <= mtu - IP_HLEN);
+
+    /* Set new offset and MF flag */
+    ofo += nfb;
+    tmp = omf | (IP_OFFMASK & (ofo));
+    if (!last)
+      tmp = tmp | IP_MF;
+    IPH_OFFSET_SET(iphdr, htons(tmp));
+
+    /* Fill this fragment */
+    nfb = (mtu - IP_HLEN) / 8;
+    cop = last ? left : nfb * 8;
+
+    p = copy_from_pbuf(p, &poff, (u8_t *) iphdr + IP_HLEN, cop);
+
+    /* Correct header */
+    IPH_LEN_SET(iphdr, htons(cop + IP_HLEN));
+    IPH_CHKSUM_SET(iphdr, 0);
+    IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN));
+
+    if (last)
+      pbuf_realloc(rambuf, left + IP_HLEN);
+    /* This part is ugly: we alloc a RAM based pbuf for 
+     * the link level header for each chunk and then 
+     * free it.A PBUF_ROM style pbuf for which pbuf_header
+     * worked would make things simpler.
+     */
+    header = pbuf_alloc(PBUF_LINK, 0, PBUF_RAM);
+    if (header != NULL) {
+      pbuf_chain(header, rambuf);
+      netif->output(netif, header, dest);
+      IPFRAG_STATS_INC(ip_frag.xmit);
+      snmp_inc_ipfragcreates();
+      pbuf_free(header);
+    } else {
+      LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_frag: pbuf_alloc() for header failed\n"));
+      pbuf_free(rambuf);      
+      return ERR_MEM;    
+    }
+    left -= cop;
+  }
+  pbuf_free(rambuf);
+  snmp_inc_ipfragoks();
+  return ERR_OK;
+}

lwip/core/mem.c

@@ -0,0 +1,414 @@
+/** @file
+ *
+ * Dynamic memory manager
+ *
+ */
+
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include <string.h>
+
+#include "lwip/arch.h"
+#include "lwip/opt.h"
+#include "lwip/def.h"
+#include "lwip/mem.h"
+
+#include "lwip/sys.h"
+
+#include "lwip/stats.h"
+
+#if (MEM_LIBC_MALLOC == 0)
+/* lwIP replacement for your libc malloc() */
+
+struct mem {
+  mem_size_t next, prev;
+#if MEM_ALIGNMENT == 1
+  u8_t used;
+#elif MEM_ALIGNMENT == 2
+  u16_t used;
+#elif MEM_ALIGNMENT == 4
+  u32_t used;
+#elif MEM_ALIGNMENT == 8
+  u64_t used;
+#else
+#error "unhandled MEM_ALIGNMENT size"
+#endif /* MEM_ALIGNMENT */
+};
+
+static struct mem *ram_end;
+#if 1
+/* Adam original */
+static u8_t ram[MEM_SIZE + sizeof(struct mem) + MEM_ALIGNMENT];
+#else
+/* Christiaan alignment fix */
+static u8_t *ram;
+static struct mem ram_heap[1 + ( (MEM_SIZE + sizeof(struct mem) - 1) / sizeof(struct mem))];
+#endif
+
+#define MIN_SIZE 12
+#if 0 /* this one does not align correctly for some, resulting in crashes */
+#define SIZEOF_STRUCT_MEM (unsigned int)MEM_ALIGN_SIZE(sizeof(struct mem))
+#else
+#define SIZEOF_STRUCT_MEM (sizeof(struct mem) + \
+                          (((sizeof(struct mem) % MEM_ALIGNMENT) == 0)? 0 : \
+                          (4 - (sizeof(struct mem) % MEM_ALIGNMENT))))
+#endif
+
+static struct mem *lfree;   /* pointer to the lowest free block */
+
+static sys_sem_t mem_sem;
+
+static void
+plug_holes(struct mem *mem)
+{
+  struct mem *nmem;
+  struct mem *pmem;
+
+  LWIP_ASSERT("plug_holes: mem >= ram", (u8_t *)mem >= ram);
+  LWIP_ASSERT("plug_holes: mem < ram_end", (u8_t *)mem < (u8_t *)ram_end);
+  LWIP_ASSERT("plug_holes: mem->used == 0", mem->used == 0);
+
+  /* plug hole forward */
+  LWIP_ASSERT("plug_holes: mem->next <= MEM_SIZE", mem->next <= MEM_SIZE);
+
+  nmem = (struct mem *)&ram[mem->next];
+  if (mem != nmem && nmem->used == 0 && (u8_t *)nmem != (u8_t *)ram_end) {
+    if (lfree == nmem) {
+      lfree = mem;
+    }
+    mem->next = nmem->next;
+    ((struct mem *)&ram[nmem->next])->prev = (u8_t *)mem - ram;
+  }
+
+  /* plug hole backward */
+  pmem = (struct mem *)&ram[mem->prev];
+  if (pmem != mem && pmem->used == 0) {
+    if (lfree == mem) {
+      lfree = pmem;
+    }
+    pmem->next = mem->next;
+    ((struct mem *)&ram[mem->next])->prev = (u8_t *)pmem - ram;
+  }
+}
+
+void
+mem_init(void)
+{
+  struct mem *mem;
+
+#if 1
+  /* Adam original */
+#else
+  /* Christiaan alignment fix */
+  ram = (u8_t*)ram_heap;
+#endif
+  memset(ram, 0, MEM_SIZE);
+  mem = (struct mem *)ram;
+  mem->next = MEM_SIZE;
+  mem->prev = 0;
+  mem->used = 0;
+  ram_end = (struct mem *)&ram[MEM_SIZE];
+  ram_end->used = 1;
+  ram_end->next = MEM_SIZE;
+  ram_end->prev = MEM_SIZE;
+
+  mem_sem = sys_sem_new(1);
+
+  lfree = (struct mem *)ram;
+
+#if MEM_STATS
+  lwip_stats.mem.avail = MEM_SIZE;
+#endif /* MEM_STATS */
+}
+
+void
+mem_free(void *rmem)
+{
+  struct mem *mem;
+
+  if (rmem == NULL) {
+    LWIP_DEBUGF(MEM_DEBUG | DBG_TRACE | 2, ("mem_free(p == NULL) was called.\n"));
+    return;
+  }
+
+  sys_sem_wait(mem_sem);
+
+  LWIP_ASSERT("mem_free: legal memory", (u8_t *)rmem >= (u8_t *)ram &&
+    (u8_t *)rmem < (u8_t *)ram_end);
+
+  if ((u8_t *)rmem < (u8_t *)ram || (u8_t *)rmem >= (u8_t *)ram_end) {
+    LWIP_DEBUGF(MEM_DEBUG | 3, ("mem_free: illegal memory\n"));
+#if MEM_STATS
+    ++lwip_stats.mem.err;
+#endif /* MEM_STATS */
+    sys_sem_signal(mem_sem);
+    return;
+  }
+  mem = (struct mem *)((u8_t *)rmem - SIZEOF_STRUCT_MEM);
+
+  LWIP_ASSERT("mem_free: mem->used", mem->used);
+
+  mem->used = 0;
+
+  if (mem < lfree) {
+    lfree = mem;
+  }
+
+#if MEM_STATS
+  lwip_stats.mem.used -= mem->next - ((u8_t *)mem - ram);
+
+#endif /* MEM_STATS */
+  plug_holes(mem);
+  sys_sem_signal(mem_sem);
+}
+
+void *
+mem_realloc(void *rmem, mem_size_t newsize)
+{
+  mem_size_t size;
+  mem_size_t ptr, ptr2;
+  struct mem *mem, *mem2;
+
+  /* Expand the size of the allocated memory region so that we can
+     adjust for alignment. */
+  if ((newsize % MEM_ALIGNMENT) != 0) {
+   newsize += MEM_ALIGNMENT - ((newsize + SIZEOF_STRUCT_MEM) % MEM_ALIGNMENT);
+  }
+
+  if (newsize > MEM_SIZE) {
+    return NULL;
+  }
+
+  sys_sem_wait(mem_sem);
+
+  LWIP_ASSERT("mem_realloc: legal memory", (u8_t *)rmem >= (u8_t *)ram &&
+   (u8_t *)rmem < (u8_t *)ram_end);
+
+  if ((u8_t *)rmem < (u8_t *)ram || (u8_t *)rmem >= (u8_t *)ram_end) {
+    LWIP_DEBUGF(MEM_DEBUG | 3, ("mem_realloc: illegal memory\n"));
+    return rmem;
+  }
+  mem = (struct mem *)((u8_t *)rmem - SIZEOF_STRUCT_MEM);
+
+  ptr = (u8_t *)mem - ram;
+
+  size = mem->next - ptr - SIZEOF_STRUCT_MEM;
+#if MEM_STATS
+  lwip_stats.mem.used -= (size - newsize);
+#endif /* MEM_STATS */
+
+  if (newsize + SIZEOF_STRUCT_MEM + MIN_SIZE < size) {
+    ptr2 = ptr + SIZEOF_STRUCT_MEM + newsize;
+    mem2 = (struct mem *)&ram[ptr2];
+    mem2->used = 0;
+    mem2->next = mem->next;
+    mem2->prev = ptr;
+    mem->next = ptr2;
+    if (mem2->next != MEM_SIZE) {
+      ((struct mem *)&ram[mem2->next])->prev = ptr2;
+    }
+
+    plug_holes(mem2);
+  }
+  sys_sem_signal(mem_sem);
+  return rmem;
+}
+
+#if 1
+/**
+ * Adam's mem_malloc(), suffers from bug #17922
+ * Set if to 0 for alternative mem_malloc().
+ */
+void *
+mem_malloc(mem_size_t size)
+{
+  mem_size_t ptr, ptr2;
+  struct mem *mem, *mem2;
+
+  if (size == 0) {
+    return NULL;
+  }
+
+  /* Expand the size of the allocated memory region so that we can
+     adjust for alignment. */
+  if ((size % MEM_ALIGNMENT) != 0) {
+    size += MEM_ALIGNMENT - ((size + SIZEOF_STRUCT_MEM) % MEM_ALIGNMENT);
+  }
+
+  if (size > MEM_SIZE) {
+    return NULL;
+  }
+
+  sys_sem_wait(mem_sem);
+
+  for (ptr = (u8_t *)lfree - ram; ptr < MEM_SIZE; ptr = ((struct mem *)&ram[ptr])->next) {
+    mem = (struct mem *)&ram[ptr];
+    if (!mem->used &&
+       mem->next - (ptr + SIZEOF_STRUCT_MEM) >= size + SIZEOF_STRUCT_MEM) {
+      ptr2 = ptr + SIZEOF_STRUCT_MEM + size;
+      mem2 = (struct mem *)&ram[ptr2];
+
+      mem2->prev = ptr;
+      mem2->next = mem->next;
+      mem->next = ptr2;
+      if (mem2->next != MEM_SIZE) {
+        ((struct mem *)&ram[mem2->next])->prev = ptr2;
+      }
+
+      mem2->used = 0;
+      mem->used = 1;
+#if MEM_STATS
+      lwip_stats.mem.used += (size + SIZEOF_STRUCT_MEM);
+      /*      if (lwip_stats.mem.max < lwip_stats.mem.used) {
+        lwip_stats.mem.max = lwip_stats.mem.used;
+        } */
+      if (lwip_stats.mem.max < ptr2) {
+        lwip_stats.mem.max = ptr2;
+      }
+#endif /* MEM_STATS */
+
+      if (mem == lfree) {
+        /* Find next free block after mem */
+        while (lfree->used && lfree != ram_end) {
+          lfree = (struct mem *)&ram[lfree->next];
+        }
+        LWIP_ASSERT("mem_malloc: !lfree->used", !lfree->used);
+      }
+      sys_sem_signal(mem_sem);
+      LWIP_ASSERT("mem_malloc: allocated memory not above ram_end.",
+       (mem_ptr_t)mem + SIZEOF_STRUCT_MEM + size <= (mem_ptr_t)ram_end);
+      LWIP_ASSERT("mem_malloc: allocated memory properly aligned.",
+       (unsigned long)((u8_t *)mem + SIZEOF_STRUCT_MEM) % MEM_ALIGNMENT == 0);
+      return (u8_t *)mem + SIZEOF_STRUCT_MEM;
+    }
+  }
+  LWIP_DEBUGF(MEM_DEBUG | 2, ("mem_malloc: could not allocate %"S16_F" bytes\n", (s16_t)size));
+#if MEM_STATS
+  ++lwip_stats.mem.err;
+#endif /* MEM_STATS */
+  sys_sem_signal(mem_sem);
+  return NULL;
+}
+#else
+/**
+ * Adam's mem_malloc() plus solution for bug #17922
+ */
+void *
+mem_malloc(mem_size_t size)
+{
+  mem_size_t ptr, ptr2;
+  struct mem *mem, *mem2;
+
+  if (size == 0) {
+    return NULL;
+  }
+
+  /* Expand the size of the allocated memory region so that we can
+     adjust for alignment. */
+  if ((size % MEM_ALIGNMENT) != 0) {
+    size += MEM_ALIGNMENT - ((size + SIZEOF_STRUCT_MEM) % MEM_ALIGNMENT);
+  }
+
+  if (size > MEM_SIZE) {
+    return NULL;
+  }
+
+  sys_sem_wait(mem_sem);
+
+  for (ptr = (u8_t *)lfree - ram; ptr < MEM_SIZE - size; ptr = ((struct mem *)&ram[ptr])->next) {
+    mem = (struct mem *)&ram[ptr];
+
+    if (!mem->used) {
+
+      ptr2 = ptr + SIZEOF_STRUCT_MEM + size;
+
+      if (mem->next - (ptr + (2*SIZEOF_STRUCT_MEM)) >= size) {
+        /* split large block, create empty remainder */
+        mem->next = ptr2;
+        mem->used = 1;
+        /* create mem2 struct */
+        mem2 = (struct mem *)&ram[ptr2];
+        mem2->used = 0;
+        mem2->next = mem->next;
+        mem2->prev = ptr;
+
+        if (mem2->next != MEM_SIZE) {
+          ((struct mem *)&ram[mem2->next])->prev = ptr2;
+        }
+      }
+      else if (mem->next - (ptr + SIZEOF_STRUCT_MEM) > size) {
+        /* near fit, no split, no mem2 creation,
+           round up to mem->next */
+        ptr2 = mem->next;
+        mem->used = 1;
+      }
+      else if (mem->next - (ptr + SIZEOF_STRUCT_MEM) == size) {
+        /* exact fit, do not split, no mem2 creation */
+        mem->next = ptr2;
+        mem->used = 1;
+      }
+
+      if (mem->used) {
+#if MEM_STATS
+        lwip_stats.mem.used += (size + SIZEOF_STRUCT_MEM);
+        if (lwip_stats.mem.max < ptr2) {
+          lwip_stats.mem.max = ptr2;
+        }
+#endif /* MEM_STATS */
+        if (mem == lfree) {
+          /* Find next free block after mem */
+          while (lfree->used && lfree != ram_end) {
+            lfree = (struct mem *)&ram[lfree->next];
+          }
+          LWIP_ASSERT("mem_malloc: !lfree->used", !lfree->used);
+        }
+        sys_sem_signal(mem_sem);
+        LWIP_ASSERT("mem_malloc: allocated memory not above ram_end.",
+         (mem_ptr_t)mem + SIZEOF_STRUCT_MEM + size <= (mem_ptr_t)ram_end);
+        LWIP_ASSERT("mem_malloc: allocated memory properly aligned.",
+         (unsigned long)((u8_t *)mem + SIZEOF_STRUCT_MEM) % MEM_ALIGNMENT == 0);
+        return (u8_t *)mem + SIZEOF_STRUCT_MEM;
+      }
+    }
+  }
+  LWIP_DEBUGF(MEM_DEBUG | 2, ("mem_malloc: could not allocate %"S16_F" bytes\n", (s16_t)size));
+#if MEM_STATS
+  ++lwip_stats.mem.err;
+#endif /* MEM_STATS */
+  sys_sem_signal(mem_sem);
+  return NULL;
+}
+#endif
+
+#endif /* MEM_LIBC_MALLOC == 0 */
+

lwip/core/memp.c

@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved. 
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, 
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission. 
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ * 
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include "lwip/opt.h"
+
+#include "lwip/memp.h"
+
+#include "lwip/pbuf.h"
+#include "lwip/udp.h"
+#include "lwip/raw.h"
+#include "lwip/tcp.h"
+#include "lwip/api.h"
+#include "lwip/api_msg.h"
+#include "lwip/tcpip.h"
+
+#include "lwip/sys.h"
+#include "lwip/stats.h"
+
+struct memp {
+  struct memp *next;
+};
+
+#define MEMP_SIZE MEM_ALIGN_SIZE(sizeof(struct memp))
+
+static struct memp *memp_tab[MEMP_MAX];
+
+static const u16_t memp_sizes[MEMP_MAX] = {
+  MEM_ALIGN_SIZE(sizeof(struct pbuf)),
+  MEM_ALIGN_SIZE(sizeof(struct raw_pcb)),
+  MEM_ALIGN_SIZE(sizeof(struct udp_pcb)),
+  MEM_ALIGN_SIZE(sizeof(struct tcp_pcb)),
+  MEM_ALIGN_SIZE(sizeof(struct tcp_pcb_listen)),
+  MEM_ALIGN_SIZE(sizeof(struct tcp_seg)),
+  MEM_ALIGN_SIZE(sizeof(struct netbuf)),
+  MEM_ALIGN_SIZE(sizeof(struct netconn)),
+  MEM_ALIGN_SIZE(sizeof(struct api_msg)),
+  MEM_ALIGN_SIZE(sizeof(struct tcpip_msg)),
+  MEM_ALIGN_SIZE(sizeof(struct sys_timeo))
+};
+
+static const u16_t memp_num[MEMP_MAX] = {
+  MEMP_NUM_PBUF,
+  MEMP_NUM_RAW_PCB,
+  MEMP_NUM_UDP_PCB,
+  MEMP_NUM_TCP_PCB,
+  MEMP_NUM_TCP_PCB_LISTEN,
+  MEMP_NUM_TCP_SEG,
+  MEMP_NUM_NETBUF,
+  MEMP_NUM_NETCONN,
+  MEMP_NUM_API_MSG,
+  MEMP_NUM_TCPIP_MSG,
+  MEMP_NUM_SYS_TIMEOUT
+};
+
+#define MEMP_TYPE_SIZE(qty, type) \
+  ((qty) * (MEMP_SIZE + MEM_ALIGN_SIZE(sizeof(type))))
+
+static u8_t memp_memory[MEM_ALIGNMENT - 1 + 
+  MEMP_TYPE_SIZE(MEMP_NUM_PBUF, struct pbuf) +
+  MEMP_TYPE_SIZE(MEMP_NUM_RAW_PCB, struct raw_pcb) +
+  MEMP_TYPE_SIZE(MEMP_NUM_UDP_PCB, struct udp_pcb) +
+  MEMP_TYPE_SIZE(MEMP_NUM_TCP_PCB, struct tcp_pcb) +
+  MEMP_TYPE_SIZE(MEMP_NUM_TCP_PCB_LISTEN, struct tcp_pcb_listen) +
+  MEMP_TYPE_SIZE(MEMP_NUM_TCP_SEG, struct tcp_seg) +
+  MEMP_TYPE_SIZE(MEMP_NUM_NETBUF, struct netbuf) +
+  MEMP_TYPE_SIZE(MEMP_NUM_NETCONN, struct netconn) +
+  MEMP_TYPE_SIZE(MEMP_NUM_API_MSG, struct api_msg) +
+  MEMP_TYPE_SIZE(MEMP_NUM_TCPIP_MSG, struct tcpip_msg) +
+  MEMP_TYPE_SIZE(MEMP_NUM_SYS_TIMEOUT, struct sys_timeo)];
+
+#if !SYS_LIGHTWEIGHT_PROT
+static sys_sem_t mutex;
+#endif
+
+#if MEMP_SANITY_CHECK
+static int
+memp_sanity(void)
+{
+  s16_t i, c;
+  struct memp *m, *n;
+
+  for (i = 0; i < MEMP_MAX; i++) {
+    for (m = memp_tab[i]; m != NULL; m = m->next) {
+      c = 1;
+      for (n = memp_tab[i]; n != NULL; n = n->next) {
+        if (n == m && --c < 0) {
+          return 0; /* LW was: abort(); */
+        }
+      }
+    }
+  }
+  return 1;
+}
+#endif /* MEMP_SANITY_CHECK*/
+
+void
+memp_init(void)
+{
+  struct memp *memp;
+  u16_t i, j;
+
+#if MEMP_STATS
+  for (i = 0; i < MEMP_MAX; ++i) {
+    lwip_stats.memp[i].used = lwip_stats.memp[i].max =
+      lwip_stats.memp[i].err = 0;
+    lwip_stats.memp[i].avail = memp_num[i];
+  }
+#endif /* MEMP_STATS */
+
+  memp = MEM_ALIGN(memp_memory);
+  for (i = 0; i < MEMP_MAX; ++i) {
+    memp_tab[i] = NULL;
+    for (j = 0; j < memp_num[i]; ++j) {
+      memp->next = memp_tab[i];
+      memp_tab[i] = memp;
+      memp = (struct memp *)((u8_t *)memp + MEMP_SIZE + memp_sizes[i]);
+    }
+  }
+
+#if !SYS_LIGHTWEIGHT_PROT
+  mutex = sys_sem_new(1);
+#endif
+}
+
+void *
+memp_malloc(memp_t type)
+{
+  struct memp *memp;
+  void *mem;
+#if SYS_LIGHTWEIGHT_PROT
+  SYS_ARCH_DECL_PROTECT(old_level);
+#endif
+ 
+  LWIP_ASSERT("memp_malloc: type < MEMP_MAX", type < MEMP_MAX);
+
+#if SYS_LIGHTWEIGHT_PROT
+  SYS_ARCH_PROTECT(old_level);
+#else /* SYS_LIGHTWEIGHT_PROT */  
+  sys_sem_wait(mutex);
+#endif /* SYS_LIGHTWEIGHT_PROT */  
+
+  memp = memp_tab[type];
+  
+  if (memp != NULL) {    
+    memp_tab[type] = memp->next;    
+    memp->next = NULL;
+#if MEMP_STATS
+    ++lwip_stats.memp[type].used;
+    if (lwip_stats.memp[type].used > lwip_stats.memp[type].max) {
+      lwip_stats.memp[type].max = lwip_stats.memp[type].used;
+    }
+#endif /* MEMP_STATS */
+    mem = (u8_t *)memp + MEMP_SIZE;
+    LWIP_ASSERT("memp_malloc: memp properly aligned",
+                ((mem_ptr_t)memp % MEM_ALIGNMENT) == 0);
+  } else {
+    LWIP_DEBUGF(MEMP_DEBUG | 2, ("memp_malloc: out of memory in pool %"S16_F"\n", type));
+#if MEMP_STATS
+    ++lwip_stats.memp[type].err;
+#endif /* MEMP_STATS */
+    mem = NULL;
+  }
+
+#if SYS_LIGHTWEIGHT_PROT
+  SYS_ARCH_UNPROTECT(old_level);
+#else /* SYS_LIGHTWEIGHT_PROT */
+  sys_sem_signal(mutex);
+#endif /* SYS_LIGHTWEIGHT_PROT */  
+
+  return mem;
+}
+
+void
+memp_free(memp_t type, void *mem)
+{
+  struct memp *memp;
+#if SYS_LIGHTWEIGHT_PROT
+  SYS_ARCH_DECL_PROTECT(old_level);
+#endif /* SYS_LIGHTWEIGHT_PROT */  
+
+  if (mem == NULL) {
+    return;
+  }
+
+  memp = (struct memp *)((u8_t *)mem - MEMP_SIZE);
+
+#if SYS_LIGHTWEIGHT_PROT
+  SYS_ARCH_PROTECT(old_level);
+#else /* SYS_LIGHTWEIGHT_PROT */  
+  sys_sem_wait(mutex);
+#endif /* SYS_LIGHTWEIGHT_PROT */  
+
+#if MEMP_STATS
+  lwip_stats.memp[type].used--; 
+#endif /* MEMP_STATS */
+  
+  memp->next = memp_tab[type]; 
+  memp_tab[type] = memp;
+
+#if MEMP_SANITY_CHECK
+  LWIP_ASSERT("memp sanity", memp_sanity());
+#endif  
+
+#if SYS_LIGHTWEIGHT_PROT
+  SYS_ARCH_UNPROTECT(old_level);
+#else /* SYS_LIGHTWEIGHT_PROT */
+  sys_sem_signal(mutex);
+#endif /* SYS_LIGHTWEIGHT_PROT */  
+}

lwip/core/netif.c

@@ -0,0 +1,325 @@
+/**
+ * @file
+ *
+ * lwIP network interface abstraction
+ */
+
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include "lwip/opt.h"
+
+#include "lwip/def.h"
+#include "lwip/ip_addr.h"
+#include "lwip/netif.h"
+#include "lwip/tcp.h"
+#include "lwip/snmp.h"
+
+struct netif *netif_list = NULL;
+struct netif *netif_default = NULL;
+
+/**
+ * Add a network interface to the list of lwIP netifs.
+ *
+ * @param netif a pre-allocated netif structure
+ * @param ipaddr IP address for the new netif
+ * @param netmask network mask for the new netif
+ * @param gw default gateway IP address for the new netif
+ * @param state opaque data passed to the new netif
+ * @param init callback function that initializes the interface
+ * @param input callback function that is called to pass
+ * ingress packets up in the protocol layer stack.
+ *
+ * @return netif, or NULL if failed.
+ */
+struct netif *
+netif_add(struct netif *netif, struct ip_addr *ipaddr, struct ip_addr *netmask,
+  struct ip_addr *gw,
+  void *state,
+  err_t (* init)(struct netif *netif),
+  err_t (* input)(struct pbuf *p, struct netif *netif))
+{
+  static s16_t netifnum = 0;
+
+  /* reset new interface configuration state */
+  netif->ip_addr.addr = 0;
+  netif->netmask.addr = 0;
+  netif->gw.addr = 0;
+  netif->flags = 0;
+#if LWIP_DHCP
+  /* netif not under DHCP control by default */
+  netif->dhcp = NULL;
+#endif
+  /* remember netif specific state information data */
+  netif->state = state;
+  netif->num = netifnum++;
+  netif->input = input;
+
+  netif_set_addr(netif, ipaddr, netmask, gw);
+
+  /* call user specified initialization function for netif */
+  if (init(netif) != ERR_OK) {
+    return NULL;
+  }
+
+  /* add this netif to the list */
+  netif->next = netif_list;
+  netif_list = netif;
+  snmp_inc_iflist();
+
+  LWIP_DEBUGF(NETIF_DEBUG, ("netif: added interface %c%c IP addr ",
+    netif->name[0], netif->name[1]));
+  ip_addr_debug_print(NETIF_DEBUG, ipaddr);
+  LWIP_DEBUGF(NETIF_DEBUG, (" netmask "));
+  ip_addr_debug_print(NETIF_DEBUG, netmask);
+  LWIP_DEBUGF(NETIF_DEBUG, (" gw "));
+  ip_addr_debug_print(NETIF_DEBUG, gw);
+  LWIP_DEBUGF(NETIF_DEBUG, ("\n"));
+  return netif;
+}
+
+void
+netif_set_addr(struct netif *netif,struct ip_addr *ipaddr, struct ip_addr *netmask,
+    struct ip_addr *gw)
+{
+  netif_set_ipaddr(netif, ipaddr);
+  netif_set_netmask(netif, netmask);
+  netif_set_gw(netif, gw);
+}
+
+void netif_remove(struct netif * netif)
+{
+  if ( netif == NULL ) return;
+
+  snmp_delete_ipaddridx_tree(netif);
+
+  /*  is it the first netif? */
+  if (netif_list == netif) {
+    netif_list = netif->next;
+    snmp_dec_iflist();
+  }
+  else {
+    /*  look for netif further down the list */
+    struct netif * tmpNetif;
+    for (tmpNetif = netif_list; tmpNetif != NULL; tmpNetif = tmpNetif->next) {
+      if (tmpNetif->next == netif) {
+        tmpNetif->next = netif->next;
+        snmp_dec_iflist();
+        break;
+      }
+    }
+    if (tmpNetif == NULL)
+      return; /*  we didn't find any netif today */
+  }
+  /* this netif is default? */
+  if (netif_default == netif)
+    /* reset default netif */
+    netif_default = NULL;
+  LWIP_DEBUGF( NETIF_DEBUG, ("netif_remove: removed netif\n") );
+}
+
+struct netif *
+netif_find(char *name)
+{
+  struct netif *netif;
+  u8_t num;
+
+  if (name == NULL) {
+    return NULL;
+  }
+
+  num = name[2] - '0';
+
+  for(netif = netif_list; netif != NULL; netif = netif->next) {
+    if (num == netif->num &&
+       name[0] == netif->name[0] &&
+       name[1] == netif->name[1]) {
+      LWIP_DEBUGF(NETIF_DEBUG, ("netif_find: found %c%c\n", name[0], name[1]));
+      return netif;
+    }
+  }
+  LWIP_DEBUGF(NETIF_DEBUG, ("netif_find: didn't find %c%c\n", name[0], name[1]));
+  return NULL;
+}
+
+void
+netif_set_ipaddr(struct netif *netif, struct ip_addr *ipaddr)
+{
+  /* TODO: Handling of obsolete pcbs */
+  /* See:  http://mail.gnu.org/archive/html/lwip-users/2003-03/msg00118.html */
+#if LWIP_TCP
+  struct tcp_pcb *pcb;
+  struct tcp_pcb_listen *lpcb;
+
+  /* address is actually being changed? */
+  if ((ip_addr_cmp(ipaddr, &(netif->ip_addr))) == 0)
+  {
+    /* extern struct tcp_pcb *tcp_active_pcbs; defined by tcp.h */
+    LWIP_DEBUGF(NETIF_DEBUG | 1, ("netif_set_ipaddr: netif address being changed\n"));
+    pcb = tcp_active_pcbs;
+    while (pcb != NULL) {
+      /* PCB bound to current local interface address? */
+      if (ip_addr_cmp(&(pcb->local_ip), &(netif->ip_addr))) {
+        /* this connection must be aborted */
+        struct tcp_pcb *next = pcb->next;
+        LWIP_DEBUGF(NETIF_DEBUG | 1, ("netif_set_ipaddr: aborting TCP pcb %p\n", (void *)pcb));
+        tcp_abort(pcb);
+        pcb = next;
+      } else {
+        pcb = pcb->next;
+      }
+    }
+    for (lpcb = tcp_listen_pcbs.listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
+      /* PCB bound to current local interface address? */
+      if (ip_addr_cmp(&(lpcb->local_ip), &(netif->ip_addr))) {
+        /* The PCB is listening to the old ipaddr and
+         * is set to listen to the new one instead */
+        ip_addr_set(&(lpcb->local_ip), ipaddr);
+      }
+    }
+  }
+#endif
+  snmp_delete_ipaddridx_tree(netif);
+  snmp_delete_iprteidx_tree(0,netif);
+  /* set new IP address to netif */
+  ip_addr_set(&(netif->ip_addr), ipaddr);
+  snmp_insert_ipaddridx_tree(netif);
+  snmp_insert_iprteidx_tree(0,netif);
+
+#if 0 /* only allowed for Ethernet interfaces TODO: how can we check? */
+  /** For Ethernet network interfaces, we would like to send a
+   *  "gratuitous ARP"; this is an ARP packet sent by a node in order
+   *  to spontaneously cause other nodes to update an entry in their
+   *  ARP cache. From RFC 3220 "IP Mobility Support for IPv4" section 4.6.
+   */ 
+  etharp_query(netif, ipaddr, NULL);
+#endif
+  LWIP_DEBUGF(NETIF_DEBUG | DBG_TRACE | DBG_STATE | 3, ("netif: IP address of interface %c%c set to %"U16_F".%"U16_F".%"U16_F".%"U16_F"\n",
+    netif->name[0], netif->name[1],
+    ip4_addr1(&netif->ip_addr),
+    ip4_addr2(&netif->ip_addr),
+    ip4_addr3(&netif->ip_addr),
+    ip4_addr4(&netif->ip_addr)));
+}
+
+void
+netif_set_gw(struct netif *netif, struct ip_addr *gw)
+{
+  ip_addr_set(&(netif->gw), gw);
+  LWIP_DEBUGF(NETIF_DEBUG | DBG_TRACE | DBG_STATE | 3, ("netif: GW address of interface %c%c set to %"U16_F".%"U16_F".%"U16_F".%"U16_F"\n",
+    netif->name[0], netif->name[1],
+    ip4_addr1(&netif->gw),
+    ip4_addr2(&netif->gw),
+    ip4_addr3(&netif->gw),
+    ip4_addr4(&netif->gw)));
+}
+
+void
+netif_set_netmask(struct netif *netif, struct ip_addr *netmask)
+{
+  snmp_delete_iprteidx_tree(0, netif);
+  /* set new netmask to netif */
+  ip_addr_set(&(netif->netmask), netmask);
+  snmp_insert_iprteidx_tree(0, netif);
+  LWIP_DEBUGF(NETIF_DEBUG | DBG_TRACE | DBG_STATE | 3, ("netif: netmask of interface %c%c set to %"U16_F".%"U16_F".%"U16_F".%"U16_F"\n",
+    netif->name[0], netif->name[1],
+    ip4_addr1(&netif->netmask),
+    ip4_addr2(&netif->netmask),
+    ip4_addr3(&netif->netmask),
+    ip4_addr4(&netif->netmask)));
+}
+
+void
+netif_set_default(struct netif *netif)
+{
+  if (netif == NULL)
+  {
+    /* remove default route */
+    snmp_delete_iprteidx_tree(1, netif);
+  }
+  else
+  {
+    /* install default route */
+    snmp_insert_iprteidx_tree(1, netif);
+  }
+  netif_default = netif;
+  LWIP_DEBUGF(NETIF_DEBUG, ("netif: setting default interface %c%c\n",
+           netif ? netif->name[0] : '\'', netif ? netif->name[1] : '\''));
+}
+
+/**
+ * Bring an interface up, available for processing
+ * traffic.
+ * 
+ * @note: Enabling DHCP on a down interface will make it come
+ * up once configured.
+ * 
+ * @see dhcp_start()
+ */ 
+void netif_set_up(struct netif *netif)
+{
+  netif->flags |= NETIF_FLAG_UP;
+#if LWIP_SNMP
+  snmp_get_sysuptime(&netif->ts);
+#endif
+}
+
+/**
+ * Ask if an interface is up
+ */ 
+u8_t netif_is_up(struct netif *netif)
+{
+  return (netif->flags & NETIF_FLAG_UP)?1:0;
+}
+
+/**
+ * Bring an interface down, disabling any traffic processing.
+ *
+ * @note: Enabling DHCP on a down interface will make it come
+ * up once configured.
+ * 
+ * @see dhcp_start()
+ */ 
+void netif_set_down(struct netif *netif)
+{
+  netif->flags &= ~NETIF_FLAG_UP;
+#if LWIP_SNMP
+  snmp_get_sysuptime(&netif->ts);
+#endif
+}
+
+void
+netif_init(void)
+{
+  netif_list = netif_default = NULL;
+}
+

lwip/core/pbuf.c

@@ -0,0 +1,964 @@
+/**
+ * @file
+ * Packet buffer management
+ *
+ * Packets are built from the pbuf data structure. It supports dynamic
+ * memory allocation for packet contents or can reference externally
+ * managed packet contents both in RAM and ROM. Quick allocation for
+ * incoming packets is provided through pools with fixed sized pbufs.
+ *
+ * A packet may span over multiple pbufs, chained as a singly linked
+ * list. This is called a "pbuf chain".
+ *
+ * Multiple packets may be queued, also using this singly linked list.
+ * This is called a "packet queue".
+ * 
+ * So, a packet queue consists of one or more pbuf chains, each of
+ * which consist of one or more pbufs. Currently, queues are only
+ * supported in a limited section of lwIP, this is the etharp queueing
+ * code. Outside of this section no packet queues are supported yet.
+ * 
+ * The differences between a pbuf chain and a packet queue are very
+ * precise but subtle. 
+ *
+ * The last pbuf of a packet has a ->tot_len field that equals the
+ * ->len field. It can be found by traversing the list. If the last
+ * pbuf of a packet has a ->next field other than NULL, more packets
+ * are on the queue.
+ *
+ * Therefore, looping through a pbuf of a single packet, has an
+ * loop end condition (tot_len == p->len), NOT (next == NULL).
+ */
+
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include <string.h>
+
+#include "lwip/opt.h"
+#include "lwip/stats.h"
+#include "lwip/def.h"
+#include "lwip/mem.h"
+#include "lwip/memp.h"
+#include "lwip/pbuf.h"
+#include "lwip/sys.h"
+#include "arch/perf.h"
+
+static u8_t pbuf_pool_memory[MEM_ALIGNMENT - 1 + PBUF_POOL_SIZE * MEM_ALIGN_SIZE(PBUF_POOL_BUFSIZE + sizeof(struct pbuf))];
+
+#if !SYS_LIGHTWEIGHT_PROT
+static volatile u8_t pbuf_pool_free_lock, pbuf_pool_alloc_lock;
+static sys_sem_t pbuf_pool_free_sem;
+#endif
+
+static struct pbuf *pbuf_pool = NULL;
+
+/**
+ * Initializes the pbuf module.
+ *
+ * A large part of memory is allocated for holding the pool of pbufs.
+ * The size of the individual pbufs in the pool is given by the size
+ * parameter, and the number of pbufs in the pool by the num parameter.
+ *
+ * After the memory has been allocated, the pbufs are set up. The
+ * ->next pointer in each pbuf is set up to point to the next pbuf in
+ * the pool.
+ *
+ */
+void
+pbuf_init(void)
+{
+  struct pbuf *p, *q = NULL;
+  u16_t i;
+
+  pbuf_pool = (struct pbuf *)MEM_ALIGN(pbuf_pool_memory);
+
+#if PBUF_STATS
+  lwip_stats.pbuf.avail = PBUF_POOL_SIZE;
+#endif /* PBUF_STATS */
+
+  /* Set up ->next pointers to link the pbufs of the pool together */
+  p = pbuf_pool;
+
+  for(i = 0; i < PBUF_POOL_SIZE; ++i) {
+    p->next = (struct pbuf *)((u8_t *)p + PBUF_POOL_BUFSIZE + sizeof(struct pbuf));
+    p->len = p->tot_len = PBUF_POOL_BUFSIZE;
+    p->payload = MEM_ALIGN((void *)((u8_t *)p + sizeof(struct pbuf)));
+    p->flags = PBUF_FLAG_POOL;
+    q = p;
+    p = p->next;
+  }
+
+  /* The ->next pointer of last pbuf is NULL to indicate that there
+     are no more pbufs in the pool */
+  q->next = NULL;
+
+#if !SYS_LIGHTWEIGHT_PROT
+  pbuf_pool_alloc_lock = 0;
+  pbuf_pool_free_lock = 0;
+  pbuf_pool_free_sem = sys_sem_new(1);
+#endif
+}
+
+/**
+ * @internal only called from pbuf_alloc()
+ */
+static struct pbuf *
+pbuf_pool_alloc(void)
+{
+  struct pbuf *p = NULL;
+
+  SYS_ARCH_DECL_PROTECT(old_level);
+  SYS_ARCH_PROTECT(old_level);
+
+#if !SYS_LIGHTWEIGHT_PROT
+  /* Next, check the actual pbuf pool, but if the pool is locked, we
+     pretend to be out of buffers and return NULL. */
+  if (pbuf_pool_free_lock) {
+#if PBUF_STATS
+    ++lwip_stats.pbuf.alloc_locked;
+#endif /* PBUF_STATS */
+    return NULL;
+  }
+  pbuf_pool_alloc_lock = 1;
+  if (!pbuf_pool_free_lock) {
+#endif /* SYS_LIGHTWEIGHT_PROT */
+    p = pbuf_pool;
+    if (p) {
+      pbuf_pool = p->next;
+    }
+#if !SYS_LIGHTWEIGHT_PROT
+#if PBUF_STATS
+  } else {
+    ++lwip_stats.pbuf.alloc_locked;
+#endif /* PBUF_STATS */
+  }
+  pbuf_pool_alloc_lock = 0;
+#endif /* SYS_LIGHTWEIGHT_PROT */
+
+#if PBUF_STATS
+  if (p != NULL) {
+    ++lwip_stats.pbuf.used;
+    if (lwip_stats.pbuf.used > lwip_stats.pbuf.max) {
+      lwip_stats.pbuf.max = lwip_stats.pbuf.used;
+    }
+  }
+#endif /* PBUF_STATS */
+
+  SYS_ARCH_UNPROTECT(old_level);
+  return p;
+}
+
+
+/**
+ * Allocates a pbuf of the given type (possibly a chain for PBUF_POOL type).
+ *
+ * The actual memory allocated for the pbuf is determined by the
+ * layer at which the pbuf is allocated and the requested size
+ * (from the size parameter).
+ *
+ * @param flag this parameter decides how and where the pbuf
+ * should be allocated as follows:
+ *
+ * - PBUF_RAM: buffer memory for pbuf is allocated as one large
+ *             chunk. This includes protocol headers as well.
+ * - PBUF_ROM: no buffer memory is allocated for the pbuf, even for
+ *             protocol headers. Additional headers must be prepended
+ *             by allocating another pbuf and chain in to the front of
+ *             the ROM pbuf. It is assumed that the memory used is really
+ *             similar to ROM in that it is immutable and will not be
+ *             changed. Memory which is dynamic should generally not
+ *             be attached to PBUF_ROM pbufs. Use PBUF_REF instead.
+ * - PBUF_REF: no buffer memory is allocated for the pbuf, even for
+ *             protocol headers. It is assumed that the pbuf is only
+ *             being used in a single thread. If the pbuf gets queued,
+ *             then pbuf_take should be called to copy the buffer.
+ * - PBUF_POOL: the pbuf is allocated as a pbuf chain, with pbufs from
+ *              the pbuf pool that is allocated during pbuf_init().
+ *
+ * @return the allocated pbuf. If multiple pbufs where allocated, this
+ * is the first pbuf of a pbuf chain.
+ */
+struct pbuf *
+pbuf_alloc(pbuf_layer l, u16_t length, pbuf_flag flag)
+{
+  struct pbuf *p, *q, *r;
+  u16_t offset;
+  s32_t rem_len; /* remaining length */
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 3, ("pbuf_alloc(length=%"U16_F")\n", length));
+
+  /* determine header offset */
+  offset = 0;
+  switch (l) {
+  case PBUF_TRANSPORT:
+    /* add room for transport (often TCP) layer header */
+    offset += PBUF_TRANSPORT_HLEN;
+    /* FALLTHROUGH */
+  case PBUF_IP:
+    /* add room for IP layer header */
+    offset += PBUF_IP_HLEN;
+    /* FALLTHROUGH */
+  case PBUF_LINK:
+    /* add room for link layer header */
+    offset += PBUF_LINK_HLEN;
+    break;
+  case PBUF_RAW:
+    break;
+  default:
+    LWIP_ASSERT("pbuf_alloc: bad pbuf layer", 0);
+    return NULL;
+  }
+
+  switch (flag) {
+  case PBUF_POOL:
+    /* allocate head of pbuf chain into p */
+    p = pbuf_pool_alloc();
+    LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 3, ("pbuf_alloc: allocated pbuf %p\n", (void *)p));
+    if (p == NULL) {
+#if PBUF_STATS
+      ++lwip_stats.pbuf.err;
+#endif /* PBUF_STATS */
+      return NULL;
+    }
+    p->next = NULL;
+
+    /* make the payload pointer point 'offset' bytes into pbuf data memory */
+    p->payload = MEM_ALIGN((void *)((u8_t *)p + (sizeof(struct pbuf) + offset)));
+    LWIP_ASSERT("pbuf_alloc: pbuf p->payload properly aligned",
+            ((mem_ptr_t)p->payload % MEM_ALIGNMENT) == 0);
+    /* the total length of the pbuf chain is the requested size */
+    p->tot_len = length;
+    /* set the length of the first pbuf in the chain */
+    p->len = length > PBUF_POOL_BUFSIZE - offset? PBUF_POOL_BUFSIZE - offset: length;
+    /* set reference count (needed here in case we fail) */
+    p->ref = 1;
+
+    /* now allocate the tail of the pbuf chain */
+
+    /* remember first pbuf for linkage in next iteration */
+    r = p;
+    /* remaining length to be allocated */
+    rem_len = length - p->len;
+    /* any remaining pbufs to be allocated? */
+    while (rem_len > 0) {
+      q = pbuf_pool_alloc();
+      if (q == NULL) {
+       LWIP_DEBUGF(PBUF_DEBUG | 2, ("pbuf_alloc: Out of pbufs in pool.\n"));
+#if PBUF_STATS
+        ++lwip_stats.pbuf.err;
+#endif /* PBUF_STATS */
+        /* free chain so far allocated */
+        pbuf_free(p);
+        /* bail out unsuccesfully */
+        return NULL;
+      }
+      q->next = NULL;
+      /* make previous pbuf point to this pbuf */
+      r->next = q;
+      /* set total length of this pbuf and next in chain */
+      q->tot_len = rem_len;
+      /* this pbuf length is pool size, unless smaller sized tail */
+      q->len = rem_len > PBUF_POOL_BUFSIZE? PBUF_POOL_BUFSIZE: rem_len;
+      q->payload = (void *)((u8_t *)q + sizeof(struct pbuf));
+      LWIP_ASSERT("pbuf_alloc: pbuf q->payload properly aligned",
+              ((mem_ptr_t)q->payload % MEM_ALIGNMENT) == 0);
+      q->ref = 1;
+      /* calculate remaining length to be allocated */
+      rem_len -= q->len;
+      /* remember this pbuf for linkage in next iteration */
+      r = q;
+    }
+    /* end of chain */
+    /*r->next = NULL;*/
+
+    break;
+  case PBUF_RAM:
+    /* If pbuf is to be allocated in RAM, allocate memory for it. */
+    p = mem_malloc(MEM_ALIGN_SIZE(sizeof(struct pbuf) + offset) + MEM_ALIGN_SIZE(length));
+    if (p == NULL) {
+      return NULL;
+    }
+    /* Set up internal structure of the pbuf. */
+    p->payload = MEM_ALIGN((void *)((u8_t *)p + sizeof(struct pbuf) + offset));
+    p->len = p->tot_len = length;
+    p->next = NULL;
+    p->flags = PBUF_FLAG_RAM;
+
+    LWIP_ASSERT("pbuf_alloc: pbuf->payload properly aligned",
+           ((mem_ptr_t)p->payload % MEM_ALIGNMENT) == 0);
+    break;
+  /* pbuf references existing (non-volatile static constant) ROM payload? */
+  case PBUF_ROM:
+  /* pbuf references existing (externally allocated) RAM payload? */
+  case PBUF_REF:
+    /* only allocate memory for the pbuf structure */
+    p = memp_malloc(MEMP_PBUF);
+    if (p == NULL) {
+      LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 2, ("pbuf_alloc: Could not allocate MEMP_PBUF for PBUF_%s.\n", flag == PBUF_ROM?"ROM":"REF"));
+
+      return NULL;
+    }
+    /* caller must set this field properly, afterwards */
+    p->payload = NULL;
+    p->len = p->tot_len = length;
+    p->next = NULL;
+    p->flags = (flag == PBUF_ROM? PBUF_FLAG_ROM: PBUF_FLAG_REF);
+    break;
+  default:
+    LWIP_ASSERT("pbuf_alloc: erroneous flag", 0);
+    return NULL;
+  }
+  /* set reference count */
+  p->ref = 1;
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 3, ("pbuf_alloc(length=%"U16_F") == %p\n", length, (void *)p));
+  return p;
+}
+
+
+#if PBUF_STATS
+#define DEC_PBUF_STATS do { --lwip_stats.pbuf.used; } while (0)
+#else /* PBUF_STATS */
+#define DEC_PBUF_STATS
+#endif /* PBUF_STATS */
+
+#define PBUF_POOL_FAST_FREE(p)  do {                                    \
+                                  p->next = pbuf_pool;                  \
+                                  pbuf_pool = p;                        \
+                                  DEC_PBUF_STATS;                       \
+                                } while (0)
+
+#if SYS_LIGHTWEIGHT_PROT
+#define PBUF_POOL_FREE(p)  do {                                         \
+                                SYS_ARCH_DECL_PROTECT(old_level);       \
+                                SYS_ARCH_PROTECT(old_level);            \
+                                PBUF_POOL_FAST_FREE(p);                 \
+                                SYS_ARCH_UNPROTECT(old_level);          \
+                               } while (0)
+#else /* SYS_LIGHTWEIGHT_PROT */
+#define PBUF_POOL_FREE(p)  do {                                         \
+                             sys_sem_wait(pbuf_pool_free_sem);          \
+                             PBUF_POOL_FAST_FREE(p);                    \
+                             sys_sem_signal(pbuf_pool_free_sem);        \
+                           } while (0)
+#endif /* SYS_LIGHTWEIGHT_PROT */
+
+/**
+ * Shrink a pbuf chain to a desired length.
+ *
+ * @param p pbuf to shrink.
+ * @param new_len desired new length of pbuf chain
+ *
+ * Depending on the desired length, the first few pbufs in a chain might
+ * be skipped and left unchanged. The new last pbuf in the chain will be
+ * resized, and any remaining pbufs will be freed.
+ *
+ * @note If the pbuf is ROM/REF, only the ->tot_len and ->len fields are adjusted.
+ * @note May not be called on a packet queue.
+ *
+ * @bug Cannot grow the size of a pbuf (chain) (yet).
+ */
+void
+pbuf_realloc(struct pbuf *p, u16_t new_len)
+{
+  struct pbuf *q;
+  u16_t rem_len; /* remaining length */
+  s16_t grow;
+
+  LWIP_ASSERT("pbuf_realloc: sane p->flags", p->flags == PBUF_FLAG_POOL ||
+              p->flags == PBUF_FLAG_ROM ||
+              p->flags == PBUF_FLAG_RAM ||
+              p->flags == PBUF_FLAG_REF);
+
+  /* desired length larger than current length? */
+  if (new_len >= p->tot_len) {
+    /* enlarging not yet supported */
+    return;
+  }
+
+  /* the pbuf chain grows by (new_len - p->tot_len) bytes
+   * (which may be negative in case of shrinking) */
+  grow = new_len - p->tot_len;
+
+  /* first, step over any pbufs that should remain in the chain */
+  rem_len = new_len;
+  q = p;
+  /* should this pbuf be kept? */
+  while (rem_len > q->len) {
+    /* decrease remaining length by pbuf length */
+    rem_len -= q->len;
+    /* decrease total length indicator */
+    q->tot_len += grow;
+    /* proceed to next pbuf in chain */
+    q = q->next;
+  }
+  /* we have now reached the new last pbuf (in q) */
+  /* rem_len == desired length for pbuf q */
+
+  /* shrink allocated memory for PBUF_RAM */
+  /* (other types merely adjust their length fields */
+  if ((q->flags == PBUF_FLAG_RAM) && (rem_len != q->len)) {
+    /* reallocate and adjust the length of the pbuf that will be split */
+    mem_realloc(q, (u8_t *)q->payload - (u8_t *)q + rem_len);
+  }
+  /* adjust length fields for new last pbuf */
+  q->len = rem_len;
+  q->tot_len = q->len;
+
+  /* any remaining pbufs in chain? */
+  if (q->next != NULL) {
+    /* free remaining pbufs in chain */
+    pbuf_free(q->next);
+  }
+  /* q is last packet in chain */
+  q->next = NULL;
+
+}
+
+/**
+ * Adjusts the payload pointer to hide or reveal headers in the payload.
+ *
+ * Adjusts the ->payload pointer so that space for a header
+ * (dis)appears in the pbuf payload.
+ *
+ * The ->payload, ->tot_len and ->len fields are adjusted.
+ *
+ * @param hdr_size_inc Number of bytes to increment header size which
+ * increases the size of the pbuf. New space is on the front.
+ * (Using a negative value decreases the header size.)
+ * If hdr_size_inc is 0, this function does nothing and returns succesful.
+ *
+ * PBUF_ROM and PBUF_REF type buffers cannot have their sizes increased, so
+ * the call will fail. A check is made that the increase in header size does
+ * not move the payload pointer in front of the start of the buffer.
+ * @return non-zero on failure, zero on success.
+ *
+ */
+u8_t
+pbuf_header(struct pbuf *p, s16_t header_size_increment)
+{
+  u16_t flags;
+  void *payload;
+
+  LWIP_ASSERT("p != NULL", p != NULL);
+  if ((header_size_increment == 0) || (p == NULL)) return 0;
+ 
+  flags = p->flags;
+  /* remember current payload pointer */
+  payload = p->payload;
+
+  /* pbuf types containing payloads? */
+  if (flags == PBUF_FLAG_RAM || flags == PBUF_FLAG_POOL) {
+    /* set new payload pointer */
+    p->payload = (u8_t *)p->payload - header_size_increment;
+    /* boundary check fails? */
+    if ((u8_t *)p->payload < (u8_t *)p + sizeof(struct pbuf)) {
+      LWIP_DEBUGF( PBUF_DEBUG | 2, ("pbuf_header: failed as %p < %p (not enough space for new header size)\n",
+        (void *)p->payload,
+        (void *)(p + 1)));\
+      /* restore old payload pointer */
+      p->payload = payload;
+      /* bail out unsuccesfully */
+      return 1;
+    }
+  /* pbuf types refering to external payloads? */
+  } else if (flags == PBUF_FLAG_REF || flags == PBUF_FLAG_ROM) {
+    /* hide a header in the payload? */
+    if ((header_size_increment < 0) && (header_size_increment - p->len <= 0)) {
+      /* increase payload pointer */
+      p->payload = (u8_t *)p->payload - header_size_increment;
+    } else {
+      /* cannot expand payload to front (yet!)
+       * bail out unsuccesfully */
+      return 1;
+    }
+  }
+  /* modify pbuf length fields */
+  p->len += header_size_increment;
+  p->tot_len += header_size_increment;
+
+  LWIP_DEBUGF( PBUF_DEBUG, ("pbuf_header: old %p new %p (%"S16_F")\n",
+    (void *)payload, (void *)p->payload, header_size_increment));
+
+  return 0;
+}
+
+/**
+ * Dereference a pbuf chain or queue and deallocate any no-longer-used
+ * pbufs at the head of this chain or queue.
+ *
+ * Decrements the pbuf reference count. If it reaches zero, the pbuf is
+ * deallocated.
+ *
+ * For a pbuf chain, this is repeated for each pbuf in the chain,
+ * up to the first pbuf which has a non-zero reference count after
+ * decrementing. So, when all reference counts are one, the whole
+ * chain is free'd.
+ *
+ * @param pbuf The pbuf (chain) to be dereferenced.
+ *
+ * @return the number of pbufs that were de-allocated
+ * from the head of the chain.
+ *
+ * @note MUST NOT be called on a packet queue (Not verified to work yet).
+ * @note the reference counter of a pbuf equals the number of pointers
+ * that refer to the pbuf (or into the pbuf).
+ *
+ * @internal examples:
+ *
+ * Assuming existing chains a->b->c with the following reference
+ * counts, calling pbuf_free(a) results in:
+ * 
+ * 1->2->3 becomes ...1->3
+ * 3->3->3 becomes 2->3->3
+ * 1->1->2 becomes ......1
+ * 2->1->1 becomes 1->1->1
+ * 1->1->1 becomes .......
+ *
+ */
+u8_t
+pbuf_free(struct pbuf *p)
+{
+  u16_t flags;
+  struct pbuf *q;
+  u8_t count;
+  SYS_ARCH_DECL_PROTECT(old_level);
+
+  LWIP_ASSERT("p != NULL", p != NULL);
+
+  /* if assertions are disabled, proceed with debug output */
+  if (p == NULL) {
+    LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 2, ("pbuf_free(p == NULL) was called.\n"));
+    return 0;
+  }
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 3, ("pbuf_free(%p)\n", (void *)p));
+
+  PERF_START;
+
+  LWIP_ASSERT("pbuf_free: sane flags",
+    p->flags == PBUF_FLAG_RAM || p->flags == PBUF_FLAG_ROM ||
+    p->flags == PBUF_FLAG_REF || p->flags == PBUF_FLAG_POOL);
+
+  count = 0;
+  /* Since decrementing ref cannot be guaranteed to be a single machine operation
+   * we must protect it. Also, the later test of ref must be protected.
+   */
+  SYS_ARCH_PROTECT(old_level);
+  /* de-allocate all consecutive pbufs from the head of the chain that
+   * obtain a zero reference count after decrementing*/
+  while (p != NULL) {
+    /* all pbufs in a chain are referenced at least once */
+    LWIP_ASSERT("pbuf_free: p->ref > 0", p->ref > 0);
+    /* decrease reference count (number of pointers to pbuf) */
+    p->ref--;
+    /* this pbuf is no longer referenced to? */
+    if (p->ref == 0) {
+      /* remember next pbuf in chain for next iteration */
+      q = p->next;
+      LWIP_DEBUGF( PBUF_DEBUG | 2, ("pbuf_free: deallocating %p\n", (void *)p));
+      flags = p->flags;
+      /* is this a pbuf from the pool? */
+      if (flags == PBUF_FLAG_POOL) {
+        p->len = p->tot_len = PBUF_POOL_BUFSIZE;
+        p->payload = (void *)((u8_t *)p + sizeof(struct pbuf));
+        PBUF_POOL_FREE(p);
+      /* is this a ROM or RAM referencing pbuf? */
+      } else if (flags == PBUF_FLAG_ROM || flags == PBUF_FLAG_REF) {
+        memp_free(MEMP_PBUF, p);
+      /* flags == PBUF_FLAG_RAM */
+      } else {
+        mem_free(p);
+      }
+      count++;
+      /* proceed to next pbuf */
+      p = q;
+    /* p->ref > 0, this pbuf is still referenced to */
+    /* (and so the remaining pbufs in chain as well) */
+    } else {
+      LWIP_DEBUGF( PBUF_DEBUG | 2, ("pbuf_free: %p has ref %"U16_F", ending here.\n", (void *)p, (u16_t)p->ref));
+      /* stop walking through the chain */
+      p = NULL;
+    }
+  }
+  SYS_ARCH_UNPROTECT(old_level);
+  PERF_STOP("pbuf_free");
+  /* return number of de-allocated pbufs */
+
+  return count;
+}
+
+/**
+ * Count number of pbufs in a chain
+ *
+ * @param p first pbuf of chain
+ * @return the number of pbufs in a chain
+ */
+
+u8_t
+pbuf_clen(struct pbuf *p)
+{
+  u8_t len;
+
+  len = 0;
+  while (p != NULL) {
+    ++len;
+    p = p->next;
+  }
+  return len;
+}
+
+/**
+ * Increment the reference count of the pbuf.
+ *
+ * @param p pbuf to increase reference counter of
+ *
+ */
+void
+pbuf_ref(struct pbuf *p)
+{
+  SYS_ARCH_DECL_PROTECT(old_level);
+  /* pbuf given? */
+  if (p != NULL) {
+    SYS_ARCH_PROTECT(old_level);
+    ++(p->ref);
+    SYS_ARCH_UNPROTECT(old_level);
+  }
+}
+
+/**
+ * Concatenate two pbufs (each may be a pbuf chain) and take over
+ * the caller's reference of the tail pbuf.
+ * 
+ * @note The caller MAY NOT reference the tail pbuf afterwards.
+ * Use pbuf_chain() for that purpose.
+ * 
+ * @see pbuf_chain()
+ */
+
+void
+pbuf_cat(struct pbuf *h, struct pbuf *t)
+{
+  struct pbuf *p;
+
+  LWIP_ASSERT("h != NULL (programmer violates API)", h != NULL);
+  LWIP_ASSERT("t != NULL (programmer violates API)", t != NULL);
+  if ((h == NULL) || (t == NULL)) return;
+
+  /* proceed to last pbuf of chain */
+  for (p = h; p->next != NULL; p = p->next) {
+    /* add total length of second chain to all totals of first chain */
+    p->tot_len += t->tot_len;
+  }
+  /* { p is last pbuf of first h chain, p->next == NULL } */
+  LWIP_ASSERT("p->tot_len == p->len (of last pbuf in chain)", p->tot_len == p->len);
+  LWIP_ASSERT("p->next == NULL", p->next == NULL);
+  /* add total length of second chain to last pbuf total of first chain */
+  p->tot_len += t->tot_len;
+  /* chain last pbuf of head (p) with first of tail (t) */
+  p->next = t;
+  /* p->next now references t, but the caller will drop its reference to t,
+   * so netto there is no change to the reference count of t.
+   */
+}
+
+/**
+ * Chain two pbufs (or pbuf chains) together.
+ * 
+ * The caller MUST call pbuf_free(t) once it has stopped
+ * using it. Use pbuf_cat() instead if you no longer use t.
+ * 
+ * @param h head pbuf (chain)
+ * @param t tail pbuf (chain)
+ * @note The pbufs MUST belong to the same packet.
+ * @note MAY NOT be called on a packet queue.
+ *
+ * The ->tot_len fields of all pbufs of the head chain are adjusted.
+ * The ->next field of the last pbuf of the head chain is adjusted.
+ * The ->ref field of the first pbuf of the tail chain is adjusted.
+ *
+ */
+void
+pbuf_chain(struct pbuf *h, struct pbuf *t)
+{
+  pbuf_cat(h, t);
+  /* t is now referenced by h */
+  pbuf_ref(t);
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_FRESH | 2, ("pbuf_chain: %p references %p\n", (void *)h, (void *)t));
+}
+
+/* For packet queueing. Note that queued packets MUST be dequeued first
+ * using pbuf_dequeue() before calling other pbuf_() functions. */
+#if ARP_QUEUEING
+/**
+ * Add a packet to the end of a queue.
+ *
+ * @param q pointer to first packet on the queue
+ * @param n packet to be queued
+ *
+ * Both packets MUST be given, and must be different.
+ */
+void
+pbuf_queue(struct pbuf *p, struct pbuf *n)
+{
+#if PBUF_DEBUG /* remember head of queue */
+  struct pbuf *q = p;
+#endif
+  /* programmer stupidity checks */
+  LWIP_ASSERT("p == NULL in pbuf_queue: this indicates a programmer error\n", p != NULL);
+  LWIP_ASSERT("n == NULL in pbuf_queue: this indicates a programmer error\n", n != NULL);
+  LWIP_ASSERT("p == n in pbuf_queue: this indicates a programmer error\n", p != n);
+  if ((p == NULL) || (n == NULL) || (p == n)){
+    LWIP_DEBUGF(PBUF_DEBUG | DBG_HALT | 3, ("pbuf_queue: programmer argument error\n"));
+    return;
+  }
+
+  /* iterate through all packets on queue */
+  while (p->next != NULL) {
+/* be very picky about pbuf chain correctness */
+#if PBUF_DEBUG
+    /* iterate through all pbufs in packet */
+    while (p->tot_len != p->len) {
+      /* make sure invariant condition holds */
+      LWIP_ASSERT("p->len < p->tot_len", p->len < p->tot_len);
+      /* make sure each packet is complete */
+      LWIP_ASSERT("p->next != NULL", p->next != NULL);
+      p = p->next;
+      /* { p->tot_len == p->len => p is last pbuf of a packet } */
+    }
+    /* { p is last pbuf of a packet } */
+    /* proceed to next packet on queue */
+#endif
+    /* proceed to next pbuf */
+    if (p->next != NULL) p = p->next;
+  }
+  /* { p->tot_len == p->len and p->next == NULL } ==>
+   * { p is last pbuf of last packet on queue } */
+  /* chain last pbuf of queue with n */
+  p->next = n;
+  /* n is now referenced to by the (packet p in the) queue */
+  pbuf_ref(n);
+#if PBUF_DEBUG
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_FRESH | 2,
+    ("pbuf_queue: newly queued packet %p sits after packet %p in queue %p\n",
+    (void *)n, (void *)p, (void *)q));
+#endif
+}
+
+/**
+ * Remove a packet from the head of a queue.
+ *
+ * The caller MUST reference the remainder of the queue (as returned). The
+ * caller MUST NOT call pbuf_ref() as it implicitly takes over the reference
+ * from p.
+ * 
+ * @param p pointer to first packet on the queue which will be dequeued.
+ * @return first packet on the remaining queue (NULL if no further packets).
+ *
+ */
+struct pbuf *
+pbuf_dequeue(struct pbuf *p)
+{
+  struct pbuf *q;
+  LWIP_ASSERT("p != NULL", p != NULL);
+
+  /* iterate through all pbufs in packet p */
+  while (p->tot_len != p->len) {
+    /* make sure invariant condition holds */
+    LWIP_ASSERT("p->len < p->tot_len", p->len < p->tot_len);
+    /* make sure each packet is complete */
+    LWIP_ASSERT("p->next != NULL", p->next != NULL);
+    p = p->next;
+  }
+  /* { p->tot_len == p->len } => p is the last pbuf of the first packet */
+  /* remember next packet on queue in q */
+  q = p->next;
+  /* dequeue packet p from queue */
+  p->next = NULL;
+  /* any next packet on queue? */
+  if (q != NULL) {
+    /* although q is no longer referenced by p, it MUST be referenced by
+     * the caller, who is maintaining this packet queue. So, we do not call
+     * pbuf_free(q) here, resulting in an implicit pbuf_ref(q) for the caller. */
+    LWIP_DEBUGF(PBUF_DEBUG | DBG_FRESH | 2, ("pbuf_dequeue: first remaining packet on queue is %p\n", (void *)q));
+  } else {
+    LWIP_DEBUGF(PBUF_DEBUG | DBG_FRESH | 2, ("pbuf_dequeue: no further packets on queue\n"));
+  }
+  return q;
+}
+#endif
+
+/**
+ *
+ * Create PBUF_POOL (or PBUF_RAM) copies of PBUF_REF pbufs.
+ *
+ * Used to queue packets on behalf of the lwIP stack, such as
+ * ARP based queueing.
+ *
+ * Go through a pbuf chain and replace any PBUF_REF buffers
+ * with PBUF_POOL (or PBUF_RAM) pbufs, each taking a copy of
+ * the referenced data.
+ *
+ * @note You MUST explicitly use p = pbuf_take(p);
+ * The pbuf you give as argument, may have been replaced
+ * by a (differently located) copy through pbuf_take()!
+ *
+ * @note Any replaced pbufs will be freed through pbuf_free().
+ * This may deallocate them if they become no longer referenced.
+ *
+ * @param p Head of pbuf chain to process
+ *
+ * @return Pointer to head of pbuf chain
+ */
+struct pbuf *
+pbuf_take(struct pbuf *p)
+{
+  struct pbuf *q , *prev, *head;
+  LWIP_ASSERT("pbuf_take: p != NULL\n", p != NULL);
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 3, ("pbuf_take(%p)\n", (void*)p));
+
+  prev = NULL;
+  head = p;
+  /* iterate through pbuf chain */
+  do
+  {
+    /* pbuf is of type PBUF_REF? */
+    if (p->flags == PBUF_FLAG_REF) {
+      LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE, ("pbuf_take: encountered PBUF_REF %p\n", (void *)p));
+      /* allocate a pbuf (w/ payload) fully in RAM */
+      /* PBUF_POOL buffers are faster if we can use them */
+      if (p->len <= PBUF_POOL_BUFSIZE) {
+        q = pbuf_alloc(PBUF_RAW, p->len, PBUF_POOL);
+        if (q == NULL) {
+          LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 2, ("pbuf_take: Could not allocate PBUF_POOL\n"));
+        }
+      } else {
+        /* no replacement pbuf yet */
+        q = NULL;
+        LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 2, ("pbuf_take: PBUF_POOL too small to replace PBUF_REF\n"));
+      }
+      /* no (large enough) PBUF_POOL was available? retry with PBUF_RAM */
+      if (q == NULL) {
+        q = pbuf_alloc(PBUF_RAW, p->len, PBUF_RAM);
+        if (q == NULL) {
+          LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 2, ("pbuf_take: Could not allocate PBUF_RAM\n"));
+        }
+      }
+      /* replacement pbuf could be allocated? */
+      if (q != NULL)
+      {
+        /* copy p to q */
+        /* copy successor */
+        q->next = p->next;
+        /* remove linkage from original pbuf */
+        p->next = NULL;
+        /* remove linkage to original pbuf */
+        if (prev != NULL) {
+          /* prev->next == p at this point */
+          LWIP_ASSERT("prev->next == p", prev->next == p);
+          /* break chain and insert new pbuf instead */
+          prev->next = q;
+        /* prev == NULL, so we replaced the head pbuf of the chain */
+        } else {
+          head = q;
+        }
+        /* copy pbuf payload */
+        memcpy(q->payload, p->payload, p->len);
+        q->tot_len = p->tot_len;
+        q->len = p->len;
+        /* in case p was the first pbuf, it is no longer refered to by
+         * our caller, as the caller MUST do p = pbuf_take(p);
+         * in case p was not the first pbuf, it is no longer refered to
+         * by prev. we can safely free the pbuf here.
+         * (note that we have set p->next to NULL already so that
+         * we will not free the rest of the chain by accident.)
+         */
+        pbuf_free(p);
+        /* do not copy ref, since someone else might be using the old buffer */
+        LWIP_DEBUGF(PBUF_DEBUG, ("pbuf_take: replaced PBUF_REF %p with %p\n", (void *)p, (void *)q));
+        p = q;
+      } else {
+        /* deallocate chain */
+        pbuf_free(head);
+        LWIP_DEBUGF(PBUF_DEBUG | 2, ("pbuf_take: failed to allocate replacement pbuf for %p\n", (void *)p));
+        return NULL;
+      }
+    /* p->flags != PBUF_FLAG_REF */
+    } else {
+      LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 1, ("pbuf_take: skipping pbuf not of type PBUF_REF\n"));
+    }
+    /* remember this pbuf */
+    prev = p;
+    /* proceed to next pbuf in original chain */
+    p = p->next;
+  } while (p);
+  LWIP_DEBUGF(PBUF_DEBUG | DBG_TRACE | 1, ("pbuf_take: end of chain reached.\n"));
+
+  return head;
+}
+
+/**
+ * Dechains the first pbuf from its succeeding pbufs in the chain.
+ *
+ * Makes p->tot_len field equal to p->len.
+ * @param p pbuf to dechain
+ * @return remainder of the pbuf chain, or NULL if it was de-allocated.
+ * @note May not be called on a packet queue.
+ */
+struct pbuf *
+pbuf_dechain(struct pbuf *p)
+{
+  struct pbuf *q;
+  u8_t tail_gone = 1;
+  /* tail */
+  q = p->next;
+  /* pbuf has successor in chain? */
+  if (q != NULL) {
+    /* assert tot_len invariant: (p->tot_len == p->len + (p->next? p->next->tot_len: 0) */
+    LWIP_ASSERT("p->tot_len == p->len + q->tot_len", q->tot_len == p->tot_len - p->len);
+    /* enforce invariant if assertion is disabled */
+    q->tot_len = p->tot_len - p->len;
+    /* decouple pbuf from remainder */
+    p->next = NULL;
+    /* total length of pbuf p is its own length only */
+    p->tot_len = p->len;
+    /* q is no longer referenced by p, free it */
+    LWIP_DEBUGF(PBUF_DEBUG | DBG_STATE, ("pbuf_dechain: unreferencing %p\n", (void *)q));
+    tail_gone = pbuf_free(q);
+    if (tail_gone > 0) {
+      LWIP_DEBUGF(PBUF_DEBUG | DBG_STATE,
+                  ("pbuf_dechain: deallocated %p (as it is no longer referenced)\n", (void *)q));
+    }
+    /* return remaining tail or NULL if deallocated */
+  }
+  /* assert tot_len invariant: (p->tot_len == p->len + (p->next? p->next->tot_len: 0) */
+  LWIP_ASSERT("p->tot_len == p->len", p->tot_len == p->len);
+  return (tail_gone > 0? NULL: q);
+}

lwip/core/raw.c

@@ -0,0 +1,326 @@
+/**
+ * @file
+ * 
+ * Implementation of raw protocol PCBs for low-level handling of
+ * different types of protocols besides (or overriding) those
+ * already available in lwIP.
+ *
+ */
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include <string.h>
+
+#include "lwip/opt.h"
+
+#include "lwip/def.h"
+#include "lwip/memp.h"
+#include "lwip/inet.h"
+#include "lwip/ip_addr.h"
+#include "lwip/netif.h"
+#include "lwip/raw.h"
+
+#include "lwip/stats.h"
+
+#include "arch/perf.h"
+#include "lwip/snmp.h"
+
+#if LWIP_RAW
+
+/** The list of RAW PCBs */
+static struct raw_pcb *raw_pcbs = NULL;
+
+void
+raw_init(void)
+{
+  raw_pcbs = NULL;
+}
+
+/**
+ * Determine if in incoming IP packet is covered by a RAW PCB
+ * and if so, pass it to a user-provided receive callback function.
+ *
+ * Given an incoming IP datagram (as a chain of pbufs) this function
+ * finds a corresponding RAW PCB and calls the corresponding receive
+ * callback function.
+ *
+ * @param pbuf pbuf to be demultiplexed to a RAW PCB.
+ * @param netif network interface on which the datagram was received.
+ * @Return - 1 if the packet has been eaten by a RAW PCB receive
+ *           callback function. The caller MAY NOT not reference the
+ *           packet any longer, and MAY NOT call pbuf_free().
+ * @return - 0 if packet is not eaten (pbuf is still referenced by the
+ *           caller).
+ *
+ */
+u8_t
+raw_input(struct pbuf *p, struct netif *inp)
+{
+  struct raw_pcb *pcb;
+  struct ip_hdr *iphdr;
+  s16_t proto;
+  u8_t eaten = 0;
+
+  iphdr = p->payload;
+  proto = IPH_PROTO(iphdr);
+
+  pcb = raw_pcbs;
+  /* loop through all raw pcbs until the packet is eaten by one */
+  /* this allows multiple pcbs to match against the packet by design */
+  while ((eaten == 0) && (pcb != NULL)) {
+    if (pcb->protocol == proto) {
+      /* receive callback function available? */
+      if (pcb->recv != NULL) {
+        /* the receive callback function did not eat the packet? */
+        if (pcb->recv(pcb->recv_arg, pcb, p, &(iphdr->src)) != 0)
+        {
+          /* receive function ate the packet */
+          p = NULL;
+          eaten = 1;
+        }
+      }
+      /* no receive callback function was set for this raw PCB */
+      /* drop the packet */
+    }
+    pcb = pcb->next;
+  }
+  return eaten;
+}
+
+/**
+ * Bind a RAW PCB.
+ *
+ * @param pcb RAW PCB to be bound with a local address ipaddr.
+ * @param ipaddr local IP address to bind with. Use IP_ADDR_ANY to
+ * bind to all local interfaces.
+ *
+ * @return lwIP error code.
+ * - ERR_OK. Successful. No error occured.
+ * - ERR_USE. The specified IP address is already bound to by
+ * another RAW PCB.
+ *
+ * @see raw_disconnect()
+ */
+err_t
+raw_bind(struct raw_pcb *pcb, struct ip_addr *ipaddr)
+{
+  ip_addr_set(&pcb->local_ip, ipaddr);
+  return ERR_OK;
+}
+
+/**
+ * Connect an RAW PCB. This function is required by upper layers
+ * of lwip. Using the raw api you could use raw_sendto() instead
+ *
+ * This will associate the RAW PCB with the remote address.
+ *
+ * @param pcb RAW PCB to be connected with remote address ipaddr and port.
+ * @param ipaddr remote IP address to connect with.
+ *
+ * @return lwIP error code
+ *
+ * @see raw_disconnect() and raw_sendto()
+ */
+err_t
+raw_connect(struct raw_pcb *pcb, struct ip_addr *ipaddr)
+{
+  ip_addr_set(&pcb->remote_ip, ipaddr);
+  return ERR_OK;
+}
+
+
+/**
+ * Set the callback function for received packets that match the
+ * raw PCB's protocol and binding. 
+ * 
+ * The callback function MUST either
+ * - eat the packet by calling pbuf_free() and returning non-zero. The
+ *   packet will not be passed to other raw PCBs or other protocol layers.
+ * - not free the packet, and return zero. The packet will be matched
+ *   against further PCBs and/or forwarded to another protocol layers.
+ * 
+ * @return non-zero if the packet was free()d, zero if the packet remains
+ * available for others.
+ */
+void
+raw_recv(struct raw_pcb *pcb,
+         u8_t (* recv)(void *arg, struct raw_pcb *upcb, struct pbuf *p,
+                      struct ip_addr *addr),
+         void *recv_arg)
+{
+  /* remember recv() callback and user data */
+  pcb->recv = recv;
+  pcb->recv_arg = recv_arg;
+}
+
+/**
+ * Send the raw IP packet to the given address. Note that actually you cannot
+ * modify the IP headers (this is inconsistent with the receive callback where
+ * you actually get the IP headers), you can only specify the IP payload here.
+ * It requires some more changes in lwIP. (there will be a raw_send() function
+ * then.)
+ *
+ * @param pcb the raw pcb which to send
+ * @param p the IP payload to send
+ * @param ipaddr the destination address of the IP packet
+ *
+ */
+err_t
+raw_sendto(struct raw_pcb *pcb, struct pbuf *p, struct ip_addr *ipaddr)
+{
+  err_t err;
+  struct netif *netif;
+  struct ip_addr *src_ip;
+  struct pbuf *q; /* q will be sent down the stack */
+  
+  LWIP_DEBUGF(RAW_DEBUG | DBG_TRACE | 3, ("raw_sendto\n"));
+  
+  /* not enough space to add an IP header to first pbuf in given p chain? */
+  if (pbuf_header(p, IP_HLEN)) {
+    /* allocate header in new pbuf */
+    q = pbuf_alloc(PBUF_IP, 0, PBUF_RAM);
+    /* new header pbuf could not be allocated? */
+    if (q == NULL) {
+      LWIP_DEBUGF(RAW_DEBUG | DBG_TRACE | 2, ("raw_sendto: could not allocate header\n"));
+      return ERR_MEM;
+    }
+    /* chain header q in front of given pbuf p */
+    pbuf_chain(q, p);
+    /* { first pbuf q points to header pbuf } */
+    LWIP_DEBUGF(RAW_DEBUG, ("raw_sendto: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
+  }  else {
+    /* first pbuf q equals given pbuf */
+    q = p;
+    pbuf_header(q, -IP_HLEN);
+  }
+  
+  if ((netif = ip_route(ipaddr)) == NULL) {
+    LWIP_DEBUGF(RAW_DEBUG | 1, ("raw_sendto: No route to 0x%"X32_F"\n", ipaddr->addr));
+#if RAW_STATS
+    /*    ++lwip_stats.raw.rterr;*/
+#endif /* RAW_STATS */
+    /* free any temporary header pbuf allocated by pbuf_header() */
+    if (q != p) {
+      pbuf_free(q);
+    }
+    return ERR_RTE;
+  }
+
+  if (ip_addr_isany(&pcb->local_ip)) {
+    /* use outgoing network interface IP address as source address */
+    src_ip = &(netif->ip_addr);
+  } else {
+    /* use RAW PCB local IP address as source address */
+    src_ip = &(pcb->local_ip);
+  }
+
+  err = ip_output_if (q, src_ip, ipaddr, pcb->ttl, pcb->tos, pcb->protocol, netif);
+
+  /* did we chain a header earlier? */
+  if (q != p) {
+    /* free the header */
+    pbuf_free(q);
+  }
+  return err;
+}
+
+/**
+ * Send the raw IP packet to the address given by raw_connect()
+ *
+ * @param pcb the raw pcb which to send
+ * @param p the IP payload to send
+ * @param ipaddr the destination address of the IP packet
+ *
+ */
+err_t
+raw_send(struct raw_pcb *pcb, struct pbuf *p)
+{
+  return raw_sendto(pcb, p, &pcb->remote_ip);
+}
+
+/**
+ * Remove an RAW PCB.
+ *
+ * @param pcb RAW PCB to be removed. The PCB is removed from the list of
+ * RAW PCB's and the data structure is freed from memory.
+ *
+ * @see raw_new()
+ */
+void
+raw_remove(struct raw_pcb *pcb)
+{
+  struct raw_pcb *pcb2;
+  /* pcb to be removed is first in list? */
+  if (raw_pcbs == pcb) {
+    /* make list start at 2nd pcb */
+    raw_pcbs = raw_pcbs->next;
+    /* pcb not 1st in list */
+  } else for(pcb2 = raw_pcbs; pcb2 != NULL; pcb2 = pcb2->next) {
+    /* find pcb in raw_pcbs list */
+    if (pcb2->next != NULL && pcb2->next == pcb) {
+      /* remove pcb from list */
+      pcb2->next = pcb->next;
+    }
+  }
+  memp_free(MEMP_RAW_PCB, pcb);
+}
+
+/**
+ * Create a RAW PCB.
+ *
+ * @return The RAW PCB which was created. NULL if the PCB data structure
+ * could not be allocated.
+ *
+ * @param proto the protocol number of the IPs payload (e.g. IP_PROTO_ICMP)
+ *
+ * @see raw_remove()
+ */
+struct raw_pcb *
+raw_new(u16_t proto) {
+  struct raw_pcb *pcb;
+
+  LWIP_DEBUGF(RAW_DEBUG | DBG_TRACE | 3, ("raw_new\n"));
+
+  pcb = memp_malloc(MEMP_RAW_PCB);
+  /* could allocate RAW PCB? */
+  if (pcb != NULL) {
+    /* initialize PCB to all zeroes */
+    memset(pcb, 0, sizeof(struct raw_pcb));
+    pcb->protocol = proto;
+    pcb->ttl = RAW_TTL;
+    pcb->next = raw_pcbs;
+    raw_pcbs = pcb;
+  }
+  return pcb;
+}
+
+#endif /* LWIP_RAW */

lwip/core/stats.c

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved. 
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, 
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission. 
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ * 
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include <string.h>
+
+#include "lwip/opt.h"
+
+#include "lwip/def.h"
+
+#include "lwip/stats.h"
+#include "lwip/mem.h"
+
+
+#if LWIP_STATS
+struct stats_ lwip_stats;
+
+void
+stats_init(void)
+{
+  memset(&lwip_stats, 0, sizeof(struct stats_));
+}
+#if LWIP_STATS_DISPLAY
+void
+stats_display_proto(struct stats_proto *proto, char *name)
+{
+  LWIP_PLATFORM_DIAG(("\n%s\n\t", name));
+  LWIP_PLATFORM_DIAG(("xmit: %"S16_F"\n\t", proto->xmit)); 
+  LWIP_PLATFORM_DIAG(("rexmit: %"S16_F"\n\t", proto->rexmit)); 
+  LWIP_PLATFORM_DIAG(("recv: %"S16_F"\n\t", proto->recv)); 
+  LWIP_PLATFORM_DIAG(("fw: %"S16_F"\n\t", proto->fw)); 
+  LWIP_PLATFORM_DIAG(("drop: %"S16_F"\n\t", proto->drop)); 
+  LWIP_PLATFORM_DIAG(("chkerr: %"S16_F"\n\t", proto->chkerr)); 
+  LWIP_PLATFORM_DIAG(("lenerr: %"S16_F"\n\t", proto->lenerr)); 
+  LWIP_PLATFORM_DIAG(("memerr: %"S16_F"\n\t", proto->memerr)); 
+  LWIP_PLATFORM_DIAG(("rterr: %"S16_F"\n\t", proto->rterr)); 
+  LWIP_PLATFORM_DIAG(("proterr: %"S16_F"\n\t", proto->proterr)); 
+  LWIP_PLATFORM_DIAG(("opterr: %"S16_F"\n\t", proto->opterr)); 
+  LWIP_PLATFORM_DIAG(("err: %"S16_F"\n\t", proto->err)); 
+  LWIP_PLATFORM_DIAG(("cachehit: %"S16_F"\n", proto->cachehit)); 
+}
+
+void
+stats_display_pbuf(struct stats_pbuf *pbuf)
+{
+  LWIP_PLATFORM_DIAG(("\nPBUF\n\t"));
+  LWIP_PLATFORM_DIAG(("avail: %"S16_F"\n\t", pbuf->avail)); 
+  LWIP_PLATFORM_DIAG(("used: %"S16_F"\n\t", pbuf->used)); 
+  LWIP_PLATFORM_DIAG(("max: %"S16_F"\n\t", pbuf->max)); 
+  LWIP_PLATFORM_DIAG(("err: %"S16_F"\n\t", pbuf->err)); 
+  LWIP_PLATFORM_DIAG(("alloc_locked: %"S16_F"\n\t", pbuf->alloc_locked)); 
+  LWIP_PLATFORM_DIAG(("refresh_locked: %"S16_F"\n", pbuf->refresh_locked)); 
+}
+
+void
+stats_display_mem(struct stats_mem *mem, char *name)
+{
+  LWIP_PLATFORM_DIAG(("\n MEM %s\n\t", name));
+  LWIP_PLATFORM_DIAG(("avail: %"MEM_SIZE_F"\n\t", mem->avail)); 
+  LWIP_PLATFORM_DIAG(("used: %"MEM_SIZE_F"\n\t", mem->used)); 
+  LWIP_PLATFORM_DIAG(("max: %"MEM_SIZE_F"\n\t", mem->max)); 
+  LWIP_PLATFORM_DIAG(("err: %"MEM_SIZE_F"\n", mem->err));
+  
+}
+
+void
+stats_display(void)
+{
+  s16_t i;
+  char * memp_names[] = {"PBUF", "RAW_PCB", "UDP_PCB", "TCP_PCB", "TCP_PCB_LISTEN",
+        "TCP_SEG", "NETBUF", "NETCONN", "API_MSG", "TCP_MSG", "TIMEOUT"};
+  stats_display_proto(&lwip_stats.link, "LINK");
+  stats_display_proto(&lwip_stats.ip_frag, "IP_FRAG");
+  stats_display_proto(&lwip_stats.ip, "IP");
+  stats_display_proto(&lwip_stats.icmp, "ICMP");
+  stats_display_proto(&lwip_stats.udp, "UDP");
+  stats_display_proto(&lwip_stats.tcp, "TCP");
+  stats_display_pbuf(&lwip_stats.pbuf);
+  stats_display_mem(&lwip_stats.mem, "HEAP");
+  for (i = 0; i < MEMP_MAX; i++) {
+    stats_display_mem(&lwip_stats.memp[i], memp_names[i]);
+  }
+  
+}
+#endif /* LWIP_STATS_DISPLAY */
+#endif /* LWIP_STATS */
+

lwip/core/sys.c

@@ -0,0 +1,294 @@
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include "lwip/sys.h"
+#include "lwip/opt.h"
+#include "lwip/def.h"
+#include "lwip/memp.h"
+
+#if (NO_SYS == 0)
+
+struct sswt_cb
+{
+    s16_t timeflag;
+    sys_sem_t *psem;
+};
+
+
+
+void
+sys_mbox_fetch(sys_mbox_t mbox, void **msg)
+{
+  u32_t time;
+  struct sys_timeouts *timeouts;
+  struct sys_timeo *tmptimeout;
+  sys_timeout_handler h;
+  void *arg;
+
+
+ again:
+  timeouts = sys_arch_timeouts();
+
+  if (!timeouts || !timeouts->next) {
+    sys_arch_mbox_fetch(mbox, msg, 0);
+  } else {
+    if (timeouts->next->time > 0) {
+      time = sys_arch_mbox_fetch(mbox, msg, timeouts->next->time);
+    } else {
+      time = SYS_ARCH_TIMEOUT;
+    }
+
+    if (time == SYS_ARCH_TIMEOUT) {
+      /* If time == SYS_ARCH_TIMEOUT, a timeout occured before a message
+   could be fetched. We should now call the timeout handler and
+   deallocate the memory allocated for the timeout. */
+      tmptimeout = timeouts->next;
+      timeouts->next = tmptimeout->next;
+      h = tmptimeout->h;
+      arg = tmptimeout->arg;
+      memp_free(MEMP_SYS_TIMEOUT, tmptimeout);
+      if (h != NULL) {
+        LWIP_DEBUGF(SYS_DEBUG, ("smf calling h=%p(%p)\n", (void *)h, (void *)arg));
+      	h(arg);
+      }
+
+      /* We try again to fetch a message from the mbox. */
+      goto again;
+    } else {
+      /* If time != SYS_ARCH_TIMEOUT, a message was received before the timeout
+   occured. The time variable is set to the number of
+   milliseconds we waited for the message. */
+      if (time <= timeouts->next->time) {
+  timeouts->next->time -= time;
+      } else {
+  timeouts->next->time = 0;
+      }
+    }
+
+  }
+}
+
+void
+sys_sem_wait(sys_sem_t sem)
+{
+  u32_t time;
+  struct sys_timeouts *timeouts;
+  struct sys_timeo *tmptimeout;
+  sys_timeout_handler h;
+  void *arg;
+
+  /*  while (sys_arch_sem_wait(sem, 1000) == 0);
+      return;*/
+
+ again:
+
+  timeouts = sys_arch_timeouts();
+
+  if (!timeouts || !timeouts->next) {
+    sys_arch_sem_wait(sem, 0);
+  } else {
+    if (timeouts->next->time > 0) {
+      time = sys_arch_sem_wait(sem, timeouts->next->time);
+    } else {
+      time = SYS_ARCH_TIMEOUT;
+    }
+
+    if (time == SYS_ARCH_TIMEOUT) {
+      /* If time == SYS_ARCH_TIMEOUT, a timeout occured before a message
+   could be fetched. We should now call the timeout handler and
+   deallocate the memory allocated for the timeout. */
+      tmptimeout = timeouts->next;
+      timeouts->next = tmptimeout->next;
+      h = tmptimeout->h;
+      arg = tmptimeout->arg;
+      memp_free(MEMP_SYS_TIMEOUT, tmptimeout);
+      if (h != NULL) {
+        LWIP_DEBUGF(SYS_DEBUG, ("ssw h=%p(%p)\n", (void *)h, (void *)arg));
+        h(arg);
+      }
+
+
+      /* We try again to fetch a message from the mbox. */
+      goto again;
+    } else {
+      /* If time != SYS_ARCH_TIMEOUT, a message was received before the timeout
+   occured. The time variable is set to the number of
+   milliseconds we waited for the message. */
+      if (time <= timeouts->next->time) {
+  timeouts->next->time -= time;
+      } else {
+  timeouts->next->time = 0;
+      }
+    }
+
+  }
+}
+
+void
+sys_timeout(u32_t msecs, sys_timeout_handler h, void *arg)
+{
+  struct sys_timeouts *timeouts;
+  struct sys_timeo *timeout, *t;
+
+  timeout = memp_malloc(MEMP_SYS_TIMEOUT);
+  if (timeout == NULL) {
+    return;
+  }
+  timeout->next = NULL;
+  timeout->h = h;
+  timeout->arg = arg;
+  timeout->time = msecs;
+
+  timeouts = sys_arch_timeouts();
+
+  LWIP_DEBUGF(SYS_DEBUG, ("sys_timeout: %p msecs=%"U32_F" h=%p arg=%p\n",
+    (void *)timeout, msecs, (void *)h, (void *)arg));
+
+  LWIP_ASSERT("sys_timeout: timeouts != NULL", timeouts != NULL);
+
+  if (timeouts->next == NULL) {
+    timeouts->next = timeout;
+    return;
+  }
+
+  if (timeouts->next->time > msecs) {
+    timeouts->next->time -= msecs;
+    timeout->next = timeouts->next;
+    timeouts->next = timeout;
+  } else {
+    for(t = timeouts->next; t != NULL; t = t->next) {
+      timeout->time -= t->time;
+      if (t->next == NULL || t->next->time > timeout->time) {
+        if (t->next != NULL) {
+          t->next->time -= timeout->time;
+        }
+        timeout->next = t->next;
+        t->next = timeout;
+        break;
+      }
+    }
+  }
+
+}
+
+/* Go through timeout list (for this task only) and remove the first matching entry,
+   even though the timeout has not triggered yet.
+*/
+
+void
+sys_untimeout(sys_timeout_handler h, void *arg)
+{
+    struct sys_timeouts *timeouts;
+    struct sys_timeo *prev_t, *t;
+
+    timeouts = sys_arch_timeouts();
+
+    if (timeouts->next == NULL)
+        return;
+
+    for (t = timeouts->next, prev_t = NULL; t != NULL; prev_t = t, t = t->next)
+    {
+        if ((t->h == h) && (t->arg == arg))
+        {
+            /* We have a match */
+            /* Unlink from previous in list */
+            if (prev_t == NULL)
+                timeouts->next = t->next;
+            else
+                prev_t->next = t->next;
+            /* If not the last one, add time of this one back to next */
+            if (t->next != NULL)
+                t->next->time += t->time;
+            memp_free(MEMP_SYS_TIMEOUT, t);
+            return;
+        }
+    }
+    return;
+}
+
+
+
+
+
+static void
+sswt_handler(void *arg)
+{
+    struct sswt_cb *sswt_cb = (struct sswt_cb *) arg;
+
+    /* Timeout. Set flag to TRUE and signal semaphore */
+    sswt_cb->timeflag = 1;
+    sys_sem_signal(*(sswt_cb->psem));
+}
+
+/* Wait for a semaphore with timeout (specified in ms) */
+/* timeout = 0: wait forever */
+/* Returns 0 on timeout. 1 otherwise */
+
+int
+sys_sem_wait_timeout(sys_sem_t sem, u32_t timeout)
+{
+    struct sswt_cb sswt_cb;
+
+    sswt_cb.psem = &sem;
+    sswt_cb.timeflag = 0;
+
+    /* If timeout is zero, then just wait forever */
+    if (timeout > 0)
+        /* Create a timer and pass it the address of our flag */
+        sys_timeout(timeout, sswt_handler, &sswt_cb);
+    sys_sem_wait(sem);
+    /* Was it a timeout? */
+    if (sswt_cb.timeflag)
+    {
+        /* timeout */
+        return 0;
+    } else {
+        /* Not a timeout. Remove timeout entry */
+        sys_untimeout(sswt_handler, &sswt_cb);
+        return 1;
+    }
+
+}
+
+
+void
+sys_msleep(u32_t ms)
+{
+  sys_sem_t delaysem = sys_sem_new(0);
+
+  sys_sem_wait_timeout(delaysem, ms);
+
+  sys_sem_free(delaysem);
+}
+
+
+#endif /* NO_SYS */

lwip/core/tcp_out.c

@@ -0,0 +1,727 @@
+/**
+ * @file
+ *
+ * Transmission Control Protocol, outgoing traffic
+ *
+ * The output functions of TCP.
+ *
+ */
+
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include <string.h>
+
+#include "lwip/def.h"
+#include "lwip/opt.h"
+#include "lwip/mem.h"
+#include "lwip/memp.h"
+#include "lwip/sys.h"
+#include "lwip/ip_addr.h"
+#include "lwip/netif.h"
+#include "lwip/inet.h"
+#include "lwip/tcp.h"
+#include "lwip/stats.h"
+#include "lwip/snmp.h"
+
+#if LWIP_TCP
+
+/* Forward declarations.*/
+static void tcp_output_segment(struct tcp_seg *seg, struct tcp_pcb *pcb);
+
+err_t
+tcp_send_ctrl(struct tcp_pcb *pcb, u8_t flags)
+{
+  /* no data, no length, flags, copy=1, no optdata, no optdatalen */
+  return tcp_enqueue(pcb, NULL, 0, flags, 1, NULL, 0);
+}
+
+/**
+ * Write data for sending (but does not send it immediately).
+ *
+ * It waits in the expectation of more data being sent soon (as
+ * it can send them more efficiently by combining them together).
+ * To prompt the system to send data now, call tcp_output() after
+ * calling tcp_write().
+ * 
+ * @arg pcb Protocol control block of the TCP connection to enqueue data for. 
+ * 
+ * @see tcp_write()
+ */
+
+err_t
+tcp_write(struct tcp_pcb *pcb, const void *arg, u16_t len, u8_t copy)
+{
+  LWIP_DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_write(pcb=%p, arg=%p, len=%"U16_F", copy=%"U16_F")\n", (void *)pcb,
+    arg, len, (u16_t)copy));
+  /* connection is in valid state for data transmission? */
+  if (pcb->state == ESTABLISHED ||
+     pcb->state == CLOSE_WAIT ||
+     pcb->state == SYN_SENT ||
+     pcb->state == SYN_RCVD) {
+    if (len > 0) {
+      return tcp_enqueue(pcb, (void *)arg, len, 0, copy, NULL, 0);
+    }
+    return ERR_OK;
+  } else {
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG | DBG_STATE | 3, ("tcp_write() called in invalid state\n"));
+    return ERR_CONN;
+  }
+}
+
+/**
+ * Enqueue either data or TCP options (but not both) for tranmission
+ * 
+ * 
+ * 
+ * @arg pcb Protocol control block for the TCP connection to enqueue data for.
+ * @arg arg Pointer to the data to be enqueued for sending.
+ * @arg len Data length in bytes
+ * @arg flags
+ * @arg copy 1 if data must be copied, 0 if data is non-volatile and can be
+ * referenced.
+ * @arg optdata
+ * @arg optlen
+ */
+err_t
+tcp_enqueue(struct tcp_pcb *pcb, void *arg, u16_t len,
+  u8_t flags, u8_t copy,
+  u8_t *optdata, u8_t optlen)
+{
+  struct pbuf *p;
+  struct tcp_seg *seg, *useg, *queue;
+  u32_t left, seqno;
+  u16_t seglen;
+  void *ptr;
+  u8_t queuelen;
+
+  LWIP_DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue(pcb=%p, arg=%p, len=%"U16_F", flags=%"X16_F", copy=%"U16_F")\n",
+    (void *)pcb, arg, len, (u16_t)flags, (u16_t)copy));
+  LWIP_ASSERT("tcp_enqueue: len == 0 || optlen == 0 (programmer violates API)",
+      len == 0 || optlen == 0);
+  LWIP_ASSERT("tcp_enqueue: arg == NULL || optdata == NULL (programmer violates API)",
+      arg == NULL || optdata == NULL);
+  /* fail on too much data */
+  if (len > pcb->snd_buf) {
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 3, ("tcp_enqueue: too much data (len=%"U16_F" > snd_buf=%"U16_F")\n", len, pcb->snd_buf));
+    return ERR_MEM;
+  }
+  left = len;
+  ptr = arg;
+
+  /* seqno will be the sequence number of the first segment enqueued
+   * by the call to this function. */
+  seqno = pcb->snd_lbb;
+
+  LWIP_DEBUGF(TCP_QLEN_DEBUG, ("tcp_enqueue: queuelen: %"U16_F"\n", (u16_t)pcb->snd_queuelen));
+
+  /* If total number of pbufs on the unsent/unacked queues exceeds the
+   * configured maximum, return an error */
+  queuelen = pcb->snd_queuelen;
+  if (queuelen >= TCP_SND_QUEUELEN) {
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 3, ("tcp_enqueue: too long queue %"U16_F" (max %"U16_F")\n", queuelen, TCP_SND_QUEUELEN));
+    TCP_STATS_INC(tcp.memerr);
+    return ERR_MEM;
+  }
+  if (queuelen != 0) {
+    LWIP_ASSERT("tcp_enqueue: pbufs on queue => at least one queue non-empty",
+      pcb->unacked != NULL || pcb->unsent != NULL);
+  } else {
+    LWIP_ASSERT("tcp_enqueue: no pbufs on queue => both queues empty",
+      pcb->unacked == NULL && pcb->unsent == NULL);
+  }
+
+  /* First, break up the data into segments and tuck them together in
+   * the local "queue" variable. */
+  useg = queue = seg = NULL;
+  seglen = 0;
+  while (queue == NULL || left > 0) {
+
+    /* The segment length should be the MSS if the data to be enqueued
+     * is larger than the MSS. */
+    seglen = left > pcb->mss? pcb->mss: left;
+
+    /* Allocate memory for tcp_seg, and fill in fields. */
+    seg = memp_malloc(MEMP_TCP_SEG);
+    if (seg == NULL) {
+      LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 2, ("tcp_enqueue: could not allocate memory for tcp_seg\n"));
+      goto memerr;
+    }
+    seg->next = NULL;
+    seg->p = NULL;
+
+    /* first segment of to-be-queued data? */
+    if (queue == NULL) {
+      queue = seg;
+    }
+    /* subsequent segments of to-be-queued data */
+    else {
+      /* Attach the segment to the end of the queued segments */
+      LWIP_ASSERT("useg != NULL", useg != NULL);
+      useg->next = seg;
+    }
+    /* remember last segment of to-be-queued data for next iteration */
+    useg = seg;
+
+    /* If copy is set, memory should be allocated
+     * and data copied into pbuf, otherwise data comes from
+     * ROM or other static memory, and need not be copied. If
+     * optdata is != NULL, we have options instead of data. */
+     
+    /* options? */
+    if (optdata != NULL) {
+      if ((seg->p = pbuf_alloc(PBUF_TRANSPORT, optlen, PBUF_RAM)) == NULL) {
+        goto memerr;
+      }
+      ++queuelen;
+      seg->dataptr = seg->p->payload;
+    }
+    /* copy from volatile memory? */
+    else if (copy) {
+      if ((seg->p = pbuf_alloc(PBUF_TRANSPORT, seglen, PBUF_RAM)) == NULL) {
+        LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 2, ("tcp_enqueue : could not allocate memory for pbuf copy size %"U16_F"\n", seglen));
+        goto memerr;
+      }
+      ++queuelen;
+      if (arg != NULL) {
+        memcpy(seg->p->payload, ptr, seglen);
+      }
+      seg->dataptr = seg->p->payload;
+    }
+    /* do not copy data */
+    else {
+      /* First, allocate a pbuf for holding the data.
+       * since the referenced data is available at least until it is sent out on the
+       * link (as it has to be ACKed by the remote party) we can safely use PBUF_ROM
+       * instead of PBUF_REF here.
+       */
+      if ((p = pbuf_alloc(PBUF_TRANSPORT, seglen, PBUF_ROM)) == NULL) {
+        LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 2, ("tcp_enqueue: could not allocate memory for zero-copy pbuf\n"));
+        goto memerr;
+      }
+      ++queuelen;
+      /* reference the non-volatile payload data */
+      p->payload = ptr;
+      seg->dataptr = ptr;
+
+      /* Second, allocate a pbuf for the headers. */
+      if ((seg->p = pbuf_alloc(PBUF_TRANSPORT, 0, PBUF_RAM)) == NULL) {
+        /* If allocation fails, we have to deallocate the data pbuf as
+         * well. */
+        pbuf_free(p);
+        LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 2, ("tcp_enqueue: could not allocate memory for header pbuf\n"));
+        goto memerr;
+      }
+      ++queuelen;
+
+      /* Concatenate the headers and data pbufs together. */
+      pbuf_cat(seg->p/*header*/, p/*data*/);
+      p = NULL;
+    }
+
+    /* Now that there are more segments queued, we check again if the
+    length of the queue exceeds the configured maximum. */
+    if (queuelen > TCP_SND_QUEUELEN) {
+      LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 2, ("tcp_enqueue: queue too long %"U16_F" (%"U16_F")\n", queuelen, TCP_SND_QUEUELEN));
+      goto memerr;
+    }
+
+    seg->len = seglen;
+
+    /* build TCP header */
+    if (pbuf_header(seg->p, TCP_HLEN)) {
+      LWIP_DEBUGF(TCP_OUTPUT_DEBUG | 2, ("tcp_enqueue: no room for TCP header in pbuf.\n"));
+      TCP_STATS_INC(tcp.err);
+      goto memerr;
+    }
+    seg->tcphdr = seg->p->payload;
+    seg->tcphdr->src = htons(pcb->local_port);
+    seg->tcphdr->dest = htons(pcb->remote_port);
+    seg->tcphdr->seqno = htonl(seqno);
+    seg->tcphdr->urgp = 0;
+    TCPH_FLAGS_SET(seg->tcphdr, flags);
+    /* don't fill in tcphdr->ackno and tcphdr->wnd until later */
+
+    /* Copy the options into the header, if they are present. */
+    if (optdata == NULL) {
+      TCPH_HDRLEN_SET(seg->tcphdr, 5);
+    }
+    else {
+      TCPH_HDRLEN_SET(seg->tcphdr, (5 + optlen / 4));
+      /* Copy options into data portion of segment.
+       Options can thus only be sent in non data carrying
+       segments such as SYN|ACK. */
+      memcpy(seg->dataptr, optdata, optlen);
+    }
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG | DBG_TRACE, ("tcp_enqueue: queueing %"U32_F":%"U32_F" (0x%"X16_F")\n",
+      ntohl(seg->tcphdr->seqno),
+      ntohl(seg->tcphdr->seqno) + TCP_TCPLEN(seg),
+      (u16_t)flags));
+
+    left -= seglen;
+    seqno += seglen;
+    ptr = (void *)((u8_t *)ptr + seglen);
+  }
+
+  /* Now that the data to be enqueued has been broken up into TCP
+  segments in the queue variable, we add them to the end of the
+  pcb->unsent queue. */
+  if (pcb->unsent == NULL) {
+    useg = NULL;
+  }
+  else {
+    for (useg = pcb->unsent; useg->next != NULL; useg = useg->next);
+  }
+  /* { useg is last segment on the unsent queue, NULL if list is empty } */
+
+  /* If there is room in the last pbuf on the unsent queue,
+  chain the first pbuf on the queue together with that. */
+  if (useg != NULL &&
+    TCP_TCPLEN(useg) != 0 &&
+    !(TCPH_FLAGS(useg->tcphdr) & (TCP_SYN | TCP_FIN)) &&
+    !(flags & (TCP_SYN | TCP_FIN)) &&
+    /* fit within max seg size */
+    useg->len + queue->len <= pcb->mss) {
+    /* Remove TCP header from first segment of our to-be-queued list */
+    pbuf_header(queue->p, -TCP_HLEN);
+    pbuf_cat(useg->p, queue->p);
+    useg->len += queue->len;
+    useg->next = queue->next;
+
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG | DBG_TRACE | DBG_STATE, ("tcp_enqueue: chaining segments, new len %"U16_F"\n", useg->len));
+    if (seg == queue) {
+      seg = NULL;
+    }
+    memp_free(MEMP_TCP_SEG, queue);
+  }
+  else {
+    /* empty list */
+    if (useg == NULL) {
+      /* initialize list with this segment */
+      pcb->unsent = queue;
+    }
+    /* enqueue segment */
+    else {
+      useg->next = queue;
+    }
+  }
+  if ((flags & TCP_SYN) || (flags & TCP_FIN)) {
+    ++len;
+  }
+  pcb->snd_lbb += len;
+
+  pcb->snd_buf -= len;
+
+  /* update number of segments on the queues */
+  pcb->snd_queuelen = queuelen;
+  LWIP_DEBUGF(TCP_QLEN_DEBUG, ("tcp_enqueue: %"S16_F" (after enqueued)\n", pcb->snd_queuelen));
+  if (pcb->snd_queuelen != 0) {
+    LWIP_ASSERT("tcp_enqueue: valid queue length",
+      pcb->unacked != NULL || pcb->unsent != NULL);
+  }
+
+  /* Set the PSH flag in the last segment that we enqueued, but only
+  if the segment has data (indicated by seglen > 0). */
+  if (seg != NULL && seglen > 0 && seg->tcphdr != NULL) {
+    TCPH_SET_FLAG(seg->tcphdr, TCP_PSH);
+  }
+
+  return ERR_OK;
+memerr:
+  TCP_STATS_INC(tcp.memerr);
+
+  if (queue != NULL) {
+    tcp_segs_free(queue);
+  }
+  if (pcb->snd_queuelen != 0) {
+    LWIP_ASSERT("tcp_enqueue: valid queue length", pcb->unacked != NULL ||
+      pcb->unsent != NULL);
+  }
+  LWIP_DEBUGF(TCP_QLEN_DEBUG | DBG_STATE, ("tcp_enqueue: %"S16_F" (with mem err)\n", pcb->snd_queuelen));
+  return ERR_MEM;
+}
+
+/* find out what we can send and send it */
+err_t
+tcp_output(struct tcp_pcb *pcb)
+{
+  struct pbuf *p;
+  struct tcp_hdr *tcphdr;
+  struct tcp_seg *seg, *useg;
+  u32_t wnd;
+#if TCP_CWND_DEBUG
+  s16_t i = 0;
+#endif /* TCP_CWND_DEBUG */
+
+  /* First, check if we are invoked by the TCP input processing
+     code. If so, we do not output anything. Instead, we rely on the
+     input processing code to call us when input processing is done
+     with. */
+  if (tcp_input_pcb == pcb) {
+    return ERR_OK;
+  }
+
+  wnd = LWIP_MIN(pcb->snd_wnd, pcb->cwnd);
+
+  seg = pcb->unsent;
+
+  /* useg should point to last segment on unacked queue */
+  useg = pcb->unacked;
+  if (useg != NULL) {
+    for (; useg->next != NULL; useg = useg->next);
+  }                                                                             
+   
+  /* If the TF_ACK_NOW flag is set and no data will be sent (either
+   * because the ->unsent queue is empty or because the window does
+   * not allow it), construct an empty ACK segment and send it.
+   *
+   * If data is to be sent, we will just piggyback the ACK (see below).
+   */
+  if (pcb->flags & TF_ACK_NOW &&
+     (seg == NULL ||
+      ntohl(seg->tcphdr->seqno) - pcb->lastack + seg->len > wnd)) {
+    p = pbuf_alloc(PBUF_IP, TCP_HLEN, PBUF_RAM);
+    if (p == NULL) {
+      LWIP_DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_output: (ACK) could not allocate pbuf\n"));
+      return ERR_BUF;
+    }
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_output: sending ACK for %"U32_F"\n", pcb->rcv_nxt));
+    /* remove ACK flags from the PCB, as we send an empty ACK now */
+    pcb->flags &= ~(TF_ACK_DELAY | TF_ACK_NOW);
+
+    tcphdr = p->payload;
+    tcphdr->src = htons(pcb->local_port);
+    tcphdr->dest = htons(pcb->remote_port);
+    tcphdr->seqno = htonl(pcb->snd_nxt);
+    tcphdr->ackno = htonl(pcb->rcv_nxt);
+    TCPH_FLAGS_SET(tcphdr, TCP_ACK);
+    tcphdr->wnd = htons(pcb->rcv_wnd);
+    tcphdr->urgp = 0;
+    TCPH_HDRLEN_SET(tcphdr, 5);
+
+    tcphdr->chksum = 0;
+#if CHECKSUM_GEN_TCP
+    tcphdr->chksum = inet_chksum_pseudo(p, &(pcb->local_ip), &(pcb->remote_ip),
+          IP_PROTO_TCP, p->tot_len);
+#endif
+    ip_output(p, &(pcb->local_ip), &(pcb->remote_ip), pcb->ttl, pcb->tos,
+        IP_PROTO_TCP);
+    pbuf_free(p);
+
+    return ERR_OK;
+  }
+
+#if TCP_OUTPUT_DEBUG
+  if (seg == NULL) {
+    LWIP_DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_output: nothing to send (%p)\n", (void*)pcb->unsent));
+  }
+#endif /* TCP_OUTPUT_DEBUG */
+#if TCP_CWND_DEBUG
+  if (seg == NULL) {
+    LWIP_DEBUGF(TCP_CWND_DEBUG, ("tcp_output: snd_wnd %"U32_F", cwnd %"U16_F", wnd %"U32_F", seg == NULL, ack %"U32_F"\n",
+                            pcb->snd_wnd, pcb->cwnd, wnd,
+                            pcb->lastack));
+  } else {
+    LWIP_DEBUGF(TCP_CWND_DEBUG, ("tcp_output: snd_wnd %"U32_F", cwnd %"U16_F", wnd %"U32_F", effwnd %"U32_F", seq %"U32_F", ack %"U32_F"\n",
+                            pcb->snd_wnd, pcb->cwnd, wnd,
+                            ntohl(seg->tcphdr->seqno) - pcb->lastack + seg->len,
+                            ntohl(seg->tcphdr->seqno), pcb->lastack));
+  }
+#endif /* TCP_CWND_DEBUG */
+  /* data available and window allows it to be sent? */
+  while (seg != NULL &&
+  ntohl(seg->tcphdr->seqno) - pcb->lastack + seg->len <= wnd) {
+#if TCP_CWND_DEBUG
+    LWIP_DEBUGF(TCP_CWND_DEBUG, ("tcp_output: snd_wnd %"U32_F", cwnd %"U16_F", wnd %"U32_F", effwnd %"U32_F", seq %"U32_F", ack %"U32_F", i %"S16_F"\n",
+                            pcb->snd_wnd, pcb->cwnd, wnd,
+                            ntohl(seg->tcphdr->seqno) + seg->len -
+                            pcb->lastack,
+                            ntohl(seg->tcphdr->seqno), pcb->lastack, i));
+    ++i;
+#endif /* TCP_CWND_DEBUG */
+
+    pcb->unsent = seg->next;
+
+    if (pcb->state != SYN_SENT) {
+      TCPH_SET_FLAG(seg->tcphdr, TCP_ACK);
+      pcb->flags &= ~(TF_ACK_DELAY | TF_ACK_NOW);
+    }
+
+    tcp_output_segment(seg, pcb);
+    pcb->snd_nxt = ntohl(seg->tcphdr->seqno) + TCP_TCPLEN(seg);
+    if (TCP_SEQ_LT(pcb->snd_max, pcb->snd_nxt)) {
+      pcb->snd_max = pcb->snd_nxt;
+    }
+    /* put segment on unacknowledged list if length > 0 */
+    if (TCP_TCPLEN(seg) > 0) {
+      seg->next = NULL;
+      /* unacked list is empty? */
+      if (pcb->unacked == NULL) {
+        pcb->unacked = seg;
+        useg = seg;
+      /* unacked list is not empty? */
+      } else {
+        /* In the case of fast retransmit, the packet should not go to the tail
+         * of the unacked queue, but rather at the head. We need to check for
+         * this case. -STJ Jul 27, 2004 */
+        if (TCP_SEQ_LT(ntohl(seg->tcphdr->seqno), ntohl(useg->tcphdr->seqno))){
+          /* add segment to head of unacked list */
+          seg->next = pcb->unacked;
+          pcb->unacked = seg;
+        } else {
+          /* add segment to tail of unacked list */
+          useg->next = seg;
+          useg = useg->next;
+        }
+      }
+    /* do not queue empty segments on the unacked list */
+    } else {
+      tcp_seg_free(seg);
+    }
+    seg = pcb->unsent;
+  }
+  return ERR_OK;
+}
+
+/**
+ * Actually send a TCP segment over IP
+ */
+static void
+tcp_output_segment(struct tcp_seg *seg, struct tcp_pcb *pcb)
+{
+  u16_t len;
+  struct netif *netif;
+
+  /** @bug Exclude retransmitted segments from this count. */
+  snmp_inc_tcpoutsegs();
+
+  /* The TCP header has already been constructed, but the ackno and
+   wnd fields remain. */
+  seg->tcphdr->ackno = htonl(pcb->rcv_nxt);
+
+  /* silly window avoidance */
+  if (pcb->rcv_wnd < pcb->mss) {
+    seg->tcphdr->wnd = 0;
+  } else {
+    /* advertise our receive window size in this TCP segment */
+    seg->tcphdr->wnd = htons(pcb->rcv_wnd);
+  }
+
+  /* If we don't have a local IP address, we get one by
+     calling ip_route(). */
+  if (ip_addr_isany(&(pcb->local_ip))) {
+    netif = ip_route(&(pcb->remote_ip));
+    if (netif == NULL) {
+      return;
+    }
+    ip_addr_set(&(pcb->local_ip), &(netif->ip_addr));
+  }
+
+  pcb->rtime = 0;
+
+  if (pcb->rttest == 0) {
+    pcb->rttest = tcp_ticks;
+    pcb->rtseq = ntohl(seg->tcphdr->seqno);
+
+    LWIP_DEBUGF(TCP_RTO_DEBUG, ("tcp_output_segment: rtseq %"U32_F"\n", pcb->rtseq));
+  }
+  LWIP_DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_output_segment: %"U32_F":%"U32_F"\n",
+          htonl(seg->tcphdr->seqno), htonl(seg->tcphdr->seqno) +
+          seg->len));
+
+  len = (u16_t)((u8_t *)seg->tcphdr - (u8_t *)seg->p->payload);
+
+  seg->p->len -= len;
+  seg->p->tot_len -= len;
+
+  seg->p->payload = seg->tcphdr;
+
+  seg->tcphdr->chksum = 0;
+#if CHECKSUM_GEN_TCP
+  seg->tcphdr->chksum = inet_chksum_pseudo(seg->p,
+             &(pcb->local_ip),
+             &(pcb->remote_ip),
+             IP_PROTO_TCP, seg->p->tot_len);
+#endif
+  TCP_STATS_INC(tcp.xmit);
+
+  ip_output(seg->p, &(pcb->local_ip), &(pcb->remote_ip), pcb->ttl, pcb->tos,
+      IP_PROTO_TCP);
+}
+
+void
+tcp_rst(u32_t seqno, u32_t ackno,
+  struct ip_addr *local_ip, struct ip_addr *remote_ip,
+  u16_t local_port, u16_t remote_port)
+{
+  struct pbuf *p;
+  struct tcp_hdr *tcphdr;
+  p = pbuf_alloc(PBUF_IP, TCP_HLEN, PBUF_RAM);
+  if (p == NULL) {
+      LWIP_DEBUGF(TCP_DEBUG, ("tcp_rst: could not allocate memory for pbuf\n"));
+      return;
+  }
+
+  tcphdr = p->payload;
+  tcphdr->src = htons(local_port);
+  tcphdr->dest = htons(remote_port);
+  tcphdr->seqno = htonl(seqno);
+  tcphdr->ackno = htonl(ackno);
+  TCPH_FLAGS_SET(tcphdr, TCP_RST | TCP_ACK);
+  tcphdr->wnd = htons(TCP_WND);
+  tcphdr->urgp = 0;
+  TCPH_HDRLEN_SET(tcphdr, 5);
+
+  tcphdr->chksum = 0;
+#if CHECKSUM_GEN_TCP
+  tcphdr->chksum = inet_chksum_pseudo(p, local_ip, remote_ip,
+              IP_PROTO_TCP, p->tot_len);
+#endif
+  TCP_STATS_INC(tcp.xmit);
+  snmp_inc_tcpoutrsts();
+   /* Send output with hardcoded TTL since we have no access to the pcb */
+  ip_output(p, local_ip, remote_ip, TCP_TTL, 0, IP_PROTO_TCP);
+  pbuf_free(p);
+  LWIP_DEBUGF(TCP_RST_DEBUG, ("tcp_rst: seqno %"U32_F" ackno %"U32_F".\n", seqno, ackno));
+}
+
+/* requeue all unacked segments for retransmission */
+void
+tcp_rexmit_rto(struct tcp_pcb *pcb)
+{
+  struct tcp_seg *seg;
+
+  if (pcb->unacked == NULL) {
+    return;
+  }
+
+  /* Move all unacked segments to the head of the unsent queue */
+  for (seg = pcb->unacked; seg->next != NULL; seg = seg->next);
+  /* concatenate unsent queue after unacked queue */
+  seg->next = pcb->unsent;
+  /* unsent queue is the concatenated queue (of unacked, unsent) */
+  pcb->unsent = pcb->unacked;
+  /* unacked queue is now empty */
+  pcb->unacked = NULL;
+
+  pcb->snd_nxt = ntohl(pcb->unsent->tcphdr->seqno);
+  /* increment number of retransmissions */
+  ++pcb->nrtx;
+
+  /* Don't take any RTT measurements after retransmitting. */
+  pcb->rttest = 0;
+
+  /* Do the actual retransmission */
+  tcp_output(pcb);
+}
+
+void
+tcp_rexmit(struct tcp_pcb *pcb)
+{
+  struct tcp_seg *seg;
+
+  if (pcb->unacked == NULL) {
+    return;
+  }
+
+  /* Move the first unacked segment to the unsent queue */
+  seg = pcb->unacked->next;
+  pcb->unacked->next = pcb->unsent;
+  pcb->unsent = pcb->unacked;
+  pcb->unacked = seg;
+
+  pcb->snd_nxt = ntohl(pcb->unsent->tcphdr->seqno);
+
+  ++pcb->nrtx;
+
+  /* Don't take any rtt measurements after retransmitting. */
+  pcb->rttest = 0;
+
+  /* Do the actual retransmission. */
+  snmp_inc_tcpretranssegs();
+  tcp_output(pcb);
+
+}
+
+
+void
+tcp_keepalive(struct tcp_pcb *pcb)
+{
+   struct pbuf *p;
+   struct tcp_hdr *tcphdr;
+
+   LWIP_DEBUGF(TCP_DEBUG, ("tcp_keepalive: sending KEEPALIVE probe to %"U16_F".%"U16_F".%"U16_F".%"U16_F"\n",
+                           ip4_addr1(&pcb->remote_ip), ip4_addr2(&pcb->remote_ip),
+                           ip4_addr3(&pcb->remote_ip), ip4_addr4(&pcb->remote_ip)));
+
+   LWIP_DEBUGF(TCP_DEBUG, ("tcp_keepalive: tcp_ticks %"U32_F"   pcb->tmr %"U32_F"  pcb->keep_cnt %"U16_F"\n", tcp_ticks, pcb->tmr, pcb->keep_cnt));
+   
+   p = pbuf_alloc(PBUF_IP, TCP_HLEN, PBUF_RAM);
+
+   if(p == NULL) {
+      LWIP_DEBUGF(TCP_DEBUG, ("tcp_keepalive: could not allocate memory for pbuf\n"));
+      return;
+   }
+
+   tcphdr = p->payload;
+   tcphdr->src = htons(pcb->local_port);
+   tcphdr->dest = htons(pcb->remote_port);
+   tcphdr->seqno = htonl(pcb->snd_nxt - 1);
+   tcphdr->ackno = htonl(pcb->rcv_nxt);
+   tcphdr->wnd = htons(pcb->rcv_wnd);
+   tcphdr->urgp = 0;
+   TCPH_HDRLEN_SET(tcphdr, 5);
+   
+   tcphdr->chksum = 0;
+#if CHECKSUM_GEN_TCP
+   tcphdr->chksum = inet_chksum_pseudo(p, &pcb->local_ip, &pcb->remote_ip, IP_PROTO_TCP, p->tot_len);
+#endif
+  TCP_STATS_INC(tcp.xmit);
+
+   /* Send output to IP */
+  ip_output(p, &pcb->local_ip, &pcb->remote_ip, pcb->ttl, 0, IP_PROTO_TCP);
+
+  pbuf_free(p);
+
+  LWIP_DEBUGF(TCP_RST_DEBUG, ("tcp_keepalive: seqno %"U32_F" ackno %"U32_F".\n", pcb->snd_nxt - 1, pcb->rcv_nxt));
+}
+
+#endif /* LWIP_TCP */
+
+
+
+
+
+
+
+
+

lwip/core/udp.c

@@ -0,0 +1,665 @@
+/**
+ * @file
+ * User Datagram Protocol module
+ *
+ */
+/*
+ * Copyright (c) 2001-2004 Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+
+/* udp.c
+ *
+ * The code for the User Datagram Protocol UDP.
+ *
+ */
+
+#include <string.h>
+
+#include "lwip/opt.h"
+
+#include "lwip/def.h"
+#include "lwip/memp.h"
+#include "lwip/inet.h"
+#include "lwip/ip_addr.h"
+#include "lwip/netif.h"
+#include "lwip/udp.h"
+#include "lwip/icmp.h"
+
+#include "lwip/stats.h"
+
+#include "arch/perf.h"
+#include "lwip/snmp.h"
+
+/* The list of UDP PCBs */
+#if LWIP_UDP
+/* exported in udp.h (was static) */
+struct udp_pcb *udp_pcbs = NULL;
+
+static struct udp_pcb *pcb_cache = NULL;
+
+void
+udp_init(void)
+{
+  udp_pcbs = pcb_cache = NULL;
+}
+
+/**
+ * Process an incoming UDP datagram.
+ *
+ * Given an incoming UDP datagram (as a chain of pbufs) this function
+ * finds a corresponding UDP PCB and
+ *
+ * @param pbuf pbuf to be demultiplexed to a UDP PCB.
+ * @param netif network interface on which the datagram was received.
+ *
+ */
+void
+udp_input(struct pbuf *p, struct netif *inp)
+{
+  struct udp_hdr *udphdr;
+  struct udp_pcb *pcb;
+  struct udp_pcb *uncon_pcb;
+  struct ip_hdr *iphdr;
+  u16_t src, dest;
+  u8_t local_match;
+
+  PERF_START;
+
+  UDP_STATS_INC(udp.recv);
+
+  iphdr = p->payload;
+
+  if (p->tot_len < (IPH_HL(iphdr) * 4 + UDP_HLEN)) {
+    /* drop short packets */
+//    LWIP_DEBUGF(UDP_DEBUG,
+//                ("udp_input: short UDP datagram (%"U16_F" bytes) discarded\n", p->tot_len));
+
+    LWIP_DEBUGF(UDP_DEBUG,
+                ("udp_input: short UDP datagram (%u bytes) discarded\n", p->tot_len));
+
+    UDP_STATS_INC(udp.lenerr);
+    UDP_STATS_INC(udp.drop);
+    snmp_inc_udpinerrors();
+    pbuf_free(p);
+    goto end;
+  }
+
+  pbuf_header(p, -((s16_t)(IPH_HL(iphdr) * 4)));
+
+  udphdr = (struct udp_hdr *)p->payload;
+
+  LWIP_DEBUGF(UDP_DEBUG, ("udp_input: received datagram of length %"U16_F"\n", p->tot_len));
+
+  src = ntohs(udphdr->src);
+  dest = ntohs(udphdr->dest);
+
+  udp_debug_print(udphdr);
+
+  /* print the UDP source and destination */
+  LWIP_DEBUGF(UDP_DEBUG,
+              ("udp (%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F") <-- "
+               "(%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F")\n",
+               ip4_addr1(&iphdr->dest), ip4_addr2(&iphdr->dest),
+               ip4_addr3(&iphdr->dest), ip4_addr4(&iphdr->dest), ntohs(udphdr->dest),
+               ip4_addr1(&iphdr->src), ip4_addr2(&iphdr->src),
+               ip4_addr3(&iphdr->src), ip4_addr4(&iphdr->src), ntohs(udphdr->src)));
+
+  local_match = 0;
+  uncon_pcb = NULL;
+  /* Iterate through the UDP pcb list for a matching pcb */
+  for (pcb = udp_pcbs; pcb != NULL; pcb = pcb->next) {
+    /* print the PCB local and remote address */
+    LWIP_DEBUGF(UDP_DEBUG,
+                ("pcb (%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F") --- "
+                 "(%"U16_F".%"U16_F".%"U16_F".%"U16_F", %"U16_F")\n",
+                 ip4_addr1(&pcb->local_ip), ip4_addr2(&pcb->local_ip),
+                 ip4_addr3(&pcb->local_ip), ip4_addr4(&pcb->local_ip), pcb->local_port,
+                 ip4_addr1(&pcb->remote_ip), ip4_addr2(&pcb->remote_ip),
+                 ip4_addr3(&pcb->remote_ip), ip4_addr4(&pcb->remote_ip), pcb->remote_port));
+
+    /* compare PCB local addr+port to UDP destination addr+port */
+    if ((pcb->local_port == dest) &&
+        (ip_addr_isany(&pcb->local_ip) ||
+         ip_addr_cmp(&(pcb->local_ip), &(iphdr->dest)) || 
+         ip_addr_isbroadcast(&(iphdr->dest), inp))) {
+      local_match = 1;
+      if ((uncon_pcb == NULL) && 
+          ((pcb->flags & UDP_FLAGS_CONNECTED) == 0)) {
+        /* the first unconnected matching PCB */     
+        uncon_pcb = pcb;
+      }
+    }
+    /* compare PCB remote addr+port to UDP source addr+port */
+    if ((local_match != 0) &&
+        (pcb->remote_port == src) &&
+        (ip_addr_isany(&pcb->remote_ip) ||
+         ip_addr_cmp(&(pcb->remote_ip), &(iphdr->src)))) {
+      /* the first fully matching PCB */
+      break;
+    }
+  }
+  /* no fully matching pcb found? then look for an unconnected pcb */
+  if (pcb == NULL) {
+    pcb = uncon_pcb;
+  }
+
+  /* Check checksum if this is a match or if it was directed at us. */
+  if (pcb != NULL || ip_addr_cmp(&inp->ip_addr, &iphdr->dest)) {
+    LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE, ("udp_input: calculating checksum\n"));
+#ifdef IPv6
+    if (iphdr->nexthdr == IP_PROTO_UDPLITE) {
+#else
+    if (IPH_PROTO(iphdr) == IP_PROTO_UDPLITE) {
+#endif /* IPv4 */
+      /* Do the UDP Lite checksum */
+#if CHECKSUM_CHECK_UDP
+      if (inet_chksum_pseudo(p, (struct ip_addr *)&(iphdr->src),
+                             (struct ip_addr *)&(iphdr->dest),
+                             IP_PROTO_UDPLITE, ntohs(udphdr->len)) != 0) {
+        LWIP_DEBUGF(UDP_DEBUG | 2,
+                    ("udp_input: UDP Lite datagram discarded due to failing checksum\n"));
+        UDP_STATS_INC(udp.chkerr);
+        UDP_STATS_INC(udp.drop);
+        snmp_inc_udpinerrors();
+        pbuf_free(p);
+        goto end;
+      }
+#endif
+    } else {
+#if CHECKSUM_CHECK_UDP
+      if (udphdr->chksum != 0) {
+        if (inet_chksum_pseudo(p, (struct ip_addr *)&(iphdr->src),
+                               (struct ip_addr *)&(iphdr->dest),
+                               IP_PROTO_UDP, p->tot_len) != 0) {
+          LWIP_DEBUGF(UDP_DEBUG | 2,
+                      ("udp_input: UDP datagram discarded due to failing checksum\n"));
+          UDP_STATS_INC(udp.chkerr);
+          UDP_STATS_INC(udp.drop);
+          snmp_inc_udpinerrors();
+          pbuf_free(p);
+          goto end;
+        }
+      }
+#endif
+    }
+    pbuf_header(p, -UDP_HLEN);
+    if (pcb != NULL) {
+      snmp_inc_udpindatagrams();
+      /* callback */
+      if (pcb->recv != NULL)
+        pcb->recv(pcb->recv_arg, pcb, p, &(iphdr->src), src);
+    } else {
+      LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE, ("udp_input: not for us.\n"));
+
+      /* No match was found, send ICMP destination port unreachable unless
+         destination address was broadcast/multicast. */
+
+      if (!ip_addr_isbroadcast(&iphdr->dest, inp) &&
+          !ip_addr_ismulticast(&iphdr->dest)) {
+
+        /* restore pbuf pointer */
+        p->payload = iphdr;
+        icmp_dest_unreach(p, ICMP_DUR_PORT);
+      }
+      UDP_STATS_INC(udp.proterr);
+      UDP_STATS_INC(udp.drop);
+      snmp_inc_udpnoports();
+      pbuf_free(p);
+    }
+  } else {
+    pbuf_free(p);
+  }
+end:
+  PERF_STOP("udp_input");
+}
+
+/**
+ * Send data to a specified address using UDP.
+ *
+ * @param pcb UDP PCB used to send the data.
+ * @param pbuf chain of pbuf's to be sent.
+ * @param dst_ip Destination IP address.
+ * @param dst_port Destination UDP port.
+ *
+ * If the PCB already has a remote address association, it will
+ * be restored after the data is sent.
+ * 
+ * @return lwIP error code.
+ * - ERR_OK. Successful. No error occured.
+ * - ERR_MEM. Out of memory.
+ * - ERR_RTE. Could not find route to destination address.
+ *
+ * @see udp_disconnect() udp_send()
+ */
+err_t
+udp_sendto(struct udp_pcb *pcb, struct pbuf *p,
+  struct ip_addr *dst_ip, u16_t dst_port)
+{
+  err_t err;
+  /* temporary space for current PCB remote address */
+  struct ip_addr pcb_remote_ip;
+  u16_t pcb_remote_port;
+  /* remember current remote peer address of PCB */
+  pcb_remote_ip.addr = pcb->remote_ip.addr;
+  pcb_remote_port = pcb->remote_port;
+  /* copy packet destination address to PCB remote peer address */
+  pcb->remote_ip.addr = dst_ip->addr;
+  pcb->remote_port = dst_port;
+  /* send to the packet destination address */
+  err = udp_send(pcb, p);
+  /* restore PCB remote peer address */
+  pcb->remote_ip.addr = pcb_remote_ip.addr;
+  pcb->remote_port = pcb_remote_port;
+  return err;
+}
+
+/**
+ * Send data using UDP.
+ *
+ * @param pcb UDP PCB used to send the data.
+ * @param pbuf chain of pbuf's to be sent.
+ *
+ * @return lwIP error code.
+ * - ERR_OK. Successful. No error occured.
+ * - ERR_MEM. Out of memory.
+ * - ERR_RTE. Could not find route to destination address.
+ *
+ * @see udp_disconnect() udp_sendto()
+ */
+err_t
+udp_send(struct udp_pcb *pcb, struct pbuf *p)
+{
+  struct udp_hdr *udphdr;
+  struct netif *netif;
+  struct ip_addr *src_ip;
+  err_t err;
+  struct pbuf *q; /* q will be sent down the stack */
+
+  LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | 3, ("udp_send\n"));
+
+  /* if the PCB is not yet bound to a port, bind it here */
+  if (pcb->local_port == 0) {
+    LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | 2, ("udp_send: not yet bound to a port, binding now\n"));
+    err = udp_bind(pcb, &pcb->local_ip, pcb->local_port);
+    if (err != ERR_OK) {
+      LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | 2, ("udp_send: forced port bind failed\n"));
+      return err;
+    }
+  }
+  /* find the outgoing network interface for this packet */
+  netif = ip_route(&(pcb->remote_ip));
+  /* no outgoing network interface could be found? */
+  if (netif == NULL) {
+    LWIP_DEBUGF(UDP_DEBUG | 1, ("udp_send: No route to 0x%"X32_F"\n", pcb->remote_ip.addr));
+    UDP_STATS_INC(udp.rterr);
+    return ERR_RTE;
+  }
+
+  /* not enough space to add an UDP header to first pbuf in given p chain? */
+  if (pbuf_header(p, UDP_HLEN)) {
+    /* allocate header in a seperate new pbuf */
+    q = pbuf_alloc(PBUF_IP, UDP_HLEN, PBUF_RAM);
+    /* new header pbuf could not be allocated? */
+    if (q == NULL) {
+      LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | 2, ("udp_send: could not allocate header\n"));
+      return ERR_MEM;
+    }
+    /* chain header q in front of given pbuf p */
+    pbuf_chain(q, p);
+    /* first pbuf q points to header pbuf */
+    LWIP_DEBUGF(UDP_DEBUG,
+                ("udp_send: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
+    /* adding a header within p succeeded */
+  } else {
+    /* first pbuf q equals given pbuf */
+    q = p;
+    LWIP_DEBUGF(UDP_DEBUG, ("udp_send: added header in given pbuf %p\n", (void *)p));
+  }
+  /* q now represents the packet to be sent */
+  udphdr = q->payload;
+  udphdr->src = htons(pcb->local_port);
+  udphdr->dest = htons(pcb->remote_port);
+  /* in UDP, 0 checksum means 'no checksum' */
+  udphdr->chksum = 0x0000; 
+
+  /* PCB local address is IP_ANY_ADDR? */
+  if (ip_addr_isany(&pcb->local_ip)) {
+    /* use outgoing network interface IP address as source address */
+    src_ip = &(netif->ip_addr);
+  } else {
+    /* use UDP PCB local IP address as source address */
+    src_ip = &(pcb->local_ip);
+  }
+
+  LWIP_DEBUGF(UDP_DEBUG, ("udp_send: sending datagram of length %"U16_F"\n", q->tot_len));
+
+  /* UDP Lite protocol? */
+  if (pcb->flags & UDP_FLAGS_UDPLITE) {
+    LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP LITE packet length %"U16_F"\n", q->tot_len));
+    /* set UDP message length in UDP header */
+    udphdr->len = htons(pcb->chksum_len);
+    /* calculate checksum */
+#if CHECKSUM_GEN_UDP
+    udphdr->chksum = inet_chksum_pseudo(q, src_ip, &(pcb->remote_ip),
+                                        IP_PROTO_UDP, pcb->chksum_len);
+    /* chksum zero must become 0xffff, as zero means 'no checksum' */
+    if (udphdr->chksum == 0x0000)
+      udphdr->chksum = 0xffff;
+#else
+    udphdr->chksum = 0x0000;
+#endif
+    /* output to IP */
+    LWIP_DEBUGF(UDP_DEBUG, ("udp_send: ip_output_if (,,,,IP_PROTO_UDPLITE,)\n"));
+    err = ip_output_if(q, src_ip, &pcb->remote_ip, pcb->ttl, pcb->tos, IP_PROTO_UDPLITE, netif);    
+  } else {      /* UDP */
+    LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP packet length %"U16_F"\n", q->tot_len));
+    udphdr->len = htons(q->tot_len);
+    /* calculate checksum */
+#if CHECKSUM_GEN_UDP
+    if ((pcb->flags & UDP_FLAGS_NOCHKSUM) == 0) {
+      udphdr->chksum = inet_chksum_pseudo(q, src_ip, &pcb->remote_ip, IP_PROTO_UDP, q->tot_len);
+      /* chksum zero must become 0xffff, as zero means 'no checksum' */
+      if (udphdr->chksum == 0x0000) udphdr->chksum = 0xffff;
+    }
+#else
+    udphdr->chksum = 0x0000;
+#endif
+    LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP checksum 0x%04"X16_F"\n", udphdr->chksum));
+    LWIP_DEBUGF(UDP_DEBUG, ("udp_send: ip_output_if (,,,,IP_PROTO_UDP,)\n"));
+    /* output to IP */
+    err = ip_output_if(q, src_ip, &pcb->remote_ip, pcb->ttl, pcb->tos, IP_PROTO_UDP, netif);    
+  }
+  /* TODO: must this be increased even if error occured? */
+  snmp_inc_udpoutdatagrams();
+
+  /* did we chain a seperate header pbuf earlier? */
+  if (q != p) {
+    /* free the header pbuf */
+    pbuf_free(q);
+    q = NULL;
+    /* p is still referenced by the caller, and will live on */
+  }
+
+  UDP_STATS_INC(udp.xmit);
+  return err;
+}
+
+/**
+ * Bind an UDP PCB.
+ *
+ * @param pcb UDP PCB to be bound with a local address ipaddr and port.
+ * @param ipaddr local IP address to bind with. Use IP_ADDR_ANY to
+ * bind to all local interfaces.
+ * @param port local UDP port to bind with.
+ *
+ * @return lwIP error code.
+ * - ERR_OK. Successful. No error occured.
+ * - ERR_USE. The specified ipaddr and port are already bound to by
+ * another UDP PCB.
+ *
+ * @see udp_disconnect()
+ */
+err_t
+udp_bind(struct udp_pcb *pcb, struct ip_addr *ipaddr, u16_t port)
+{
+  struct udp_pcb *ipcb;
+  u8_t rebind;
+
+  LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | 3, ("udp_bind(ipaddr = "));
+  ip_addr_debug_print(UDP_DEBUG, ipaddr);
+  LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | 3, (", port = %"U16_F")\n", port));
+
+  rebind = 0;
+  /* Check for double bind and rebind of the same pcb */
+  for (ipcb = udp_pcbs; ipcb != NULL; ipcb = ipcb->next) {
+    /* is this UDP PCB already on active list? */
+    if (pcb == ipcb) {
+      /* pcb may occur at most once in active list */
+      LWIP_ASSERT("rebind == 0", rebind == 0);
+      /* pcb already in list, just rebind */
+      rebind = 1;
+    }
+
+    /* this code does not allow upper layer to share a UDP port for
+       listening to broadcast or multicast traffic (See SO_REUSE_ADDR and
+       SO_REUSE_PORT under *BSD). TODO: See where it fits instead, OR
+       combine with implementation of UDP PCB flags. Leon Woestenberg. */
+#ifdef LWIP_UDP_TODO
+    /* port matches that of PCB in list? */
+    else
+      if ((ipcb->local_port == port) &&
+          /* IP address matches, or one is IP_ADDR_ANY? */
+          (ip_addr_isany(&(ipcb->local_ip)) ||
+           ip_addr_isany(ipaddr) ||
+           ip_addr_cmp(&(ipcb->local_ip), ipaddr))) {
+        /* other PCB already binds to this local IP and port */
+        LWIP_DEBUGF(UDP_DEBUG,
+                    ("udp_bind: local port %"U16_F" already bound by another pcb\n", port));
+        return ERR_USE;
+      }
+#endif
+  }
+
+  ip_addr_set(&pcb->local_ip, ipaddr);
+
+  /* no port specified? */
+  if (port == 0) {
+#ifndef UDP_LOCAL_PORT_RANGE_START
+#define UDP_LOCAL_PORT_RANGE_START 4096
+#define UDP_LOCAL_PORT_RANGE_END   0x7fff
+#endif
+    port = UDP_LOCAL_PORT_RANGE_START;
+    ipcb = udp_pcbs;
+    while ((ipcb != NULL) && (port != UDP_LOCAL_PORT_RANGE_END)) {
+      if (ipcb->local_port == port) {
+        port++;
+        ipcb = udp_pcbs;
+      } else
+        ipcb = ipcb->next;
+    }
+    if (ipcb != NULL) {
+      /* no more ports available in local range */
+      LWIP_DEBUGF(UDP_DEBUG, ("udp_bind: out of free UDP ports\n"));
+      return ERR_USE;
+    }
+  }
+  pcb->local_port = port;
+  snmp_insert_udpidx_tree(pcb);
+  /* pcb not active yet? */
+  if (rebind == 0) {
+    /* place the PCB on the active list if not already there */
+    pcb->next = udp_pcbs;
+    udp_pcbs = pcb;
+  }
+  LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | DBG_STATE,
+              ("udp_bind: bound to %"U16_F".%"U16_F".%"U16_F".%"U16_F", port %"U16_F"\n",
+               (u16_t)(ntohl(pcb->local_ip.addr) >> 24 & 0xff),
+               (u16_t)(ntohl(pcb->local_ip.addr) >> 16 & 0xff),
+               (u16_t)(ntohl(pcb->local_ip.addr) >> 8 & 0xff),
+               (u16_t)(ntohl(pcb->local_ip.addr) & 0xff), pcb->local_port));
+  return ERR_OK;
+}
+/**
+ * Connect an UDP PCB.
+ *
+ * This will associate the UDP PCB with the remote address.
+ *
+ * @param pcb UDP PCB to be connected with remote address ipaddr and port.
+ * @param ipaddr remote IP address to connect with.
+ * @param port remote UDP port to connect with.
+ *
+ * @return lwIP error code
+ *
+ * @see udp_disconnect()
+ */
+err_t
+udp_connect(struct udp_pcb *pcb, struct ip_addr *ipaddr, u16_t port)
+{
+  struct udp_pcb *ipcb;
+
+  if (pcb->local_port == 0) {
+    err_t err = udp_bind(pcb, &pcb->local_ip, pcb->local_port);
+    if (err != ERR_OK)
+      return err;
+  }
+
+  ip_addr_set(&pcb->remote_ip, ipaddr);
+  pcb->remote_port = port;
+  pcb->flags |= UDP_FLAGS_CONNECTED;
+/** TODO: this functionality belongs in upper layers */
+#ifdef LWIP_UDP_TODO
+  /* Nail down local IP for netconn_addr()/getsockname() */
+  if (ip_addr_isany(&pcb->local_ip) && !ip_addr_isany(&pcb->remote_ip)) {
+    struct netif *netif;
+
+    if ((netif = ip_route(&(pcb->remote_ip))) == NULL) {
+      LWIP_DEBUGF(UDP_DEBUG, ("udp_connect: No route to 0x%lx\n", pcb->remote_ip.addr));
+      UDP_STATS_INC(udp.rterr);
+      return ERR_RTE;
+    }
+    /** TODO: this will bind the udp pcb locally, to the interface which
+        is used to route output packets to the remote address. However, we
+        might want to accept incoming packets on any interface! */
+    pcb->local_ip = netif->ip_addr;
+  } else if (ip_addr_isany(&pcb->remote_ip)) {
+    pcb->local_ip.addr = 0;
+  }
+#endif
+  LWIP_DEBUGF(UDP_DEBUG | DBG_TRACE | DBG_STATE,
+              ("udp_connect: connected to %"U16_F".%"U16_F".%"U16_F".%"U16_F",port %"U16_F"\n",
+               (u16_t)(ntohl(pcb->remote_ip.addr) >> 24 & 0xff),
+               (u16_t)(ntohl(pcb->remote_ip.addr) >> 16 & 0xff),
+               (u16_t)(ntohl(pcb->remote_ip.addr) >> 8 & 0xff),
+               (u16_t)(ntohl(pcb->remote_ip.addr) & 0xff), pcb->remote_port));
+
+  /* Insert UDP PCB into the list of active UDP PCBs. */
+  for (ipcb = udp_pcbs; ipcb != NULL; ipcb = ipcb->next) {
+    if (pcb == ipcb) {
+      /* already on the list, just return */
+      return ERR_OK;
+    }
+  }
+  /* PCB not yet on the list, add PCB now */
+  pcb->next = udp_pcbs;
+  udp_pcbs = pcb;
+  return ERR_OK;
+}
+
+void
+udp_disconnect(struct udp_pcb *pcb)
+{
+  /* reset remote address association */
+  ip_addr_set(&pcb->remote_ip, IP_ADDR_ANY);
+  pcb->remote_port = 0;
+  /* mark PCB as unconnected */
+  pcb->flags &= ~UDP_FLAGS_CONNECTED;
+}
+
+void
+udp_recv(struct udp_pcb *pcb,
+         void (* recv)(void *arg, struct udp_pcb *upcb, struct pbuf *p,
+                       struct ip_addr *addr, u16_t port),
+         void *recv_arg)
+{
+  /* remember recv() callback and user data */
+  pcb->recv = recv;
+  pcb->recv_arg = recv_arg;
+}
+
+/**
+ * Remove an UDP PCB.
+ *
+ * @param pcb UDP PCB to be removed. The PCB is removed from the list of
+ * UDP PCB's and the data structure is freed from memory.
+ *
+ * @see udp_new()
+ */
+void
+udp_remove(struct udp_pcb *pcb)
+{
+  struct udp_pcb *pcb2;
+
+  snmp_delete_udpidx_tree(pcb);
+  /* pcb to be removed is first in list? */
+  if (udp_pcbs == pcb) {
+    /* make list start at 2nd pcb */
+    udp_pcbs = udp_pcbs->next;
+    /* pcb not 1st in list */
+  } else
+    for (pcb2 = udp_pcbs; pcb2 != NULL; pcb2 = pcb2->next) {
+      /* find pcb in udp_pcbs list */
+      if (pcb2->next != NULL && pcb2->next == pcb) {
+        /* remove pcb from list */
+        pcb2->next = pcb->next;
+      }
+    }
+  memp_free(MEMP_UDP_PCB, pcb);
+}
+
+/**
+ * Create a UDP PCB.
+ *
+ * @return The UDP PCB which was created. NULL if the PCB data structure
+ * could not be allocated.
+ *
+ * @see udp_remove()
+ */
+struct udp_pcb *
+udp_new(void)
+{
+  struct udp_pcb *pcb;
+  pcb = memp_malloc(MEMP_UDP_PCB);
+  /* could allocate UDP PCB? */
+  if (pcb != NULL) {
+    /* initialize PCB to all zeroes */
+    memset(pcb, 0, sizeof(struct udp_pcb));
+    pcb->ttl = UDP_TTL;
+  }
+  return pcb;
+}
+
+#if UDP_DEBUG
+void
+udp_debug_print(struct udp_hdr *udphdr)
+{
+  LWIP_DEBUGF(UDP_DEBUG, ("UDP header:\n"));
+  LWIP_DEBUGF(UDP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(UDP_DEBUG, ("|     %5"U16_F"     |     %5"U16_F"     | (src port, dest port)\n",
+                          ntohs(udphdr->src), ntohs(udphdr->dest)));
+  LWIP_DEBUGF(UDP_DEBUG, ("+-------------------------------+\n"));
+  LWIP_DEBUGF(UDP_DEBUG, ("|     %5"U16_F"     |     0x%04"X16_F"    | (len, chksum)\n",
+                          ntohs(udphdr->len), ntohs(udphdr->chksum)));
+  LWIP_DEBUGF(UDP_DEBUG, ("+-------------------------------+\n"));
+}
+#endif /* UDP_DEBUG */
+
+#endif /* LWIP_UDP */