/*
 * snmp_trap_pdu2.c
 *
 *  Created on: 30.10.2017
 *      Author: balbekova
 */

#include "lwip/apps/snmp_opts.h"

#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */

#include "snmp_msg.h"
#include "snmp_asn1.h"
#include "snmp_core_priv.h"
#include "lwip/ip_addr.h"
#include "lwip/stats.h"

#if LWIP_SNMP_V3
#include "lwip/apps/snmpv3.h"
#include "snmpv3_priv.h"
#ifdef LWIP_SNMPV3_INCLUDE_ENGINE
#include LWIP_SNMPV3_INCLUDE_ENGINE
#endif
#endif

#include <string.h>

#define BUILD_EXEC_TRAP(code, retValue) \
	if ((code) != ERR_OK) { \
		LWIP_DEBUGF(SNMP_DEBUG, ("SNMP error during creation of outbound frame!: " # code)); \
		return retValue; \
	}

#define OF_BUILD_EXEC_TRAP(code) BUILD_EXEC_TRAP(code, ERR_ARG)


struct snmp_trap_pduv2_dst
{
  /* destination IP address in network order */
  ip_addr_t dip;
  /* set to 0 when disabled, >0 when enabled */
  u8_t enable;
};
static struct snmp_trap_pduv2_dst trap_pduv2_dst[SNMP_TRAP_DESTINATIONS];


void* snmp_traps_pdu2_handle;

/**
 * @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.
 */
void
snmp_trap_pduv2_dst_enable(u8_t dst_idx, u8_t enable)
{
  if (dst_idx < SNMP_TRAP_DESTINATIONS) {
    trap_pduv2_dst[dst_idx].enable = enable;
  }
}

static err_t
snmp_complete_outbound_frame_trap_pduv2(struct snmp_request *request)
{
  struct snmp_asn1_tlv tlv;
  u16_t frame_size;
  u8_t outbound_padding = 0;

  if (request->version == SNMP_VERSION_1) {
    if (request->error_status != SNMP_ERR_NOERROR) {
      /* map v2c error codes to v1 compliant error code (according to RFC 2089) */
      switch (request->error_status) {
        /* mapping of implementation specific "virtual" error codes
         * (during processing of frame we already stored them in error_status field,
         * so no need to check all varbinds here for those exceptions as suggested by RFC) */
        case SNMP_ERR_NOSUCHINSTANCE:
        case SNMP_ERR_NOSUCHOBJECT:
        case SNMP_ERR_ENDOFMIBVIEW:
          request->error_status = SNMP_ERR_NOSUCHNAME;
          break;
        /* mapping according to RFC */
        case SNMP_ERR_WRONGVALUE:
        case SNMP_ERR_WRONGENCODING:
        case SNMP_ERR_WRONGTYPE:
        case SNMP_ERR_WRONGLENGTH:
        case SNMP_ERR_INCONSISTENTVALUE:
          request->error_status = SNMP_ERR_BADVALUE;
          break;
        case SNMP_ERR_NOACCESS:
        case SNMP_ERR_NOTWRITABLE:
        case SNMP_ERR_NOCREATION:
        case SNMP_ERR_INCONSISTENTNAME:
        case SNMP_ERR_AUTHORIZATIONERROR:
          request->error_status = SNMP_ERR_NOSUCHNAME;
          break;
        case SNMP_ERR_RESOURCEUNAVAILABLE:
        case SNMP_ERR_COMMITFAILED:
        case SNMP_ERR_UNDOFAILED:
        default:
          request->error_status = SNMP_ERR_GENERROR;
          break;
       }
    }
  } else {
    if (request->request_type == SNMP_ASN1_CONTEXT_PDU_SET_REQ) {
      /* map error codes to according to RFC 1905 (4.2.5.  The SetRequest-PDU) return 'NotWritable' for unknown OIDs) */
      switch (request->error_status) {
        case SNMP_ERR_NOSUCHINSTANCE:
        case SNMP_ERR_NOSUCHOBJECT:
        case SNMP_ERR_ENDOFMIBVIEW:
          request->error_status = SNMP_ERR_NOTWRITABLE;
          break;
        default:
          break;
      }
    }

    if (request->error_status >= SNMP_VARBIND_EXCEPTION_OFFSET) {
      /* should never occur because v2 frames store exceptions directly inside varbinds and not as frame error_status */
      LWIP_DEBUGF(SNMP_DEBUG, ("snmp_complete_outbound_frame() > Found v2 request with varbind exception code stored as error status!\n"));
      return ERR_ARG;
    }
  }

  if ((request->error_status != SNMP_ERR_NOERROR) || (request->request_type == SNMP_ASN1_CONTEXT_PDU_SET_REQ)) {
    /* all inbound vars are returned in response without any modification for error responses and successful set requests*/
    struct snmp_pbuf_stream inbound_stream;
    OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_init(&inbound_stream, request->inbound_pbuf, request->inbound_varbind_offset, request->inbound_varbind_len) );
    OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_init(&(request->outbound_pbuf_stream), request->outbound_pbuf, request->outbound_varbind_offset, request->outbound_pbuf->tot_len - request->outbound_varbind_offset) );
    snmp_pbuf_stream_writeto(&inbound_stream, &(request->outbound_pbuf_stream), 0);
  }

  frame_size = request->outbound_pbuf_stream.offset;

#if LWIP_SNMP_V3 && LWIP_SNMP_V3_CRYPTO
  /* Calculate padding for encryption */
  if (request->version == SNMP_VERSION_3 && (request->msg_flags & SNMP_V3_PRIV_FLAG)) {
    u8_t i;
    outbound_padding = (8 - (u8_t)((frame_size - request->outbound_scoped_pdu_seq_offset) & 0x07)) & 0x07;
    for (i = 0; i < outbound_padding; i++) {
      snmp_pbuf_stream_write(&request->outbound_pbuf_stream, 0);
    }
  }
#endif

  /* complete missing length in 'Message' sequence ; 'Message' tlv is located at the beginning (offset 0) */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 2, frame_size + outbound_padding - 1 - 2); /* - type - length_len(fixed, see snmp_prepare_outbound_frame()) */
  OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_init(&(request->outbound_pbuf_stream), request->outbound_pbuf, 0, request->outbound_pbuf->tot_len) );
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv) );

#if LWIP_SNMP_V3
  if (request->version == SNMP_VERSION_3) {
    /* complete missing length in 'globalData' sequence */
    /* - type - length_len(fixed, see snmp_prepare_outbound_frame()) */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 1, request->outbound_msg_global_data_end
        - request->outbound_msg_global_data_offset - 1 - 1);
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_msg_global_data_offset));
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv));

    /* complete missing length in 'msgSecurityParameters' sequence */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 1, request->outbound_msg_security_parameters_end
        - request->outbound_msg_security_parameters_str_offset - 1 - 1);
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_msg_security_parameters_str_offset));
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv));

    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 1, request->outbound_msg_security_parameters_end
        - request->outbound_msg_security_parameters_seq_offset - 1 - 1);
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_msg_security_parameters_seq_offset));
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv));

    /* complete missing length in scoped PDU sequence */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 3, frame_size - request->outbound_scoped_pdu_seq_offset - 1 - 3);
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_scoped_pdu_seq_offset));
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv));
  }
#endif

  /* complete missing length in 'PDU' sequence */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_GET_NEXT_REQ | SNMP_ASN1_CONTEXT_PDU_GET_RESP | SNMP_ASN1_CONTEXT_PDU_TRAP), 2,
      frame_size - request->outbound_pdu_offset - 1 - 2); /* - type - length_len(fixed, see snmp_prepare_outbound_frame()) */
  OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_pdu_offset) );
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv) );

  /* process and encode final error status */
  if (request->error_status != 0) {
    u16_t len;
    snmp_asn1_enc_s32t_cnt(request->error_status, &len);
    if (len != 1) {
      /* error, we only reserved one byte for it */
      return ERR_ARG;
    }
    OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_error_status_offset) );
    OF_BUILD_EXEC_TRAP( snmp_asn1_enc_s32t(&(request->outbound_pbuf_stream), len, request->error_status) );

    /* for compatibility to v1, log statistics; in v2 (RFC 1907) these statistics are obsoleted */
    switch (request->error_status) {
      case SNMP_ERR_TOOBIG:
        snmp_stats.outtoobigs++;
        break;
      case SNMP_ERR_NOSUCHNAME:
        snmp_stats.outnosuchnames++;
        break;
      case SNMP_ERR_BADVALUE:
        snmp_stats.outbadvalues++;
        break;
      case SNMP_ERR_GENERROR:
      default:
        snmp_stats.outgenerrs++;
        break;
    }

    if (request->error_status == SNMP_ERR_TOOBIG) {
      request->error_index = 0; /* defined by RFC 1157 */
    } else if (request->error_index == 0) {
      /* set index to varbind where error occured (if not already set before, e.g. during GetBulk processing) */
      request->error_index = request->inbound_varbind_enumerator.varbind_count;
    }
  } else {
    if (request->request_type == SNMP_ASN1_CONTEXT_PDU_SET_REQ) {
      snmp_stats.intotalsetvars += request->inbound_varbind_enumerator.varbind_count;
    } else {
      snmp_stats.intotalreqvars += request->inbound_varbind_enumerator.varbind_count;
    }
  }

  /* encode final error index*/
  if (request->error_index != 0) {
    u16_t len;
    snmp_asn1_enc_s32t_cnt(request->error_index, &len);
    if (len != 1) {
      /* error, we only reserved one byte for it */
      return ERR_VAL;
    }
    OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_error_index_offset) );
    OF_BUILD_EXEC_TRAP( snmp_asn1_enc_s32t(&(request->outbound_pbuf_stream), len, request->error_index) );
  }

  /* complete missing length in 'VarBindList' sequence ; 'VarBindList' tlv is located directly before varbind offset */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 1, frame_size - request->outbound_varbind_offset);
  OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_varbind_offset - 1 - 1) ); /* - type - length_len(fixed, see snmp_prepare_outbound_frame()) */
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv) );

  /* Authenticate response */
#if LWIP_SNMP_V3 && LWIP_SNMP_V3_CRYPTO
  /* Encrypt response */
  if (request->version == SNMP_VERSION_3 && (request->msg_flags & SNMP_V3_PRIV_FLAG)) {
    u8_t key[20];
    u8_t algo;

    /* complete missing length in PDU sequence */
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_init(&request->outbound_pbuf_stream, request->outbound_pbuf, 0, request->outbound_pbuf->tot_len));
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_seek_abs(&(request->outbound_pbuf_stream), request->outbound_scoped_pdu_string_offset));
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 3, frame_size + outbound_padding
        - request->outbound_scoped_pdu_string_offset - 1 - 3);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(&(request->outbound_pbuf_stream), &tlv));

    OF_BUILD_EXEC_TRAP(snmpv3_get_user((char*)request->msg_user_name, NULL, NULL, &algo, key));

    OF_BUILD_EXEC_TRAP(snmpv3_crypt(&request->outbound_pbuf_stream, tlv.value_len, key,
        request->msg_privacy_parameters, request->msg_authoritative_engine_boots,
        request->msg_authoritative_engine_time, algo, SNMP_V3_PRIV_MODE_ENCRYPT));
  }

  if (request->version == SNMP_VERSION_3 && (request->msg_flags & SNMP_V3_AUTH_FLAG)) {
    u8_t key[20];
    u8_t algo;
    u8_t hmac[20];

    OF_BUILD_EXEC_TRAP(snmpv3_get_user((char*)request->msg_user_name, &algo, key, NULL, NULL));
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_init(&(request->outbound_pbuf_stream),
        request->outbound_pbuf, 0, request->outbound_pbuf->tot_len));
    OF_BUILD_EXEC_TRAP(snmpv3_auth(&request->outbound_pbuf_stream, frame_size + outbound_padding, key, algo, hmac));

    MEMCPY(request->msg_authentication_parameters, hmac, SNMP_V3_MAX_AUTH_PARAM_LENGTH);
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_init(&request->outbound_pbuf_stream,
                  request->outbound_pbuf, 0, request->outbound_pbuf->tot_len));
    OF_BUILD_EXEC_TRAP(snmp_pbuf_stream_seek_abs(&request->outbound_pbuf_stream,
                  request->outbound_msg_authentication_parameters_offset));

    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 1, SNMP_V3_MAX_AUTH_PARAM_LENGTH);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(&request->outbound_pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(&request->outbound_pbuf_stream,
                  request->msg_authentication_parameters, SNMP_V3_MAX_AUTH_PARAM_LENGTH));
  }
#endif

  pbuf_realloc(request->outbound_pbuf, frame_size + outbound_padding);

  snmp_stats.outgetresponses++;
  snmp_stats.outpkts++;

  return ERR_OK;
}

/**
 * @ingroup snmp_traps
 * 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_pduv2_dst_ip_set(u8_t dst_idx, const ip_addr_t *dst)
{
  if (dst_idx < SNMP_TRAP_DESTINATIONS) {
    ip_addr_set(&trap_pduv2_dst[dst_idx].dip, dst);
  }
}


static err_t
snmp_prepare_outbound_frame_trap_pduv2(struct snmp_request *request)
{
  struct snmp_asn1_tlv tlv;
  struct snmp_pbuf_stream* pbuf_stream = &(request->outbound_pbuf_stream);

  /* try allocating pbuf(s) for maximum response size */
  request->outbound_pbuf = pbuf_alloc(PBUF_TRANSPORT, 1472, PBUF_RAM);
  if (request->outbound_pbuf == NULL) {
    return ERR_MEM;
  }

  snmp_pbuf_stream_init(pbuf_stream, request->outbound_pbuf, 0, request->outbound_pbuf->tot_len);

  /* 'Message' sequence */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 2, 0);
  OF_BUILD_EXEC_TRAP( 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(request->version, &tlv.value_len);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
  OF_BUILD_EXEC_TRAP( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->version) );

#if LWIP_SNMP_V3
  if (request->version < SNMP_VERSION_3) {
#endif
  /* community */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, request->community_strlen);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
  OF_BUILD_EXEC_TRAP( snmp_asn1_enc_raw(pbuf_stream, request->community, request->community_strlen) );
#if LWIP_SNMP_V3
  } else {
    const char* id;

    /* globalData */
    request->outbound_msg_global_data_offset = pbuf_stream->offset;
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 1, 0);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));

    /* msgID */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 1);
    snmp_asn1_enc_s32t_cnt(request->msg_id, &tlv.value_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->msg_id));

    /* msgMaxSize */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 1);
    snmp_asn1_enc_s32t_cnt(request->msg_max_size, &tlv.value_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->msg_max_size));

    /* msgFlags */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, 1);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, &request->msg_flags, 1));

    /* msgSecurityModel */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 1);
    snmp_asn1_enc_s32t_cnt(request->msg_security_model, &tlv.value_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->msg_security_model));

    /* end of msgGlobalData */
    request->outbound_msg_global_data_end = pbuf_stream->offset;

    /* msgSecurityParameters */
    request->outbound_msg_security_parameters_str_offset = pbuf_stream->offset;
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 1, 0);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));

    request->outbound_msg_security_parameters_seq_offset = pbuf_stream->offset;
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 1, 0);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));

    /* msgAuthoritativeEngineID */
    snmpv3_get_engine_id(&id, &request->msg_authoritative_engine_id_len);
    MEMCPY(request->msg_authoritative_engine_id, id, request->msg_authoritative_engine_id_len);
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, request->msg_authoritative_engine_id_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, request->msg_authoritative_engine_id, request->msg_authoritative_engine_id_len));

    request->msg_authoritative_engine_time = snmpv3_get_engine_time();
    request->msg_authoritative_engine_boots = snmpv3_get_engine_boots();

    /* msgAuthoritativeEngineBoots */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
    snmp_asn1_enc_s32t_cnt(request->msg_authoritative_engine_boots, &tlv.value_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->msg_authoritative_engine_boots));

    /* msgAuthoritativeEngineTime */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
    snmp_asn1_enc_s32t_cnt(request->msg_authoritative_engine_time, &tlv.value_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->msg_authoritative_engine_time));

    /* msgUserName */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, request->msg_user_name_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, request->msg_user_name, request->msg_user_name_len));

#if LWIP_SNMP_V3_CRYPTO
    /* msgAuthenticationParameters */
    if (request->msg_flags & SNMP_V3_AUTH_FLAG) {
      memset(request->msg_authentication_parameters, 0, SNMP_V3_MAX_AUTH_PARAM_LENGTH);
      request->outbound_msg_authentication_parameters_offset = pbuf_stream->offset;
      SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 1, SNMP_V3_MAX_AUTH_PARAM_LENGTH);
      OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
      OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, request->msg_authentication_parameters, SNMP_V3_MAX_AUTH_PARAM_LENGTH));
    } else
#endif
    {
      SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, 0);
      OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    }

#if LWIP_SNMP_V3_CRYPTO
    /* msgPrivacyParameters */
    if (request->msg_flags & SNMP_V3_PRIV_FLAG) {
      snmpv3_build_priv_param(request->msg_privacy_parameters);

      SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, SNMP_V3_MAX_PRIV_PARAM_LENGTH);
      OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
      OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, request->msg_privacy_parameters, SNMP_V3_MAX_PRIV_PARAM_LENGTH));
    } else
#endif
    {
      SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, 0);
      OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
    }

    /* End of msgSecurityParameters, so we can calculate the length of this sequence later */
    request->outbound_msg_security_parameters_end = pbuf_stream->offset;

#if LWIP_SNMP_V3_CRYPTO
    /* For encryption we have to encapsulate the payload in an octet string */
    if (request->msg_flags & SNMP_V3_PRIV_FLAG) {
      request->outbound_scoped_pdu_string_offset = pbuf_stream->offset;
      SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 3, 0);
      OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    }
#endif
    /* Scoped PDU
     * Encryption context
     */
    request->outbound_scoped_pdu_seq_offset = pbuf_stream->offset;
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 3, 0);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));

    /* contextEngineID */
    snmpv3_get_engine_id(&id, &request->context_engine_id_len);
    MEMCPY(request->context_engine_id, id, request->context_engine_id_len);
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, request->context_engine_id_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, request->context_engine_id, request->context_engine_id_len));

    /* contextName */
    SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, request->context_name_len);
    OF_BUILD_EXEC_TRAP(snmp_ans1_enc_tlv(pbuf_stream, &tlv));
    OF_BUILD_EXEC_TRAP(snmp_asn1_enc_raw(pbuf_stream, request->context_name, request->context_name_len));
  }
#endif

  /* 'PDU' sequence */
  request->outbound_pdu_offset = pbuf_stream->offset;
//  SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_TRAP), 3, 0);
  SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_TRAP | SNMP_ASN1_CONTEXT_PDU_GET_NEXT_REQ | SNMP_ASN1_CONTEXT_PDU_GET_RESP), 2, 0);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );

  /* request ID */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
  snmp_asn1_enc_s32t_cnt(request->request_id, &tlv.value_len);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
  OF_BUILD_EXEC_TRAP( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, request->request_id) );

  /* error status */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 1);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
  request->outbound_error_status_offset = pbuf_stream->offset;
  OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_write(pbuf_stream, 0) );

  /* error index */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 1);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
  request->outbound_error_index_offset = pbuf_stream->offset;
  OF_BUILD_EXEC_TRAP( snmp_pbuf_stream_write(pbuf_stream, 0) );

  /* 'VarBindList' sequence */
  SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 1, 1);
  OF_BUILD_EXEC_TRAP( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );

  request->outbound_varbind_offset = pbuf_stream->offset;

  return ERR_OK;
}

err_t
snmp_send_trap_pduv2(struct snmp_varbind *varbinds)
{
  err_t err = ERR_OK;
  struct snmp_request request;
  struct snmp_trap_pduv2_dst *td;
  u16_t i;
  ip_addr_t sip;

  memset(&request, 0, sizeof(request));
  request.handle = snmp_traps_handle;
  for (i = 0, td = &trap_pduv2_dst[0]; i < SNMP_TRAP_DESTINATIONS; 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(request.handle, &td->dip, &sip)) {//request.source_ip
			request.source_ip = &td->dip;
			request.source_port  = SNMP_TRAP_PORT;
			request.version  = SNMP_VERSION_2c;
			strcpy((char*)request.community, "public");//snmp_get_community_trap
			request.community_strlen = strlen(request.community);
			request.request_type = SNMP_ASN1_CONTEXT_PDU_TRAP;
			request.request_id = 0x54dc84ac;

			err = snmp_prepare_outbound_frame_trap_pduv2(&request);

			while (varbinds != NULL) {
				err = snmp_append_outbound_varbind(&(request.outbound_pbuf_stream), varbinds);
				varbinds = varbinds->next;
			}
			if (err == ERR_OK) {
			  /* we stored the exception in varbind -> go on */
			  request.error_status = SNMP_ERR_NOERROR;
			} else if (err == ERR_BUF) {
			  request.error_status = SNMP_ERR_TOOBIG;
			} else {
			  request.error_status = SNMP_ERR_GENERROR;
			}
			if (err == ERR_OK) {
				err = snmp_complete_outbound_frame_trap_pduv2(&request);

				if (err == ERR_OK) {
					err = snmp_sendto(request.handle, request.outbound_pbuf, request.source_ip, request.source_port);
					snmp_stats.outtraps++;
				  }
			}
			 if (request.outbound_pbuf != NULL) {
			  pbuf_free(request.outbound_pbuf);
			}
		}
      }
  }

}

#endif