Apply patch for task #15072: SNMP support for notifications and informs (v2c)

Original author: Ognjen Bjelica
with minor fixes from Dirk Ziegelmeier

contrib/examples/snmp/snmp_example.c

@@ -1,5 +1,5 @@
 /*
- * Redistribution and use in source and binary forms, with or without modification, 
+ * 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,
@@ -8,25 +8,26 @@
  *    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. 
+ *    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 
+ * 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: Dirk Ziegelmeier <dziegel@gmx.de>
  *
  */
 
+#include "lwip/netif.h"
 #include "lwip/apps/snmp.h"
 #include "lwip/apps/snmp_mib2.h"
 #include "lwip/apps/snmpv3.h"
@@ -36,7 +37,6 @@
 #include "examples/snmp/snmp_private_mib/private_mib.h"
 #include "snmp_example.h"
 
-
 #if LWIP_SNMP
 static const struct snmp_mib *mibs[] = {
   &mib2,
@@ -51,6 +51,7 @@ static const struct snmp_mib *mibs[] = {
 void
 snmp_example_init(void)
 {
+  s32_t req_nr;
 #if LWIP_SNMP
   lwip_privmib_init();
 #if SNMP_LWIP_MIB2
@@ -59,7 +60,7 @@ snmp_example_init(void)
 #endif /* SNMP_USE_NETCONN */
   snmp_mib2_set_syscontact_readonly((const u8_t*)"root", NULL);
   snmp_mib2_set_syslocation_readonly((const u8_t*)"lwIP development PC", NULL);
-  snmp_mib2_set_sysdescr((const u8_t*)"simhost", NULL);
+  snmp_mib2_set_sysdescr((const u8_t*)"lwIP example", NULL);
 #endif /* SNMP_LWIP_MIB2 */
 
 #if LWIP_SNMP_V3
@@ -68,5 +69,12 @@ snmp_example_init(void)
 
   snmp_set_mibs(mibs, LWIP_ARRAYSIZE(mibs));
   snmp_init();
+
+  snmp_trap_dst_ip_set(0, &netif_default->gw);
+  snmp_trap_dst_enable(0, 1);
+
+  snmp_send_inform_generic(SNMP_GENTRAP_COLDSTART, NULL, &req_nr);
+  snmp_send_trap_generic(SNMP_GENTRAP_COLDSTART);
+
 #endif /* LWIP_SNMP */
 }

src/apps/snmp/snmp_msg.c

@@ -155,6 +155,9 @@ snmp_v3_enable(u8_t enable)
 
 #endif
 
+snmp_inform_callback_fct snmp_inform_callback     = NULL;
+void*                    snmp_inform_callback_arg = NULL;
+
 /**
  * @ingroup snmp_core
  * Returns current SNMP community string.
@@ -249,6 +252,17 @@ snmp_set_write_callback(snmp_write_callback_fct write_callback, void *callback_a
   snmp_write_callback_arg = callback_arg;
 }
 
+/**
+ * @ingroup snmp_core
+ * Callback fired on every received INFORM confirmation (get-response)
+ */
+void
+snmp_set_inform_callback(snmp_inform_callback_fct inform_callback, void* callback_arg)
+{
+  snmp_inform_callback     = inform_callback;
+  snmp_inform_callback_arg = callback_arg;
+}
+
 /* ----------------------------------------------------------------------- */
 /* forward declarations */
 /* ----------------------------------------------------------------------- */
@@ -297,6 +311,11 @@ snmp_receive(void *handle, struct pbuf *p, const ip_addr_t *source_ip, u16_t por
           err = snmp_process_getbulk_request(&request);
         } else if (request.request_type == SNMP_ASN1_CONTEXT_PDU_SET_REQ) {
           err = snmp_process_set_request(&request);
+        } else if(request.request_type == SNMP_ASN1_CONTEXT_PDU_GET_RESP) {
+          /* If callback function has been defined call it. */
+          if (snmp_inform_callback != NULL) {
+            snmp_inform_callback(&request, snmp_inform_callback_arg);
+          }
         }
       }
 #if LWIP_SNMP_V3
@@ -360,7 +379,7 @@ snmp_receive(void *handle, struct pbuf *p, const ip_addr_t *source_ip, u16_t por
       }
 #endif
 
-      if (err == ERR_OK) {
+      if ((err == ERR_OK) && (request.request_type != SNMP_ASN1_CONTEXT_PDU_GET_RESP)) {
         err = snmp_complete_outbound_frame(&request);
 
         if (err == ERR_OK) {
@@ -1174,6 +1193,10 @@ snmp_parse_inbound_frame(struct snmp_request *request)
       /* SetRequest PDU */
       snmp_stats.insetrequests++;
       break;
+    case (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_GET_RESP):
+      /* GetResponse PDU */
+      snmp_stats.ingetresponses++;
+      break;
     default:
       /* unsupported input PDU for this agent (no parse error) */
       LWIP_DEBUGF(SNMP_DEBUG, ("Unknown/Invalid SNMP PDU type received: %d", tlv.type)); \

src/apps/snmp/snmp_traps.c

@@ -1,6 +1,6 @@
 /**
  * @file
- * SNMPv1 traps implementation.
+ * SNMPv1 and SNMPv2 traps implementation.
  */
 
 /*
@@ -51,7 +51,11 @@
 #include "snmp_asn1.h"
 #include "snmp_core_priv.h"
 
-struct snmp_msg_trap {
+#define SNMP_IS_INFORM                            1
+#define SNMP_IS_TRAP                              0
+
+struct snmp_msg_trap
+{
   /* source enterprise ID (sysObjectID) */
   const struct snmp_obj_id *enterprise;
   /* source IP address, raw network order format */
@@ -74,12 +78,26 @@ struct snmp_msg_trap {
   u16_t seqlen;
   /* encoding varbinds sequence length */
   u16_t vbseqlen;
+
+  /* error status */
+  s32_t error_status;
+  /* error index */
+  s32_t error_index;
+  /* trap or inform? */
+  u8_t trap_or_inform;
 };
 
 static u16_t snmp_trap_varbind_sum(struct snmp_msg_trap *trap, struct snmp_varbind *varbinds);
 static u16_t snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len);
 static err_t snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream);
 static err_t snmp_trap_varbind_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream, struct snmp_varbind *varbinds);
+static u16_t snmp_trap_header_sum_v1_specific(struct snmp_msg_trap *trap);
+static u16_t snmp_trap_header_sum_v2c_specific(struct snmp_msg_trap *trap);
+static err_t snmp_trap_header_enc_v1_specific(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream);
+static err_t snmp_trap_header_enc_v2c_specific(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream);
+static err_t snmp_prepare_trap_oid(struct snmp_obj_id *dest_snmp_trap_oid, const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap);
+static void snmp_prepare_necessary_msg_fields(struct snmp_msg_trap *trap_msg, const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds);
+static err_t snmp_send_msg(struct snmp_msg_trap *trap_msg, struct snmp_varbind *varbinds, u16_t tot_len, ip_addr_t *dip);
 
 #define BUILD_EXEC(code) \
   if ((code) != ERR_OK) { \
@@ -92,7 +110,12 @@ extern const char *snmp_community_trap;
 
 void *snmp_traps_handle;
 
-struct snmp_trap_dst {
+/**
+ * @ingroup snmp_traps
+ * @struct snmp_trap_dst
+ */
+struct snmp_trap_dst
+{
   /* destination IP address in network order */
   ip_addr_t dip;
   /* set to 0 when disabled, >0 when enabled */
@@ -102,11 +125,19 @@ static struct snmp_trap_dst trap_dst[SNMP_TRAP_DESTINATIONS];
 
 static u8_t snmp_auth_traps_enabled = 0;
 
+/* This is used in functions like snmp_coldstart_trap where user didn't specify which version of trap to use */
+static u8_t snmp_default_trap_version = SNMP_VERSION_1;
+
+/* This is used in trap messages v2c */
+static s32_t req_id = 1;
+
 /**
  * @ingroup snmp_traps
  * 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.
+ *
+ * @retval void
  */
 void
 snmp_trap_dst_enable(u8_t dst_idx, u8_t enable)
@@ -122,6 +153,8 @@ snmp_trap_dst_enable(u8_t dst_idx, u8_t 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.
+ *
+ * @retval void
  */
 void
 snmp_trap_dst_ip_set(u8_t dst_idx, const ip_addr_t *dst)
@@ -135,6 +168,10 @@ snmp_trap_dst_ip_set(u8_t dst_idx, const ip_addr_t *dst)
 /**
  * @ingroup snmp_traps
  * Enable/disable authentication traps
+ *
+ * @param enable enable SNMP traps
+ *
+ * @retval void
  */
 void
 snmp_set_auth_traps_enabled(u8_t enable)
@@ -145,6 +182,8 @@ snmp_set_auth_traps_enabled(u8_t enable)
 /**
  * @ingroup snmp_traps
  * Get authentication traps enabled state
+ *
+ * @return TRUE if traps are enabled, FALSE if they aren't
  */
 u8_t
 snmp_get_auth_traps_enabled(void)
@@ -152,11 +191,146 @@ snmp_get_auth_traps_enabled(void)
   return snmp_auth_traps_enabled;
 }
 
+/**
+ * @ingroup snmp_traps
+ * Choose default SNMP version for sending traps (if not specified, default version is SNMP_VERSION_1)
+ * SNMP_VERSION_1  0
+ * SNMP_VERSION_2c 1
+ * SNMP_VERSION_3  3
+ *
+ * @param snmp_version version that will be used for sending traps
+ *
+ * @retval void
+ */
+void
+snmp_set_default_trap_version(u8_t snmp_version)
+{
+  snmp_default_trap_version = snmp_version;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Get default SNMP version for sending traps
+ *
+ * @return selected default version:
+ * 0 - SNMP_VERSION_1
+ * 1 - SNMP_VERSION_2c
+ * 3 - SNMP_VERSION_3
+ */
+u8_t
+snmp_get_default_trap_version(void)
+{
+  return snmp_default_trap_version;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Prepares snmpTrapOID for SNMP v2c
+ * @param dest_snmp_trap_oid pointer to destination snmpTrapOID
+ * @param eoid enterprise oid (can be NULL)
+ * @param generic_trap SNMP v1 generic trap
+ * @param specific_trap SNMP v1 specific trap
+ * @return ERR_OK if completed successfully;
+ *         ERR_MEM if there wasn't enough memory allocated for destination;
+ *         ERR_VAL if value for generic trap was incorrect;
+ */
+static err_t
+snmp_prepare_trap_oid(struct snmp_obj_id *dest_snmp_trap_oid, const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap)
+{
+  err_t err = ERR_OK;
+  const u32_t snmpTrapOID[] = {1, 3, 6, 1, 6, 3, 1, 1, 5};     /* please see rfc3584 */
+
+  if (generic_trap == SNMP_GENTRAP_ENTERPRISE_SPECIFIC) {
+    if (eoid == NULL) {
+      MEMCPY(dest_snmp_trap_oid, snmp_get_device_enterprise_oid(), sizeof(*dest_snmp_trap_oid));
+    } else {
+      MEMCPY(dest_snmp_trap_oid, eoid, sizeof(*dest_snmp_trap_oid));
+    }
+    if (dest_snmp_trap_oid->len + 2 < SNMP_MAX_OBJ_ID_LEN) {
+      dest_snmp_trap_oid->id[dest_snmp_trap_oid->len++] = 0;
+      dest_snmp_trap_oid->id[dest_snmp_trap_oid->len++] = specific_trap;
+    } else {
+      err = ERR_MEM;
+    }
+  } else if ((generic_trap >= SNMP_GENTRAP_COLDSTART) && (generic_trap < SNMP_GENTRAP_ENTERPRISE_SPECIFIC)) {
+    if (sizeof(dest_snmp_trap_oid->id) >= sizeof(snmpTrapOID)) {
+      MEMCPY(&dest_snmp_trap_oid->id, snmpTrapOID , sizeof(snmpTrapOID));
+      dest_snmp_trap_oid->len = LWIP_ARRAYSIZE(snmpTrapOID);
+      dest_snmp_trap_oid->id[dest_snmp_trap_oid->len++] = specific_trap + 1;
+    } else {
+      err = ERR_MEM;
+    }
+  } else {
+    err = ERR_VAL;
+  }
+  return err;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Prepare the rest of the necessary fields for trap/notification/inform message.
+ * @param trap_msg message that should be set
+ * @param eoid enterprise oid (can be NULL)
+ * @param generic_trap SNMP v1 generic trap
+ * @param specific_trap SNMP v1 specific trap
+ * @param varbinds list of varbinds
+ * @retval void
+ */
+static void
+snmp_prepare_necessary_msg_fields(struct snmp_msg_trap *trap_msg, const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds)
+{
+  if (trap_msg->snmp_version == SNMP_VERSION_1) {
+    trap_msg->enterprise = (eoid == NULL) ? snmp_get_device_enterprise_oid() : eoid;
+    trap_msg->gen_trap = generic_trap;
+    trap_msg->spc_trap = (generic_trap == SNMP_GENTRAP_ENTERPRISE_SPECIFIC) ? specific_trap : 0;
+    MIB2_COPY_SYSUPTIME_TO(&trap_msg->ts);
+  } else if (trap_msg->snmp_version == SNMP_VERSION_2c) {
+    /* Copy sysUpTime into the first varbind */
+    MIB2_COPY_SYSUPTIME_TO((u32_t *)varbinds[0].value);
+  }
+}
+
+/**
+ * @ingroup snmp_traps
+ * Copy trap message structure to pbuf and sends it
+ * @param trap_msg contains the data that should be sent
+ * @param varbinds list of varbinds
+ * @param tot_len total length of encoded data
+ * @param dip destination IP address
+ * @return ERR_OK if sending was successful
+ */
+static err_t
+snmp_send_msg(struct snmp_msg_trap *trap_msg, struct snmp_varbind *varbinds, u16_t tot_len, ip_addr_t *dip)
+{
+  err_t err = ERR_OK;
+  struct pbuf *p = NULL;
+  /* allocate pbuf(s) */
+  p = pbuf_alloc(PBUF_TRANSPORT, tot_len, PBUF_RAM);
+  if (p != NULL) {
+    struct snmp_pbuf_stream pbuf_stream;
+    snmp_pbuf_stream_init(&pbuf_stream, p, 0, tot_len);
+
+    /* pass 1, encode packet ino the pbuf(s) */
+    BUILD_EXEC( snmp_trap_header_enc(trap_msg, &pbuf_stream) );
+    BUILD_EXEC( snmp_trap_varbind_enc(trap_msg, &pbuf_stream, varbinds) );
+
+    snmp_stats.outtraps++;
+    snmp_stats.outpkts++;
+
+    /** send to the TRAP destination */
+    err = snmp_sendto(snmp_traps_handle, p, dip, LWIP_IANA_PORT_SNMP_TRAP);
+    pbuf_free(p);
+  } else {
+    err = ERR_MEM;
+  }
+  return err;
+}
 
 /**
  * @ingroup snmp_traps
- * Sends a generic or enterprise specific trap message.
+ * Prepare and sends a generic or enterprise specific trap message, notification or inform.
  *
+ * @param trap_msg defines msg type
  * @param eoid points to enterprise object identifier
  * @param generic_trap is the trap code
  * @param specific_trap used for enterprise traps when generic_trap == 6
@@ -169,94 +343,155 @@ snmp_get_auth_traps_enabled(void)
  * and .iso.org.dod.internet.private.enterprises.yourenterprise
  * (sysObjectID) for specific traps.
  */
-err_t
-snmp_send_trap(const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds)
+static err_t
+snmp_send_trap_or_notification_or_inform_generic(struct snmp_msg_trap *trap_msg, const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds)
 {
-  struct snmp_msg_trap trap_msg;
-  struct snmp_trap_dst *td;
-  struct pbuf *p;
-  u16_t i, tot_len;
+  struct snmp_trap_dst *td = NULL;
+  u16_t i = 0;
+  u16_t tot_len = 0;
   err_t err = ERR_OK;
+  u32_t timestamp = 0;
+  struct snmp_varbind *original_varbinds = varbinds;
+  struct snmp_varbind *original_prev = NULL;
+  struct snmp_varbind snmp_v2_special_varbinds[] = {
+                                                     /* First varbind is used to store sysUpTime */
+                                                     {
+                                                       &snmp_v2_special_varbinds[1],    /* *next */
+                                                       NULL,                            /* *prev */
+                                                       {                                /* oid */
+                                                         8,                             /* oid len */
+                                                         {1, 3, 6, 1, 2, 1, 1, 3}       /* oid for sysUpTime */
+                                                       },
+                                                       SNMP_ASN1_TYPE_TIMETICKS,        /* type */
+                                                       sizeof(u32_t),                   /* value_len */
+                                                       &timestamp                       /* value */
+                                                     },
+                                                     /* Second varbind is used to store snmpTrapOID */
+                                                     {
+                                                       varbinds,                        /* *next */
+                                                       &snmp_v2_special_varbinds[0],    /* *prev */
+                                                       {                                /* oid */
+                                                         10,                            /* oid len */
+                                                         {1, 3, 6, 1, 6, 3, 1, 1, 4, 1} /* oid for snmpTrapOID */
+                                                       },
+                                                       SNMP_ASN1_TYPE_OBJECT_ID,        /* type */
+                                                       0,                               /* value_len */
+                                                       NULL                             /* value */
+                                                     }
+   };
 
   LWIP_ASSERT_CORE_LOCKED();
 
-  trap_msg.snmp_version = 0;
+  /* see rfc3584 */
+  if (trap_msg->snmp_version == SNMP_VERSION_2c) {
+    struct snmp_obj_id snmp_trap_oid =  { 0 };  /* used for converting SNMPv1 generic/specific trap parameter to SNMPv2 snmpTrapOID */
+    err = snmp_prepare_trap_oid(&snmp_trap_oid, eoid, generic_trap, specific_trap);
+    if (err == ERR_OK) {
+      snmp_v2_special_varbinds[1].value_len = snmp_trap_oid.len * sizeof(snmp_trap_oid.id[0]);
+      snmp_v2_special_varbinds[1].value = snmp_trap_oid.id;
+      if (varbinds != NULL) {
+        original_prev = varbinds->prev;
+        varbinds->prev = &snmp_v2_special_varbinds[1];
+      }
+      varbinds = snmp_v2_special_varbinds;  /* After inserting two varbinds at the beginning of the list, make sure that pointer is pointing to the first element  */
+    }
+  }
 
-  for (i = 0, td = &trap_dst[0]; i < SNMP_TRAP_DESTINATIONS; i++, td++) {
+  for (i = 0, td = &trap_dst[0]; (i < SNMP_TRAP_DESTINATIONS) && (err == ERR_OK); i++, td++) {
     if ((td->enable != 0) && !ip_addr_isany(&td->dip)) {
       /* lookup current source address for this dst */
-      if (snmp_get_local_ip_for_dst(snmp_traps_handle, &td->dip, &trap_msg.sip)) {
-        if (eoid == NULL) {
-          trap_msg.enterprise = snmp_get_device_enterprise_oid();
-        } else {
-          trap_msg.enterprise = eoid;
-        }
-
-        trap_msg.gen_trap = generic_trap;
-        if (generic_trap == SNMP_GENTRAP_ENTERPRISE_SPECIFIC) {
-          trap_msg.spc_trap = specific_trap;
-        } else {
-          trap_msg.spc_trap = 0;
-        }
-
-        MIB2_COPY_SYSUPTIME_TO(&trap_msg.ts);
+      if (snmp_get_local_ip_for_dst(snmp_traps_handle, &td->dip, &trap_msg->sip)) {
+        snmp_prepare_necessary_msg_fields(trap_msg, eoid, generic_trap, specific_trap, varbinds);
 
         /* pass 0, calculate length fields */
-        tot_len = snmp_trap_varbind_sum(&trap_msg, varbinds);
-        tot_len = snmp_trap_header_sum(&trap_msg, tot_len);
-
-        /* allocate pbuf(s) */
-        p = pbuf_alloc(PBUF_TRANSPORT, tot_len, PBUF_RAM);
-        if (p != NULL) {
-          struct snmp_pbuf_stream pbuf_stream;
-          snmp_pbuf_stream_init(&pbuf_stream, p, 0, tot_len);
-
-          /* pass 1, encode packet into the pbuf(s) */
-          snmp_trap_header_enc(&trap_msg, &pbuf_stream);
-          snmp_trap_varbind_enc(&trap_msg, &pbuf_stream, varbinds);
-
-          snmp_stats.outtraps++;
-          snmp_stats.outpkts++;
-
-          /** send to the TRAP destination */
-          snmp_sendto(snmp_traps_handle, p, &td->dip, LWIP_IANA_PORT_SNMP_TRAP);
-          pbuf_free(p);
-        } else {
-          err = ERR_MEM;
-        }
+        tot_len = snmp_trap_varbind_sum(trap_msg, varbinds);
+        tot_len = snmp_trap_header_sum(trap_msg, tot_len);
+
+        /* allocate pbuf, fill it and send it */
+        err = snmp_send_msg(trap_msg, varbinds, tot_len, &td->dip);
       } else {
         /* routing error */
         err = ERR_RTE;
       }
     }
   }
+  if ((trap_msg->snmp_version == SNMP_VERSION_2c) && (original_varbinds != NULL)) {
+    original_varbinds->prev = original_prev;
+  }
+  req_id++;
   return err;
 }
 
+/**
+ * @ingroup snmp_traps
+ * This function is a wrapper function for preparing and sending generic or specific traps.
+ *
+ * @param eoid points to enterprise object identifier
+ * @param generic_trap is the trap code
+ * @param specific_trap used for enterprise traps when generic_trap == 6
+ * @param varbinds linked list of varbinds to be sent
+ * @return ERR_OK when success, ERR_MEM if we're out of memory
+ *
+ * @note the use of the enterprise 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(const struct snmp_obj_id* oid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds)
+{
+  struct snmp_msg_trap trap_msg = {0};
+  trap_msg.snmp_version = snmp_default_trap_version;
+  trap_msg.trap_or_inform = SNMP_IS_TRAP;
+  return snmp_send_trap_or_notification_or_inform_generic(&trap_msg, oid, generic_trap, specific_trap, varbinds);
+}
+
 /**
  * @ingroup snmp_traps
  * Send generic SNMP trap
+ * @param generic_trap is the trap code
+ * return ERR_OK when success
  */
 err_t
 snmp_send_trap_generic(s32_t generic_trap)
 {
-  static const struct snmp_obj_id oid = { 7, { 1, 3, 6, 1, 2, 1, 11 } };
-  return snmp_send_trap(&oid, generic_trap, 0, NULL);
+  err_t err = ERR_OK;
+  struct snmp_msg_trap trap_msg = {0};
+  trap_msg.snmp_version = snmp_default_trap_version;
+  trap_msg.trap_or_inform = SNMP_IS_TRAP;
+
+  if(snmp_default_trap_version == SNMP_VERSION_1) {
+    static const struct snmp_obj_id oid = { 7, { 1, 3, 6, 1, 2, 1, 11 } };
+    err = snmp_send_trap_or_notification_or_inform_generic(&trap_msg, &oid, generic_trap, 0, NULL);
+  } else if (snmp_default_trap_version == SNMP_VERSION_2c) {
+    err = snmp_send_trap_or_notification_or_inform_generic(&trap_msg, NULL, generic_trap, 0, NULL);
+  } else {
+    err = ERR_VAL;
+  }
+  return err;
 }
 
 /**
  * @ingroup snmp_traps
  * Send specific SNMP trap with variable bindings
+ * @param specific_trap used for enterprise traps (generic_trap = 6)
+ * @param varbinds linked list of varbinds to be sent
+ * @return ERR_OK when success
  */
 err_t
 snmp_send_trap_specific(s32_t specific_trap, struct snmp_varbind *varbinds)
 {
-  return snmp_send_trap(NULL, SNMP_GENTRAP_ENTERPRISE_SPECIFIC, specific_trap, varbinds);
+  struct snmp_msg_trap trap_msg = {0};
+  trap_msg.snmp_version = snmp_default_trap_version;
+  trap_msg.trap_or_inform = SNMP_IS_TRAP;
+  return snmp_send_trap_or_notification_or_inform_generic(&trap_msg, NULL, SNMP_GENTRAP_ENTERPRISE_SPECIFIC, specific_trap, varbinds);
 }
 
 /**
  * @ingroup snmp_traps
  * Send coldstart trap
+ * @retval void
  */
 void
 snmp_coldstart_trap(void)
@@ -267,6 +502,7 @@ snmp_coldstart_trap(void)
 /**
  * @ingroup snmp_traps
  * Send authentication failure trap (used internally by agent)
+ * @retval void
  */
 void
 snmp_authfail_trap(void)
@@ -276,6 +512,14 @@ snmp_authfail_trap(void)
   }
 }
 
+/**
+ * @ingroup snmp_traps
+ * Sums trap varbinds
+ *
+ * @param trap Trap message
+ * @param varbinds linked list of varbinds
+ * @return the required length for encoding of this part of the trap header
+ */
 static u16_t
 snmp_trap_varbind_sum(struct snmp_msg_trap *trap, struct snmp_varbind *varbinds)
 {
@@ -303,21 +547,18 @@ snmp_trap_varbind_sum(struct snmp_msg_trap *trap, struct snmp_varbind *varbinds)
 }
 
 /**
- * Sums trap header field lengths from tail to head and
- * returns trap_header_lengths for second encoding pass.
+ * @ingroup snmp_traps
+ * Sums trap header fields that are specific for SNMP v1
  *
  * @param trap Trap message
- * @param vb_len varbind-list length
- * @return the required length for encoding the trap header
+ * @return the required length for encoding of this part of the trap header
  */
 static u16_t
-snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len)
+snmp_trap_header_sum_v1_specific(struct snmp_msg_trap *trap)
 {
-  u16_t tot_len;
-  u16_t len;
-  u8_t lenlen;
-
-  tot_len = vb_len;
+  u16_t tot_len = 0;
+  u16_t len = 0;
+  u8_t lenlen = 0;
 
   snmp_asn1_enc_u32t_cnt(trap->ts, &len);
   snmp_asn1_enc_length_cnt(len, &lenlen);
@@ -347,6 +588,57 @@ snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len)
   snmp_asn1_enc_length_cnt(len, &lenlen);
   tot_len += 1 + len + lenlen;
 
+  return tot_len;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Sums trap header fields that are specific for SNMP v2c
+ *
+ * @param trap Trap message
+ * @return the required length for encoding of this part of the trap header
+ */
+static u16_t
+snmp_trap_header_sum_v2c_specific(struct snmp_msg_trap *trap)
+{
+  u16_t tot_len = 0;
+  u16_t len = 0;
+  u8_t lenlen = 0;
+
+  snmp_asn1_enc_u32t_cnt(req_id, &len);
+  snmp_asn1_enc_length_cnt(len, &lenlen);
+  tot_len += 1 + len + lenlen;
+  snmp_asn1_enc_u32t_cnt(trap->error_status, &len);
+  snmp_asn1_enc_length_cnt(len, &lenlen);
+  tot_len += 1 + len + lenlen;
+  snmp_asn1_enc_u32t_cnt(trap->error_index, &len);
+  snmp_asn1_enc_length_cnt(len, &lenlen);
+  tot_len += 1 + len + lenlen;
+
+  return tot_len;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Sums trap header field lengths from tail to head and
+ * returns trap_header_lengths for second encoding pass.
+ *
+ * @param trap Trap message
+ * @param vb_len varbind-list length
+ * @return the required length for encoding the trap header
+ */
+static u16_t
+snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len)
+{
+  u16_t tot_len = vb_len;
+  u16_t len = 0;
+  u8_t lenlen = 0;
+
+  if (trap->snmp_version == SNMP_VERSION_1) {
+    tot_len += snmp_trap_header_sum_v1_specific(trap);
+  } else if (trap->snmp_version == SNMP_VERSION_2c) {
+    tot_len += snmp_trap_header_sum_v2c_specific(trap);
+  }
   trap->pdulen = tot_len;
   snmp_asn1_enc_length_cnt(trap->pdulen, &lenlen);
   tot_len += 1 + lenlen;
@@ -366,6 +658,14 @@ snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len)
   return tot_len;
 }
 
+/**
+ * @ingroup snmp_traps
+ * Encodes varbinds.
+ * @param trap Trap message
+ * @param pbuf_stream stream used for storing data inside pbuf
+ * @param varbinds linked list of varbinds
+ * @retval err_t ERR_OK if successful, ERR_ARG otherwise
+ */
 static err_t
 snmp_trap_varbind_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream, struct snmp_varbind *varbinds)
 {
@@ -387,32 +687,45 @@ snmp_trap_varbind_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_
 }
 
 /**
- * Encodes trap header from head to tail.
+ * @ingroup snmp_traps
+ * Encodes trap header PDU part.
+ * @param trap Trap message
+ * @param pbuf_stream stream used for storing data inside pbuf
+ * @retval err_t ERR_OK if successful, ERR_ARG otherwise
  */
 static err_t
-snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream)
+snmp_trap_header_enc_pdu(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream)
 {
   struct snmp_asn1_tlv tlv;
-
-  /* 'Message' sequence */
-  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 0, trap->seqlen);
-  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
-
-  /* version */
-  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
-  snmp_asn1_enc_s32t_cnt(trap->snmp_version, &tlv.value_len);
-  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
-  BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->snmp_version) );
-
-  /* community */
-  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, trap->comlen);
-  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
-  BUILD_EXEC( snmp_asn1_enc_raw(pbuf_stream,  (const u8_t *)snmp_community_trap, trap->comlen) );
-
   /* 'PDU' sequence */
-  SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_TRAP), 0, trap->pdulen);
-  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  if (trap->snmp_version == SNMP_VERSION_1) {
+    /* TRAP V1 */
+    SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_TRAP), 0, trap->pdulen);
+    BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  } else if ((trap->snmp_version == SNMP_VERSION_2c) && (trap->trap_or_inform == SNMP_IS_INFORM)) {
+    /* TRAP v2 - INFORM */
+    SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_INFORM_REQ), 0, trap->pdulen);
+    BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  } else if (trap->snmp_version == SNMP_VERSION_2c) {
+    /* TRAP v2 - NOTIFICATION*/
+    SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_V2_TRAP), 0, trap->pdulen);
+    BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  }
 
+  return ERR_OK;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Encodes trap header part that is SNMP v1 header specific.
+ * @param trap Trap message
+ * @param pbuf_stream stream used for storing data inside pbuf
+ * @retval void
+ */
+static err_t
+snmp_trap_header_enc_v1_specific(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream)
+{
+  struct snmp_asn1_tlv tlv;
   /* object ID */
   SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OBJECT_ID, 0, 0);
   snmp_asn1_enc_oid_cnt(trap->enterprise->id, trap->enterprise->len, &tlv.value_len);
@@ -434,7 +747,7 @@ snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_s
 #endif
   }
 
-  /* trap length */
+  /* generic trap */
   SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
   snmp_asn1_enc_s32t_cnt(trap->gen_trap, &tlv.value_len);
   BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
@@ -455,4 +768,132 @@ snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_s
   return ERR_OK;
 }
 
+/**
+ * @ingroup snmp_traps
+ * Encodes trap header part that is SNMP v2c header specific.
+ *
+ * @param trap Trap message
+ * @param pbuf_stream stream used for storing data inside pbuf
+ * @retval void
+ */
+static err_t
+snmp_trap_header_enc_v2c_specific(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream)
+{
+  struct snmp_asn1_tlv tlv;
+  /* request id */
+  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
+  snmp_asn1_enc_s32t_cnt(req_id, &tlv.value_len);
+  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, req_id) );
+
+  /* error status */
+  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
+  snmp_asn1_enc_s32t_cnt(trap->error_status, &tlv.value_len);
+  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->error_status) );
+
+  /* error index */
+  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
+  snmp_asn1_enc_s32t_cnt(trap->error_index, &tlv.value_len);
+  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->error_index) );
+
+  return ERR_OK;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Encodes trap header from head to tail.
+ *
+ * @param trap Trap message
+ * @param pbuf_stream stream used for storing data inside pbuf
+ * @retval void
+ */
+static err_t
+snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream)
+{
+  struct snmp_asn1_tlv tlv;
+
+  /* 'Message' sequence */
+  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 0, trap->seqlen);
+  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+
+  /* version */
+  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
+  snmp_asn1_enc_s32t_cnt(trap->snmp_version, &tlv.value_len);
+  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->snmp_version) );
+
+  /* community */
+  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, trap->comlen);
+  BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
+  BUILD_EXEC( snmp_asn1_enc_raw(pbuf_stream,  (const u8_t *)snmp_community_trap, trap->comlen) );
+
+  /* PDU */
+  BUILD_EXEC( snmp_trap_header_enc_pdu(trap, pbuf_stream) );
+  if (trap->snmp_version == SNMP_VERSION_1) {
+    /* object ID, IP addr, generic trap, specific trap, timestamp */
+    BUILD_EXEC( snmp_trap_header_enc_v1_specific(trap, pbuf_stream) );
+  } else if (SNMP_VERSION_2c == trap->snmp_version) {
+    /* request id, error status, error index */
+    BUILD_EXEC( snmp_trap_header_enc_v2c_specific(trap, pbuf_stream) );
+  }
+
+  return ERR_OK;
+}
+
+/**
+ * @ingroup snmp_traps
+ * Wrapper function for sending informs
+ * @param specific_trap will be appended to enterprise oid [see RFC 3584]
+ * @param varbinds linked list of varbinds (at the beginning of this list function will insert 2 special purpose varbinds [see RFC 3584])
+ * @param ptr_request_id[out] variable in which to store request_id needed to verify acknowledgement
+ * @return ERR_OK if successful
+ */
+err_t
+snmp_send_inform_specific(s32_t specific_trap, struct snmp_varbind *varbinds, s32_t *ptr_request_id)
+{
+  struct snmp_msg_trap trap_msg = {0};
+  trap_msg.snmp_version = SNMP_VERSION_2c;
+  trap_msg.trap_or_inform = SNMP_IS_INFORM;
+  *ptr_request_id = req_id;
+  return snmp_send_trap_or_notification_or_inform_generic(&trap_msg, NULL, SNMP_GENTRAP_ENTERPRISE_SPECIFIC, specific_trap, varbinds);
+}
+
+/**
+ * @ingroup snmp_traps
+ * Wrapper function for sending informs
+ * @param generic_trap is the trap code
+ * @param varbinds linked list of varbinds (at the beginning of this list function will insert 2 special purpose varbinds [see RFC 3584])
+ * @param ptr_request_id[out] variable in which to store request_id needed to verify acknowledgement
+ * @return ERR_OK if successful
+ */
+err_t
+snmp_send_inform_generic(s32_t generic_trap, struct snmp_varbind *varbinds, s32_t *ptr_request_id)
+{
+  struct snmp_msg_trap trap_msg = {0};
+  trap_msg.snmp_version = SNMP_VERSION_2c;
+  trap_msg.trap_or_inform = SNMP_IS_INFORM;
+  *ptr_request_id = req_id;
+  return snmp_send_trap_or_notification_or_inform_generic(&trap_msg, NULL, generic_trap, 0, varbinds);
+}
+
+/**
+ * @ingroup snmp_traps
+ * Wrapper function for sending informs
+ * @param specific_trap will be appended to enterprise oid [see RFC 3584]
+ * @param varbinds linked list of varbinds (at the beginning of this list function will insert 2 special purpose varbinds [see RFC 3584])
+ * @param ptr_request_id[out] variable in which to store request_id needed to verify acknowledgement
+ * @return ERR_OK if successful
+ */
+err_t
+snmp_send_inform(const struct snmp_obj_id* oid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds, s32_t *ptr_request_id)
+{
+  struct snmp_msg_trap trap_msg = {0};
+  trap_msg.snmp_version = SNMP_VERSION_2c;
+  trap_msg.trap_or_inform = SNMP_IS_INFORM;
+  *ptr_request_id = req_id;
+  return snmp_send_trap_or_notification_or_inform_generic(&trap_msg, oid, generic_trap, specific_trap, varbinds);
+}
+
 #endif /* LWIP_SNMP */

src/include/lwip/apps/snmp.h

@@ -101,6 +101,16 @@ err_t snmp_send_trap_generic(s32_t generic_trap);
 err_t snmp_send_trap_specific(s32_t specific_trap, struct snmp_varbind *varbinds);
 err_t snmp_send_trap(const struct snmp_obj_id* oid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds);
 
+err_t snmp_send_inform_generic(s32_t generic_trap, struct snmp_varbind *varbinds, s32_t *ptr_request_id);
+err_t snmp_send_inform_specific(s32_t specific_trap, struct snmp_varbind *varbinds, s32_t *ptr_request_id);
+err_t snmp_send_inform(const struct snmp_obj_id* oid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds, s32_t *ptr_request_id);
+struct snmp_request;
+typedef void (*snmp_inform_callback_fct)(struct snmp_request *request, void* callback_arg);
+void snmp_set_inform_callback(snmp_inform_callback_fct inform_callback, void* callback_arg);
+
+void snmp_set_default_trap_version(u8_t snmp_version);
+u8_t snmp_get_default_trap_version(void);
+
 #define SNMP_AUTH_TRAPS_DISABLED 0
 #define SNMP_AUTH_TRAPS_ENABLED  1
 void snmp_set_auth_traps_enabled(u8_t enable);