text: /**
   * Copyright (c) 2013 - 2018, Nordic Semiconductor ASA
   * 
   * 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, except as embedded into a Nordic
   *    Semiconductor ASA integrated circuit in a product or a software update for
   *    such product, 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. Neither the name of Nordic Semiconductor ASA nor the names of its
   *    contributors may be used to endorse or promote products derived from this
   *    software without specific prior written permission.
   * 
   * 4. This software, with or without modification, must only be used with a
   *    Nordic Semiconductor ASA integrated circuit.
   * 
   * 5. Any software provided in binary form under this license must not be reverse
   *    engineered, decompiled, modified and/or disassembled.
   * 
   * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
   * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
   * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
   * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS 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.
   * 
   */
  /** @file
   *
   * @defgroup ble_sdk_6lowpan 6LoWPAN Adaptation Layer
   * @{
   * @ingroup ble_sdk_iot
   * @brief 6LoWPAN Adaptation Layer
   *
   * @details This module enables 6LoWPAN over Bluetooth Low Energy.
   *
   */
  
  #include <stdbool.h>
  #include <stdint.h>
  #include <string.h>
  #include "nrf_soc.h"
  #include "nordic_common.h"
  #include "ble_ipsp.h"
  #include "ble_6lowpan.h"
  #include "iot_common.h"
  #include "iot_context_manager.h"
  #include "app_util_platform.h"
  #include "mem_manager.h"
  
  /**
   * @defgroup ble_sdk_6lowpan Module's Log Macros
   * @details Macros used for creating module logs which can be useful in understanding handling
   *          of events or actions on API requests. These are intended for debugging purposes and
   *          can be enabled by defining the IOT_BLE_6LOWPAN_CONFIG_LOG_ENABLED.
   * @note If NRF_LOG_ENABLED is disabled, having IOT_BLE_6LOWPAN_CONFIG_LOG_ENABLED
   *       has no effect.
   * @{
   */
  
  #if IOT_BLE_6LOWPAN_CONFIG_LOG_ENABLED
  
  #define NRF_LOG_MODULE_NAME 6lowpan
  
  #define NRF_LOG_LEVEL       IOT_BLE_6LOWPAN_CONFIG_LOG_LEVEL
  #define NRF_LOG_INFO_COLOR  IOT_BLE_6LOWPAN_CONFIG_INFO_COLOR
  #define NRF_LOG_DEBUG_COLOR IOT_BLE_6LOWPAN_CONFIG_DEBUG_COLOR
  
  #include "nrf_log.h"
  NRF_LOG_MODULE_REGISTER();
  
  #define BLE_6LOWPAN_TRC     NRF_LOG_DEBUG                                                              /**< Used for getting trace of execution in the module. */
  #define BLE_6LOWPAN_ERR     NRF_LOG_ERROR                                                              /**< Used for logging errors in the module. */
  #define BLE_6LOWPAN_DUMP    NRF_LOG_HEXDUMP_DEBUG                                                      /**< Used for dumping octet information to get details of bond information etc. */
  
  #define BLE_6LOWPAN_ENTRY() BLE_6LOWPAN_TRC(">> %s", __func__)
  #define BLE_6LOWPAN_EXIT()  BLE_6LOWPAN_TRC("<< %s", __func__)
  
  #else // IOT_BLE_6LOWPAN_CONFIG_LOG_ENABLED
  
  #define BLE_6LOWPAN_TRC(...)                                                                           /**< Disables traces. */
  #define BLE_6LOWPAN_DUMP(...)                                                                          /**< Disables dumping of octet streams. */
  #define BLE_6LOWPAN_ERR(...)                                                                           /**< Disables error logs. */
  
  #define BLE_6LOWPAN_ENTRY(...)
  #define BLE_6LOWPAN_EXIT(...)
  
  #endif // IOT_BLE_6LOWPAN_CONFIG_LOG_ENABLED
  
  
  
  /** @} */
  
  /**
   * @defgroup ble_6lowpan_mutex_lock_unlock Module's Mutex Lock/Unlock Macros.
   *
   * @details Macros used to lock and unlock modules. Currently, SDK does not use mutexes but
   *          framework is provided in case need arises to use an alternative architecture.
   * @{
   */
  #define BLE_6LOWPAN_MUTEX_LOCK()   SDK_MUTEX_LOCK(m_6lowpan_mutex)                                  /**< Lock module using mutex */
  #define BLE_6LOWPAN_MUTEX_UNLOCK() SDK_MUTEX_UNLOCK(m_6lowpan_mutex)                                /**< Unlock module using mutex */
  /** @} */
  
  /**
   * @defgroup api_param_check API Parameters check macros.
   *
   * @details Macros that verify parameters passed to the module in the APIs. These macros
   *          could be mapped to nothing in final versions of code to save execution and size.
   *          BLE_6LOWPAN_DISABLE_API_PARAM_CHECK should be set to 0 to enable these checks.
   *
   * @{
   */
  
  #if (BLE_6LOWPAN_DISABLE_API_PARAM_CHECK == 0)
  
  /**@brief Macro to check is module is initialized before requesting one of the module procedures. */
  #define VERIFY_MODULE_IS_INITIALIZED()                                                             \
      if (m_event_handler == NULL)                                                                   \
      {                                                                                              \
          return (SDK_ERR_MODULE_NOT_INITIALIZED | BLE_6LOWPAN_ERR_BASE);                            \
      }
  
  /**@brief Verify NULL parameters are not passed to API by application. */
  #define NULL_PARAM_CHECK(PARAM)                                                                    \
      if ((PARAM) == NULL)                                                                           \
      {                                                                                              \
          return (NRF_ERROR_NULL | BLE_6LOWPAN_ERR_BASE);                                            \
      }
  
  /**@brief Check if packet has at least IP Header in it (40 bytes). */
  #define PACKET_LENGTH_CHECK(PARAM)                                                                 \
      if ((PARAM) < IPV6_IP_HEADER_SIZE)                                                             \
      {                                                                                              \
          return (NRF_ERROR_INVALID_PARAM | BLE_6LOWPAN_ERR_BASE);                                   \
      }
  
  #else // BLE_6LOWPAN_DISABLE_API_PARAM_CHECK
  
  #define VERIFY_MODULE_IS_INITIALIZED()
  #define NULL_PARAM_CHECK(PARAM)
  #define PACKET_LENGTH_CHECK(PARAM)
  
  #endif // BLE_6LOWPAN_DISABLE_API_PARAM_CHECK
  
  /** @} */
  
  /**@brief Maximum different between compressed and uncompressed packet. */
  #define IPHC_MAX_COMPRESSED_DIFF (IPV6_IP_HEADER_SIZE + UDP_HEADER_SIZE - 7)
  
  /**@brief Transmit FIFO mask. */
  #define TX_FIFO_MASK (BLE_6LOWPAN_TX_FIFO_SIZE - 1)
  
  /**@brief Value and position of IPHC dispatch. */
  #define IPHC_START_DISPATCH               0x03
  #define IPHC_START_DISPATCH_POS           5
  
  /**@brief Values and positions of IPHC fields. */
  #define IPHC_TF_MASK                      0x18
  #define IPHC_TF_POS                       3
  #define IPHC_NH_MASK                      0x04
  #define IPHC_NH_POS                       2
  #define IPHC_HLIM_MASK                    0x03
  #define IPHC_HLIM_POS                     0
  #define IPHC_CID_MASK                     0x80
  #define IPHC_CID_POS                      7
  #define IPHC_SAC_MASK                     0x40
  #define IPHC_SAC_POS                      6
  #define IPHC_SAM_MASK                     0x30
  #define IPHC_SAM_POS                      4
  #define IPHC_M_MASK                       0x08
  #define IPHC_M_POS                        3
  #define IPHC_DAC_MASK                     0x04
  #define IPHC_DAC_POS                      2
  #define IPHC_DAM_MASK                     0x03
  #define IPHC_DAM_POS                      0
  
  /**@brief IPHC Traffic Flow compression. */
  #define IPHC_TF_DSCP_MASK                 0x3F
  #define IPHC_TF_ECN_MASK                  0xC0
  #define IPHC_TF_ECN_POS                   6
  
  /**@brief IPHC values of fields. */
  #define IPHC_TF_00                        0x00
  #define IPHC_TF_01                        0x01
  #define IPHC_TF_10                        0x02
  #define IPHC_TF_11                        0x03
  #define IPHC_NH_0                         0x00
  #define IPHC_NH_1                         0x01
  #define IPHC_HLIM_00                      0x00
  #define IPHC_HLIM_01                      0x01
  #define IPHC_HLIM_10                      0x02
  #define IPHC_HLIM_11                      0x03
  #define IPHC_CID_0                        0x00
  #define IPHC_CID_1                        0x01
  #define IPHC_SAC_0                        0x00
  #define IPHC_SAC_1                        0x01
  #define IPHC_SAM_00                       0x00
  #define IPHC_SAM_01                       0x01
  #define IPHC_SAM_10                       0x02
  #define IPHC_SAM_11                       0x03
  #define IPHC_M_0                          0x00
  #define IPHC_M_1                          0x01
  #define IPHC_DAC_0                        0x00
  #define IPHC_DAC_1                        0x01
  #define IPHC_DAM_00                       0x00
  #define IPHC_DAM_01                       0x01
  #define IPHC_DAM_10                       0x02
  #define IPHC_DAM_11                       0x03
  
  /**@brief IPHC Context Identifier compression. */
  #define IPHC_CID_SOURCE_MASK              0xF0
  #define IPHC_CID_SOURCE_POS               4
  #define IPHC_CID_DESTINATION_MASK         0x0F
  #define IPHC_CID_DESTINATION_POS          0
  
  /**@brief IPHC Next Header Compression dispatches. */
  #define IPHC_NHC_UDP_DISPATCH             0xF0
  #define IPHC_NHC_UDP_MASK                 0xF8
  #define IPHC_NHC_EXT_DISPATCH             0xE0
  #define IPHC_NHC_EXT_MASK                 0xF0
  
  /**@brief IPHC Next Header Compression UDP fields. */
  #define IPHC_NHC_UDP_CSUM_MASK            0x04
  #define IPHC_NHC_UDP_CSUM_POS             0x02
  #define IPHC_NHC_UDP_PORTS_MASK           0x03
  #define IPHC_NHC_UDP_PORTS_POS            0x00
  #define IPHC_NHC_UDP_PORTS_00             0x00
  #define IPHC_NHC_UDP_PORTS_01             0x01
  #define IPHC_NHC_UDP_PORTS_10             0x02
  #define IPHC_NHC_UDP_PORTS_11             0x03
  
  #define IPHC_NHC_UDP_COMPRESSION_MAX_MASK 0xFFF0
  #define IPHC_NHC_UDP_COMPRESSION_MAX      0xF0B0
  #define IPHC_NHC_UDP_COMPRESSION_MIN_MASK 0xFF00
  #define IPHC_NHC_UDP_COMPRESSION_MIN      0xF000
  
  /**@brief IPHC Next Header Compression Extended Header fields. */
  #define IPHC_NHC_EXT_EID_MASK             0x0E
  #define IPHC_NHC_EXT_EID_POS              0x01
  #define IPHC_NHC_EXT_EID_HOP_BY_HOP       0x00
  #define IPHC_NHC_EXT_EID_ROUTING          0x01
  #define IPHC_NHC_EXT_EID_FRAGMENT         0x02
  #define IPHC_NHC_EXT_EID_DESTINATION      0x03
  #define IPHC_NHC_EXT_EID_MOBILITY         0x04
  #define IPHC_NHC_EXT_EID_IPV6             0x07
  
  /**@brief IPHC default value of IPv6 Header fields. */
  #define IPHC_IPHEADER_VER_TC              0x60
  #define IPHC_IPHEADER_TC_FL               0x00
  #define IPHC_IPHEADER_FL                  0x00
  
  /**@brief Check if address can be fully elidable. */
  #define IPV6_ADDRESS_IS_FULLY_ELIDABLE(ll_addr, addr)             \
      (((addr)->u8[8] == (((ll_addr[0]) ^ IPV6_IID_FLIP_VALUE))) && \
       ((addr)->u8[9] == ll_addr[1]) &&                             \
       ((addr)->u8[10] == ll_addr[2]) &&                            \
       ((addr)->u8[11] == ll_addr[3]) &&                            \
       ((addr)->u8[12] == ll_addr[4]) &&                            \
       ((addr)->u8[11] == 0xff) &&                                  \
       ((addr)->u8[12] == 0xfe) &&                                  \
       ((addr)->u8[13] == ll_addr[5]) &&                            \
       ((addr)->u8[14] == ll_addr[6]) &&                            \
       ((addr)->u8[15] == ll_addr[7])                               \
      )
  
  /**@brief Check if address is 16-bit and can be compressed.
   *        16-bit COMPRESSABLE format: ::0000:00ff:fe00:XXXX.
   */
  #define IPV6_ADDRESS_IS_16_BIT_COMPRESSABLE(addr)       \
      (((addr)->u8[8] == 0) &&                            \
       ((addr)->u8[9] == 0) &&                            \
       ((addr)->u8[10] == 0) &&                           \
       ((addr)->u8[11] == 0xff) &&                        \
       ((addr)->u8[12] == 0xfe) &&                        \
       ((addr)->u8[13] == 0)                              \
      )
  
  /**@brief Check if address is 48-bit multi-cast and can be compressed.
   *        48-bit COMPRESSABLE format: FFXX::00XX:XXXX:XXXX.
   */
  #define IPV6_ADDRESS_IS_48_BIT_MCAST_COMPRESSABLE(addr) \
      (((addr)->u16[1] == 0) &&                           \
       ((addr)->u16[2] == 0) &&                           \
       ((addr)->u16[3] == 0) &&                           \
       ((addr)->u16[4] == 0) &&                           \
       ((addr)->u8[10] == 0)                              \
      )
  
  /**@brief Check if address is 32-bit multi-cast and can be compressed.
   *        32-bit COMPRESSABLE format: FFXX::00XX:XXXX.
   */
  #define IPV6_ADDRESS_IS_32_BIT_MCAST_COMPRESSABLE(addr) \
      (((addr)->u16[1] == 0) &&                           \
       ((addr)->u32[1] == 0) &&                           \
       ((addr)->u32[2] == 0) &&                           \
       ((addr)->u8[12] == 0)                              \
      )
  
  /**@brief Check if address is 8-bit multi-cast and can be compressed.
   *        8-bit COMPRESSABLE format: FF02::XX.
   */
  #define IPV6_ADDRESS_IS_8_BIT_MCAST_COMPRESSABLE(addr)  \
      (((addr)->u8[1] == 2) &&                            \
       ((addr)->u16[1] == 0) &&                           \
       ((addr)->u32[1] == 0) &&                           \
       ((addr)->u32[2] == 0) &&                           \
       ((addr)->u16[6] == 0) &&                           \
       ((addr)->u8[14] == 0)                              \
      )
  
  /******************************************************************************
   * 6LoWPAN Core and Transport structures.
   ******************************************************************************/
  
  /**@brief Element of TX Queue. */
  typedef struct
  {
      uint8_t * p_mem_block;                                                                          /**< Base address of memory block, using for release the buffer. */
      uint8_t * p_data;                                                                               /**< Pointer to TX Data. */
      uint16_t  data_len;                                                                             /**< Size of TX data.  */
  } tx_packet_t;
  
  /**@brief A TX Queue (FIFO) structure. */
  typedef struct
  {
      tx_packet_t       packets[BLE_6LOWPAN_TX_FIFO_SIZE];                                            /**< Array of TX packet in FIFO.              */
      volatile uint32_t read_pos;                                                                     /**< Next read position in the FIFO buffer.   */
      volatile uint32_t write_pos;                                                                    /**< Next write position in the FIFO buffer.  */
  } tx_fifo_t;
  
  /**@brief Transport instance structure. */
  typedef struct
  {
      iot_interface_t         interface;
      ble_ipsp_handle_t       handle;
      tx_fifo_t               tx_fifo;
      tx_packet_t           * p_tx_cur_packet;
  } transport_instance_t;
  
  /******************************************************************************
   * @name Global variables
   *****************************************************************************/
  
  /**@brief Application Event Handler. */
  static ble_6lowpan_evt_handler_t m_event_handler = NULL;
  
  /**@brief Hop Limit options. */
  static const uint8_t m_hop_limit[] = {0, 1, 64, 255};
  
  /**@brief Link-local prefix. */
  static const uint8_t m_link_local_prefix[] = {0xFE, 0x80};
  
  /**@brief Additional extenders for EUI-48. */
  static const uint8_t m_link_local_16_middle[] = {0xFF, 0xFE};
  
  /**@brief nRF EUI-64 link-layer address */
  static eui64_t m_ll_addr = {{0, 0, 0, 0, 0, 0, 0, 0}};
  
  /**@brief Transport interfaces table. */
  static transport_instance_t m_instances[BLE_6LOWPAN_MAX_INTERFACE];
  
  /**@brief Mutex variable. Currently unused, this declaration does not occupy any space in RAM. */
  SDK_MUTEX_DEFINE(m_6lowpan_mutex)
  
  /******************************************************************************
   * @name 6LoWPAN core functions
   *****************************************************************************/
  
  /**@brief Function for checking if IID can be completely elided. This situation
   *        may happen when receiver can still reconstruct IPv6 address by using context
   *        prefix and Link Layer address.
   *
   * @param[in]   p_addr     Pointer to IPv6 address.
   * @param[in]   p_context  Pointer to context entry that is compressed with.
   * @param[in]   p_ll_addr  Pointer to link layer address of BT-LE device.
   *
   * @return      True if IID can be elided, False otherwise.
   */
  static bool is_context_cover_iid(const ipv6_addr_t   * p_addr,
                                   const iot_context_t * p_context,
                                   const eui64_t       * p_ll_addr)
  {
      uint32_t start_byte, offset;
  
      // Context covers IPv6 address by its size.
      if (p_context->prefix_len == 128)
      {
          return true;
      }
  
      // Check if IID can be retrieved in case of longer prefix than 64 bits.
      if (p_context->prefix_len > 64)
      {
          // Check only IID fields that are not covered by context prefix.
          start_byte = p_context->prefix_len >> 3;
          offset = p_context->prefix_len % 8;
  
          // Check all bytes from the second one.
          if (start_byte == 15 ||
              0 == memcmp(&p_addr->u8[start_byte+1], &p_ll_addr->identifier[start_byte-7], 15-start_byte))
          {
              // Then check first byte.
              return (p_addr->u8[start_byte] << offset) == (p_ll_addr->identifier[start_byte-8] << offset);
          }
      }
  
      return false;
  }
  
  /**@brief Function for decoding Next Header Compression.
   *        It supports UDP header decompression.
   *
   * @param[in]   p_iphc   Pointer to currently process IPHC field.
   * @param[in]   p_data   Pointer to constructing uncompressed IP packet.
   * @param[in]   p_length Place of UDP header in case of Extension Header.
   * @param[out]  p_length Length of the constructed uncompressed header.
   *
   * @return      Number of processed IPHC field.
   */
  static uint32_t iphc_nhc_decode(uint8_t * p_iphc, uint8_t * p_data, uint16_t * p_length)
  {
      uint8_t   nhc_dispatch = *p_iphc;
      uint8_t * p_nhc        = p_iphc;
  
      ipv6_header_t * iphdr = (ipv6_header_t *)&p_data[0];
  
      // UDP Next Header Compression.
      if ((nhc_dispatch & IPHC_NHC_UDP_MASK) == IPHC_NHC_UDP_DISPATCH)
      {
          udp6_header_t * udphdr = (udp6_header_t * )&p_data[IPV6_IP_HEADER_SIZE + *p_length];
  
          iphdr->next_header = IPV6_NEXT_HEADER_UDP;
  
          // Start length from UDP Header Size.
          *p_length += UDP_HEADER_SIZE;
          p_nhc     += 1;
  
          switch ((nhc_dispatch & IPHC_NHC_UDP_PORTS_MASK) >> IPHC_NHC_UDP_PORTS_POS)
          {
              case IPHC_NHC_UDP_PORTS_00:
                  memcpy(&udphdr->srcport, p_nhc, 2);
                  memcpy(&udphdr->destport, p_nhc + 2, 2);
                  p_nhc += 4;
                  break;
  
              case IPHC_NHC_UDP_PORTS_01:
                  memcpy(&udphdr->srcport, p_nhc, 2);
                  udphdr->destport = HTONS(IPHC_NHC_UDP_COMPRESSION_MIN | *(p_nhc + 2));
                  p_nhc           += 3;
                  break;
  
              case IPHC_NHC_UDP_PORTS_10:
                  udphdr->srcport = HTONS(IPHC_NHC_UDP_COMPRESSION_MIN | *p_nhc);
                  memcpy(&udphdr->destport, p_nhc + 1, 2);
                  p_nhc += 3;
                  break;
  
              case IPHC_NHC_UDP_PORTS_11:
                  udphdr->srcport  = HTONS((IPHC_NHC_UDP_COMPRESSION_MAX | ((*p_nhc & 0xf0) >> 4)));
                  udphdr->destport = HTONS((IPHC_NHC_UDP_COMPRESSION_MAX | ((*p_nhc & 0x0f))));
                  p_nhc           += 1;
                  break;
          }
  
          if ((nhc_dispatch & IPHC_NHC_UDP_CSUM_MASK) >> IPHC_NHC_UDP_CSUM_POS)
          {
              udphdr->checksum = 0;
          }
          else
          {
              memcpy(&udphdr->checksum, p_nhc, 2);
              p_nhc += 2;
          }
      }
  
      return (p_nhc - p_iphc);
  }
  
  /**@brief Function for encoding Next Header Compression.
   *        It supports UDP header compression.
   *
   * @param[in]   p_iphc   Pointer to currently process IPHC field.
   * @param[in]   p_data   Pointer to constructing uncompressed IP packet.
   * @param[in]   p_length Place of UDP header in case of Extension Header.
   * @param[out]  p_length Length of the constructed uncompressed header.
   *
   * @return      Number of processed IPHC field.
   */
  static uint32_t iphc_nhc_encode(uint8_t * p_iphc, const uint8_t * p_data, uint16_t * p_length)
  {
      uint8_t      * p_nhc = p_iphc;
      ipv6_header_t * iphdr = (ipv6_header_t *)p_data;
  
      switch (iphdr->next_header)
      {
          case IPV6_NEXT_HEADER_UDP:
          {
              udp6_header_t * udphdr = (udp6_header_t * )&p_data[IPV6_IP_HEADER_SIZE + *p_length];
              *p_iphc = IPHC_NHC_UDP_DISPATCH;
              p_nhc  += 1;
  
              // Full 4-bit compression for source and destination ports.
              if ( ((HTONS(udphdr->srcport) & IPHC_NHC_UDP_COMPRESSION_MAX_MASK) ==
                    IPHC_NHC_UDP_COMPRESSION_MAX) &&
                   ((HTONS(udphdr->destport) & IPHC_NHC_UDP_COMPRESSION_MAX_MASK) ==
                    IPHC_NHC_UDP_COMPRESSION_MAX))
              {
                  *p_iphc |= (IPHC_NHC_UDP_PORTS_11 >> IPHC_NHC_UDP_PORTS_POS);
  
                  *p_nhc =
                      (((HTONS(udphdr->srcport) & 0x0f) << 4) | (HTONS(udphdr->destport) & 0x0f));
                  p_nhc += 1;
              }
              // Source port compressed, destination in-line.
              else if ((HTONS(udphdr->srcport) & IPHC_NHC_UDP_COMPRESSION_MIN_MASK) ==
                       IPHC_NHC_UDP_COMPRESSION_MIN)
              {
                  *p_iphc |= (IPHC_NHC_UDP_PORTS_10 >> IPHC_NHC_UDP_PORTS_POS);
  
                  *p_nhc = (HTONS(udphdr->srcport) & 0xff);
                  p_nhc += 1;
  
                  memcpy(p_nhc, &udphdr->destport, 2);
                  p_nhc += 2;
              }
              // Source port in-line, destination compressed.
              else if ((HTONS(udphdr->destport) & IPHC_NHC_UDP_COMPRESSION_MIN_MASK) ==
                       IPHC_NHC_UDP_COMPRESSION_MIN)
              {
                  *p_iphc |= (IPHC_NHC_UDP_PORTS_01 >> IPHC_NHC_UDP_PORTS_POS);
  
                  memcpy(p_nhc, &udphdr->srcport, 2);
                  p_nhc += 2;
  
                  *p_nhc = (HTONS(udphdr->destport) & 0xff);
                  p_nhc += 1;
              }
              // Source and destination port in-line.
              else
              {
                  *p_iphc |= (IPHC_NHC_UDP_PORTS_00 >> IPHC_NHC_UDP_PORTS_POS);
  
                  memcpy(p_nhc, &udphdr->srcport, 2);
                  memcpy(p_nhc + 2, &udphdr->destport, 2);
                  p_nhc += 4;
              }
  
              // Checksum always in-line, [RFC4944] disallows the compression of the
              // UDP checksum. The compressor MUST NOT set the C bit unless it has received
              // authorization.
              memcpy(p_nhc, &udphdr->checksum, 2);
              p_nhc += 2;
  
              // Set UDP ext header size.
              *p_length = UDP_HEADER_SIZE;
  
              break;
          }
      }
  
      return (p_nhc - p_iphc);
  }
  
  
   /**@brief Function for checking if it's possible to use NHC.
   *
   * @param[in]   next_header Value of Next Header field in IPv6 packet.
   *
   * @return      Returns 1 if header can be compressed, otherwise 0.
   */
  static uint32_t iphc_nhc_compressable(uint8_t next_header)
  {
      switch (next_header)
      {
          case IPV6_NEXT_HEADER_UDP:
              return 1;
      }
  
      return 0;
  }
  
  
  /**@brief Function for decoding IPHC (IP Header Compression) defined in
   *        IETF RFC 6282.
   *
   * @param[in]   p_instance   Transport instance from where packet came.
   * @param[in]   p_input      Pointer to received packet from IPSP module.
   * @param[in]   input_len    Length of received packet.
   * @param[in]   p_output     Pointer to allocated buffer for decompressed packet.
   * @param[out]  p_output     Pointer to decompressed IPv6 packet.
   * @param[out]  p_output_len Length of decompressed IPv6 packet.
   *
   * @return      NRF_SUCCESS on success, otherwise an error code.
   */
  static uint32_t iphc_decode(iot_interface_t      * p_interface,
                              uint8_t              * p_output,
                              uint16_t             * p_output_len,
                              uint8_t              * p_input,
                              uint16_t               input_len,
                              iot_context_id_t     * p_rx_contexts)
  {
      uint32_t                retval      = NRF_SUCCESS;
      uint32_t                err_code    = NRF_SUCCESS;
      uint8_t               * p_iphc      = p_input;
      uint8_t                 sci         = IPV6_CONTEXT_IDENTIFIER_NONE;
      uint8_t                 dci         = IPV6_CONTEXT_IDENTIFIER_NONE;
      uint16_t                nhc_length  = 0;
      iot_context_t         * p_ctx       = NULL;
  
      // IPv6 headers used in decompression.
      ipv6_header_t         * p_iphdr     = (ipv6_header_t *)p_output;
      udp6_header_t         * p_udphdr    = (udp6_header_t *)&p_output[IPV6_IP_HEADER_SIZE];
  
      // Check if format of packet is correct.
      if ((p_input[0] >> IPHC_START_DISPATCH_POS) != IPHC_START_DISPATCH)
      {
          BLE_6LOWPAN_ERR("[6LoWPAN] Packet has incorrect IPHC structure!");
  
          return NRF_ERROR_INVALID_DATA;
      }
  
      // IPHC basic form has 2 bytes.
      p_iphc += 2;
  
      // RFC6282: An additional 8-bit Context Identifier Extension field
      // immediately follows the Destination Address Mode (DAM) field.
      if ((p_input[1] & IPHC_CID_MASK))
      {
          sci     = ((*p_iphc & IPHC_CID_SOURCE_MASK) >> IPHC_CID_SOURCE_POS);
          dci     = ((*p_iphc & IPHC_CID_DESTINATION_MASK) >> IPHC_CID_DESTINATION_POS);
          p_iphc += 1;
      }
  
      // Set proper context identifiers.
      p_rx_contexts->src_cntxt_id  = sci;
      p_rx_contexts->dest_cntxt_id = dci;
  
      switch ((p_input[0] & IPHC_TF_MASK) >> IPHC_TF_POS)
      {
          case IPHC_TF_11:
              // Elide Traffic Class and Flow Label.
              p_iphdr->version_traffic_class   = IPHC_IPHEADER_VER_TC;
              p_iphdr->traffic_class_flowlabel = IPHC_IPHEADER_TC_FL;
              p_iphdr->flowlabel               = IPHC_IPHEADER_FL;
              break;
  
          case IPHC_TF_10:
              // Elide Flow Label.
              p_iphdr->version_traffic_class   = IPHC_IPHEADER_VER_TC | ((*p_iphc & IPHC_TF_DSCP_MASK) >> 2);
              p_iphdr->traffic_class_flowlabel = ((*p_iphc & 0x03) << 6) |
                                               ((*p_iphc & IPHC_TF_ECN_MASK) >> 2);
              p_iphdr->flowlabel = IPHC_IPHEADER_FL;
              p_iphc          += 1;
              break;
  
          case IPHC_TF_01:
              // Elide DSCP, carry ECN and Flow Label.
              p_iphdr->version_traffic_class   = IPHC_IPHEADER_VER_TC;
              p_iphdr->traffic_class_flowlabel = ((*p_iphc & IPHC_TF_ECN_MASK) >> 2) |
                                               ((*p_iphc & 0x0f));
              memcpy(&p_iphdr->flowlabel, p_iphc + 1, 2);
              p_iphc += 3;
              break;
  
          case IPHC_TF_00:
              // Flow Label and Traffic Class in-line.
              p_iphdr->version_traffic_class   = IPHC_IPHEADER_VER_TC | ((*p_iphc & IPHC_TF_DSCP_MASK) >> 2);
              p_iphdr->traffic_class_flowlabel = ((*p_iphc & 0x03) << 6) |
                                               ((*p_iphc & IPHC_TF_ECN_MASK) >> 2) |
                                               ((*(p_iphc + 1) & 0x0f));
              memcpy(&p_iphdr->flowlabel, p_iphc + 2, 2);
              p_iphc += 4;
              break;
      }
  
      if (!((p_input[0] & IPHC_NH_MASK)))
      {
          // Set next header.
          p_iphdr->next_header = *p_iphc++;
      }
  
      if ((p_input[0] & IPHC_HLIM_MASK))
      {
          p_iphdr->hoplimit = m_hop_limit[((p_input[0] & IPHC_HLIM_MASK) >> IPHC_HLIM_POS)];
      }
      else
      {
          p_iphdr->hoplimit = *p_iphc++;
      }
  
      // Clear IPv6 addresses.
      memset(p_iphdr->srcaddr.u8, 0, IPV6_ADDR_SIZE);
      memset(p_iphdr->destaddr.u8, 0, IPV6_ADDR_SIZE);
  
      // Source address decompression.
      if ((p_input[1] & IPHC_SAC_MASK) >> IPHC_SAC_POS)
      {
          if ( ((p_input[1] & IPHC_SAM_MASK) >> IPHC_SAM_POS) == IPHC_SAM_00 )
          {
              // Unspecified address.
              memset(p_iphdr->srcaddr.u8, 0, IPV6_ADDR_SIZE);
          }
          else
          {
              switch ((p_input[1] & IPHC_SAM_MASK) >> IPHC_SAM_POS)
              {
                  case IPHC_SAM_01:
                      // 64 bits in-line, first IID then prefix in case prefix > 64.
                      memcpy(p_iphdr->srcaddr.u8 + 8, p_iphc, 8);
                      p_iphc += 8;
                      break;
  
                  case IPHC_SAM_10:
                      // 16 bits in-line.
                      memcpy(p_iphdr->srcaddr.u8 + 14, p_iphc, 2);
                      memcpy(p_iphdr->srcaddr.u8 + 11, m_link_local_16_middle, 2);
                      p_iphc += 2;
                      break;
  
                  case IPHC_SAM_11:
                      // Take the address from lower layer.
                      memcpy(p_iphdr->srcaddr.u8 + 8, p_interface->peer_addr.identifier, 8);
                      p_iphdr->srcaddr.u8[8] ^= IPV6_IID_FLIP_VALUE;
                      break;
              }
  
              /* Look up for context */
              BLE_6LOWPAN_MUTEX_UNLOCK();
              err_code = iot_context_manager_get_by_cid(p_interface, sci, &p_ctx);
              BLE_6LOWPAN_MUTEX_LOCK();
  
              if (err_code == NRF_SUCCESS)
              {
                /* Add prefix */
                IPV6_ADDRESS_PREFIX_SET(p_iphdr->srcaddr.u8, p_ctx->prefix.u8, p_ctx->prefix_len);
              }
              else
              {
                /* Set error and continue decompression. */
                retval = BLE_6LOWPAN_CID_NOT_FOUND;
  
                BLE_6LOWPAN_ERR("Cannot find context identifier in the context table.");
              }
          }
      }
      else
      {
          switch ((p_input[1] & IPHC_SAM_MASK) >> IPHC_SAM_POS)
          {
              case IPHC_SAM_00:
                  // Full 128-bits in-line.
                  memcpy(p_iphdr->srcaddr.u8, p_iphc, IPV6_ADDR_SIZE);
                  p_iphc += IPV6_ADDR_SIZE;
                  break;
  
              case IPHC_SAM_01:
                  // 64 bits in-line, first IID then prefix in case prefix > 64.
                  memcpy(p_iphdr->srcaddr.u8, m_link_local_prefix, 2);
                  memcpy(p_iphdr->srcaddr.u8 + 8, p_iphc, 8);
                  p_iphc += 8;
                  break;
  
              case IPHC_SAM_10:
                  // 16 bits in-line.
                  memcpy(p_iphdr->srcaddr.u8, m_link_local_prefix, 2);
                  memcpy(p_iphdr->srcaddr.u8 + 11, m_link_local_16_middle, 2);
                  memcpy(p_iphdr->srcaddr.u8 + 14, p_iphc, 2);
                  p_iphc += 2;
                  break;
  
              case IPHC_SAM_11:
                  memcpy(p_iphdr->srcaddr.u8, m_link_local_prefix, 2);
                  memcpy(p_iphdr->srcaddr.u8 + 8, p_interface->peer_addr.identifier, 8);
                  p_iphdr->srcaddr.u8[8] ^= IPV6_IID_FLIP_VALUE;
                  break;
          }
      }
  
      // Destination address decompression.
      if ((p_input[1] & IPHC_DAC_MASK) >> IPHC_DAC_POS)
      {
          if ((p_input[1] & IPHC_M_MASK) >> IPHC_M_POS)
          {
              switch ((p_input[1] & IPHC_DAM_MASK) >> IPHC_DAM_POS)
              {
                  case IPHC_DAM_00:
                      // 48-bits in-line.
                      p_iphdr->destaddr.u8[0] = 0xff;
                      memcpy(p_iphdr->destaddr.u8 + 1, p_iphc, 2);
                      memcpy(p_iphdr->destaddr.u8 + 12, p_iphc + 2, 4);
                      p_iphc += 6;
                      break;
  
                  default:
                      BLE_6LOWPAN_ERR("Reserved value in IPHC header!");
                      return NRF_ERROR_INVALID_DATA;
              }
          }
          else
          {
              switch ((p_input[1] & IPHC_DAM_MASK) >> IPHC_DAM_POS)
              {
                  case IPHC_DAM_01:
                      // 64 bits in-line.
                      memcpy(p_iphdr->destaddr.u8 + 8, p_iphc, 8);
                      p_iphc += 8;
                      break;
  
                  case IPHC_DAM_10:
                      // 16 bits in-line.
                      memcpy(p_iphdr->destaddr.u8 + 11, m_link_local_16_middle, 2);
                      memcpy(p_iphdr->destaddr.u8 + 14, p_iphc, 2);
                      p_iphc += 2;
                      break;
  
                  case IPHC_DAM_11:
                      // Take the address from lower layer.
                      memcpy(p_iphdr->destaddr.u8 + 8, p_interface->local_addr.identifier, 8);
                      p_iphdr->destaddr.u8[8] ^= IPV6_IID_FLIP_VALUE;
                      break;
  
                  default:
                      BLE_6LOWPAN_ERR("Reserved value in IPHC header!");
                      return NRF_ERROR_INVALID_DATA;
              }
  
              /* Look up for context */
              BLE_6LOWPAN_MUTEX_UNLOCK();
              err_code = iot_context_manager_get_by_cid(p_interface, dci, &p_ctx);
              BLE_6LOWPAN_MUTEX_LOCK();
  
              if (err_code == NRF_SUCCESS)
              {
                /* Add prefix */
                IPV6_ADDRESS_PREFIX_SET(p_iphdr->destaddr.u8, p_ctx->prefix.u8, p_ctx->prefix_len);
              }
              else
              {
                /* Set error and continue decompression. */
                retval = BLE_6LOWPAN_CID_NOT_FOUND;
  
                BLE_6LOWPAN_ERR("Cannot find context identifier in the context table.");
              }
          }
      }
      else
      {
          if ((p_input[1] & IPHC_M_MASK) >> IPHC_M_POS)
          {
              switch ((p_input[1] & IPHC_DAM_MASK) >> IPHC_DAM_POS)
              {
                  case IPHC_DAM_00:
                      // 128 bits in-line.
                      memcpy(p_iphdr->destaddr.u8, p_iphc, IPV6_ADDR_SIZE);
                      p_iphc += IPV6_ADDR_SIZE;
                      break;
  
                  case IPHC_DAM_01:
                      // 48 bits in-line.
                      p_iphdr->destaddr.u8[0] = 0xFF;
                      p_iphdr->destaddr.u8[1] = *p_iphc;
                      memcpy(p_iphdr->destaddr.u8 + 11, p_iphc + 1, 5);
                      p_iphc += 6;
                      break;
  
                  case IPHC_DAM_10:
                      // 32 bits in-line.
                      p_iphdr->destaddr.u8[0] = 0xFF;
                      p_iphdr->destaddr.u8[1] = *p_iphc;
                      memcpy(p_iphdr->destaddr.u8 + 13, p_iphc + 1, 3);
                      p_iphc += 4;
                      break;
  
                  case IPHC_DAM_11:
                      // 8 bits in-line.
                      p_iphdr->destaddr.u8[0]  = 0xFF;
                      p_iphdr->destaddr.u8[1]  = 0x02;
                      p_iphdr->destaddr.u8[15] = *p_iphc;
                      p_iphc                  += 1;
                      break;
              }
          }
          else
          {
              switch ((p_input[1] & IPHC_DAM_MASK) >> IPHC_DAM_POS)
              {
                  case IPHC_DAM_00:
                      // 128 bits in-line.
                      memcpy(p_iphdr->destaddr.u8, p_iphc, IPV6_ADDR_SIZE);
                      p_iphc += IPV6_ADDR_SIZE;
                      break;
  
                  case IPHC_DAM_01:
                      // 64 bits in-line, first IID then prefix in case prefix > 64.
                      memcpy(p_iphdr->destaddr.u8, m_link_local_prefix, 2);
                      memcpy(p_iphdr->destaddr.u8 + 8, p_iphc, 8);
                      p_iphc += 8;
                      break;
  
                  case IPHC_DAM_10:
                      // 16 bits in-line.
                      memcpy(p_iphdr->destaddr.u8, m_link_local_prefix, 2);
                      memcpy(p_iphdr->destaddr.u8 + 11, m_link_local_16_middle, 2);
                      memcpy(p_iphdr->destaddr.u8 + 14, p_iphc, 2);
                      p_iphc += 2;
                      break;
  
                  case IPHC_DAM_11:
                      // Take the address from lower layer.
                      memcpy(p_iphdr->destaddr.u8, m_link_local_prefix, 2);
                      memcpy(p_iphdr->destaddr.u8 + 8, p_interface->local_addr.identifier, 8);
                      p_iphdr->destaddr.u8[8] ^= IPV6_IID_FLIP_VALUE;
                      break;
              }
          }
      }
  
      // Perform next header compression.
      if (((p_input[0] & IPHC_NH_MASK) >> IPHC_NH_POS))
      {
          p_iphc += iphc_nhc_decode(p_iphc, p_output, &nhc_length);
  
          if (nhc_length == 0)
          {
              // Unknown NHC field.
              BLE_6LOWPAN_ERR("IPHC contains unsupported NHC header!");
  
              return NRF_ERROR_INVALID_DATA;
          }
      }
  
      // Calculate IPv6 Header Length.
      p_iphdr->length = input_len - (p_iphc - p_input);
  
      // Check if IPHC contains more bytes than whole packet.
      if (p_iphdr->length > input_len)
      {
          // We cannot decompress it.
          BLE_6LOWPAN_ERR("IPHC contains more bytes than expected!");
  
          return NRF_ERROR_INVALID_DATA;
      }
  
      // Copy the data.
      memcpy(p_output + IPV6_IP_HEADER_SIZE + nhc_length, p_iphc, p_iphdr->length);
  
      // Add uncompressed headers length if any.
      p_iphdr->length += nhc_length;
  
      // Return length of whole IPv6 packet.
      *p_output_len = p_iphdr->length + IPV6_IP_HEADER_SIZE;
  
      // Keep proper endianness.
      p_iphdr->length = HTONS(p_iphdr->length);
  
      // Restore UDP length if compression was used.
      if ( p_iphdr->next_header == IPV6_NEXT_HEADER_UDP )
      {
          memcpy(&p_udphdr->length, &p_iphdr->length, 2);
      }
  
      return retval;
  }
  
  
  /**@brief Function for encoding IPHC (IP Header Compression) defined in
   *        IETF RFC 6282. Instead of having separate buffer for compression,
   *        needed compression is performed on the IPv6 packet and buffer holding
   *        the packet is reused to overwrite the headers compressed.
   *
   * @param[in]   p_interface  IoT interface where packet must be sent.
   * @param[in]   p_input      Pointer to full IPv6 packet.
   * @param[in]   input_len    Length of IPv6 packet.
   * @param[out]  p_output     Pointer to place of start IPHC packet.
   * @param[out]  p_output_len Length of compressed packet.
   *
   * @return      NRF_SUCCESS on success, otherwise an error code.
   */
  static uint32_t iphc_encode(const iot_interface_t  * p_interface,
                              uint8_t               ** p_output,
                              uint16_t               * p_output_len,
                              const uint8_t          * p_input,
                              uint16_t                 input_len)
  {
      // Create a buffer with maximum of IPHC value.
      uint32_t                err_code;
      uint8_t                 iphc_buff[IPV6_IP_HEADER_SIZE + UDP_HEADER_SIZE];
      uint8_t                 traffic_class;
      uint8_t               * p_iphc         = &iphc_buff[2];
      uint16_t                iphc_len       = 0;
      uint16_t                nhc_length     = 0;
      iot_context_t         * p_ctx          = NULL;
      uint8_t                 sci            = p_interface->tx_contexts.src_cntxt_id;
      uint8_t                 dci            = p_interface->tx_contexts.dest_cntxt_id;
      bool                    sci_cover      = false;
      bool                    dci_cover      = false;
  
      // IPv6 header.
      ipv6_header_t * p_iphdr = (ipv6_header_t *)p_input;
  
      *p_iphc = 0;
  
      // Set IPHC dispatch.
      iphc_buff[0] = IPHC_START_DISPATCH << IPHC_START_DISPATCH_POS;
  
      // Check if address can be compressed using context identifier.
      if (sci == IPV6_CONTEXT_IDENTIFIER_NONE)
      {
          BLE_6LOWPAN_MUTEX_UNLOCK();
          err_code = iot_context_manager_get_by_addr(p_interface, &p_iphdr->srcaddr, &p_ctx);
          BLE_6LOWPAN_MUTEX_LOCK();
  
          if (err_code == NRF_SUCCESS)
          {
              sci_cover = is_context_cover_iid(&p_iphdr->srcaddr, p_ctx, &p_interface->local_addr);
              sci = p_ctx->context_id;
          }
      }
  
      if (dci == IPV6_CONTEXT_IDENTIFIER_NONE)
      {
          BLE_6LOWPAN_MUTEX_UNLOCK();
          err_code = iot_context_manager_get_by_addr(p_interface, &p_iphdr->destaddr, &p_ctx);
          BLE_6LOWPAN_MUTEX_LOCK();
  
          if (err_code == NRF_SUCCESS)
          {
              dci_cover = is_context_cover_iid(&p_iphdr->destaddr, p_ctx, &p_interface->peer_addr);
              dci = p_ctx->context_id;
          }
      }
  
      if (((sci != IPV6_CONTEXT_IDENTIFIER_NONE) || dci != IPV6_CONTEXT_IDENTIFIER_NONE))
      {
          iphc_buff[1] = (IPHC_CID_1 << IPHC_CID_POS);
  
          // Add Source Context if exists.
          if (sci != IPV6_CONTEXT_IDENTIFIER_NONE)
          {
              *p_iphc |= (sci << 4);
          }
  
          // Add Destination Context if exists.
          if (dci != IPV6_CONTEXT_IDENTIFIER_NONE)
          {
              *p_iphc |= dci;
          }
  
          p_iphc += 1;
      }
      else
      {
          // Unset Context Identifier bit.
          iphc_buff[1] = (IPHC_CID_0 << IPHC_CID_POS);
      }
  
      // Change ECN with DSCP in Traffic Class.
      traffic_class  = (p_iphdr->version_traffic_class & 0x0f) << 4;
      traffic_class |= ((p_iphdr->traffic_class_flowlabel & 0xf0) >> 4);
      traffic_class  = (((traffic_class & 0x03) << 6) | (traffic_class >> 2));
  
      if ((p_iphdr->flowlabel == 0) && ((p_iphdr->traffic_class_flowlabel & 0x0f) == 0))
      {
          if (traffic_class == 0)
          {
              // Elide Flow Label and Traffic Class.
              iphc_buff[0] |= (IPHC_TF_11 << IPHC_TF_POS);
          }
          else
          {
              // Elide Flow Label and carry Traffic Class in-line.
              iphc_buff[0] |= (IPHC_TF_10 << IPHC_TF_POS);
  
              *p_iphc++ = traffic_class;
          }
      }
      else
      {
          if (traffic_class & IPHC_TF_DSCP_MASK)
          {
              // Carry Flow Label and Traffic Class in-line.
              iphc_buff[0] |= (IPHC_TF_00 << IPHC_TF_POS);
  
              *p_iphc++ = traffic_class;
              *p_iphc++ = (p_iphdr->traffic_class_flowlabel & 0x0f);
              memcpy(p_iphc, &p_iphdr->flowlabel, 2);
              p_iphc += 2;
          }
          else
          {
              // Carry Flow Label and ECN only with 2-bit padding.
              iphc_buff[0] |= (IPHC_TF_01 << IPHC_TF_POS);
  
              *p_iphc++ =
                  ((traffic_class & IPHC_TF_ECN_MASK) | (p_iphdr->traffic_class_flowlabel & 0x0f));
              memcpy(p_iphc, &p_iphdr->flowlabel, 2);
              p_iphc += 2;
          }
      }
  
      // Checks if next header is compressed.
      if (iphc_nhc_compressable(p_iphdr->next_header))
      {
          iphc_buff[0] |= (IPHC_NH_1 << IPHC_NH_POS);
      }
      else
      {
          iphc_buff[0] |= (IPHC_NH_0 << IPHC_NH_POS);
          *p_iphc++     = p_iphdr->next_header;
      }
  
      // Hop limit compression.
      switch (p_iphdr->hoplimit)
      {
          case 1:
              iphc_buff[0] |= (IPHC_HLIM_01 << IPHC_HLIM_POS);
              break;
  
          case 64:
              iphc_buff[0] |= (IPHC_HLIM_10 << IPHC_HLIM_POS);
              break;
  
          case 255:
              iphc_buff[0] |= (IPHC_HLIM_11 << IPHC_HLIM_POS);
              break;
  
          default:
              // Carry Hop Limit in-line.
              iphc_buff[0] |= (IPHC_HLIM_00 << IPHC_HLIM_POS);
              *p_iphc++     = p_iphdr->hoplimit;
              break;
      }
  
      // Source address compression.
      if (IPV6_ADDRESS_IS_UNSPECIFIED(&p_iphdr->srcaddr))
      {
          iphc_buff[1] |= (IPHC_SAC_1  << IPHC_SAC_POS);
          iphc_buff[1] |= (IPHC_SAM_00 << IPHC_SAM_POS);
      }
      else if (sci != IPV6_CONTEXT_IDENTIFIER_NONE || IPV6_ADDRESS_IS_LINK_LOCAL(&p_iphdr->srcaddr))
      {
          if (sci != IPV6_CONTEXT_IDENTIFIER_NONE)
          {
              // Set stateful source address compression.
              iphc_buff[1] |= (IPHC_SAC_1 << IPHC_SAC_POS);
          }
  
          if (IPV6_ADDRESS_IS_FULLY_ELIDABLE(p_interface->local_addr.identifier,
                                             &p_iphdr->srcaddr)
                                             ||
                                             sci_cover == true)
          {
              iphc_buff[1] |= (IPHC_SAM_11 << IPHC_SAM_POS);
          }
          else if (IPV6_ADDRESS_IS_16_BIT_COMPRESSABLE(&p_iphdr->srcaddr))
          {
              iphc_buff[1] |= (IPHC_SAM_10 << IPHC_SAM_POS);
              memcpy(p_iphc, &p_iphdr->srcaddr.u8[14], 2);
              p_iphc += 2;
          }
          else
          {
              iphc_buff[1] |= (IPHC_SAM_01 << IPHC_SAM_POS);
              memcpy(p_iphc, &p_iphdr->srcaddr.u8[8], 8);
              p_iphc += 8;
          }
      }
      else
      {
          // Carry full source address in-line.
          iphc_buff[1] |= (IPHC_SAC_0  << IPHC_SAC_POS);
          iphc_buff[1] |= (IPHC_SAM_00 << IPHC_SAM_POS);
          memcpy(p_iphc, p_iphdr->srcaddr.u8, IPV6_ADDR_SIZE);
          p_iphc += IPV6_ADDR_SIZE;
      }
  
      // Destination compression.
      if (IPV6_ADDRESS_IS_MULTICAST(&p_iphdr->destaddr))
      {
          iphc_buff[1] |= (IPHC_M_1 << IPHC_M_POS);
  
          if (dci != IPV6_CONTEXT_IDENTIFIER_NONE)
          {
              iphc_buff[1] |= (IPHC_DAC_1 << IPHC_DAC_POS);
              iphc_buff[1] |= (IPHC_DAM_00 << IPHC_DAM_POS);
  
              p_iphdr->destaddr.u8[0] = 0xff;
              memcpy(p_iphc, &p_iphdr->destaddr.u8[1], 2);
              memcpy(p_iphc + 2, &p_iphdr->destaddr.u8[12], 4);
              p_iphc += 6;
          }
          else if (IPV6_ADDRESS_IS_8_BIT_MCAST_COMPRESSABLE(&p_iphdr->destaddr))
          {
              iphc_buff[1] |= (IPHC_DAC_0 << IPHC_DAC_POS);
              iphc_buff[1] |= (IPHC_DAM_11 << IPHC_DAM_POS);
              *p_iphc++     = p_iphdr->destaddr.u8[15];
  
          }
          else if (IPV6_ADDRESS_IS_32_BIT_MCAST_COMPRESSABLE(&p_iphdr->destaddr))
          {
              iphc_buff[1] |= (IPHC_DAC_0 << IPHC_DAC_POS);
              iphc_buff[1] |= (IPHC_DAM_10 << IPHC_DAM_POS);
  
              *p_iphc = p_iphdr->destaddr.u8[1];
              memcpy(p_iphc + 1, &p_iphdr->destaddr.u8[13], 3);
              p_iphc += 4;
          }
          else if (IPV6_ADDRESS_IS_48_BIT_MCAST_COMPRESSABLE(&p_iphdr->destaddr))
          {
              iphc_buff[1] |= (IPHC_DAC_0 << IPHC_DAC_POS);
              iphc_buff[1] |= (IPHC_DAM_01 << IPHC_DAM_POS);
  
              *p_iphc = p_iphdr->destaddr.u8[1];
              memcpy(p_iphc + 1, &p_iphdr->destaddr.u8[11], 5);
              p_iphc += 6;
          }
          else
          {
              // Carry full destination multi-cast address in-line.
              iphc_buff[1] |= (IPHC_DAC_0 << IPHC_DAC_POS);
              iphc_buff[1] |= (IPHC_DAM_00 << IPHC_DAM_POS);
              memcpy(p_iphc, p_iphdr->destaddr.u8, IPV6_ADDR_SIZE);
              p_iphc += IPV6_ADDR_SIZE;
          }
      }
      else
      {
          iphc_buff[1] |= (IPHC_M_0 << IPHC_M_POS);
  
          if (dci != IPV6_CONTEXT_IDENTIFIER_NONE || IPV6_ADDRESS_IS_LINK_LOCAL(&p_iphdr->destaddr))
          {
              if (dci != IPV6_CONTEXT_IDENTIFIER_NONE)
              {
                  iphc_buff[1] |= (IPHC_DAC_1 << IPHC_DAC_POS);
              }
  
              if (IPV6_ADDRESS_IS_FULLY_ELIDABLE(p_interface->peer_addr.identifier,
                                                 &p_iphdr->destaddr)
                                                 ||
                                                 dci_cover == true)
              {
                  iphc_buff[1] |= (IPHC_DAM_11 << IPHC_DAM_POS);
              }
              else if (IPV6_ADDRESS_IS_16_BIT_COMPRESSABLE(&p_iphdr->destaddr))
              {
                  iphc_buff[1] |= (IPHC_DAM_10 << IPHC_DAM_POS);
                  memcpy(p_iphc, &p_iphdr->destaddr.u8[14], 2);
                  p_iphc += 2;
              }
              else
              {
                  iphc_buff[1] |= (IPHC_DAM_01 << IPHC_DAM_POS);
                  memcpy(p_iphc, &p_iphdr->destaddr.u8[8], 8);
                  p_iphc += 8;
              }
          }
          else
          {
              // Carry full destination address in-line.
              iphc_buff[1] |= (IPHC_DAC_0  << IPHC_DAC_POS);
              iphc_buff[1] |= (IPHC_DAM_00 << IPHC_DAM_POS);
              memcpy(p_iphc, p_iphdr->destaddr.u8, IPV6_ADDR_SIZE);
              p_iphc += IPV6_ADDR_SIZE;
          }
      }
  
      if ( iphc_buff[0] & IPHC_NH_MASK)
      {
          p_iphc += iphc_nhc_encode(p_iphc, p_input, &nhc_length);
      }
  
      // Calculate IPHC size.
      iphc_len = (p_iphc - iphc_buff);
  
      // Calculate IPv6 packet size.
      *p_output_len = input_len - (IPV6_IP_HEADER_SIZE - iphc_len + nhc_length);
  
      // Use p_data as final buffer (since IPHC+NHC <= IPv6 Header + NHC (UDP)).
      memcpy((uint8_t *)&p_input[IPV6_IP_HEADER_SIZE + nhc_length - iphc_len], iphc_buff, iphc_len);
  
      // Set correct address of output data.
      *p_output = (uint8_t *) &p_input[IPV6_IP_HEADER_SIZE + nhc_length - iphc_len];
  
      return NRF_SUCCESS;
  }
  
  
  /******************************************************************************
   * @name 6LoWPAN transport functions
   *****************************************************************************/
  
  /**@brief Function for notifying application of an error in an ongoing procedure.
   *
   * @param[in]   p_interface  Pointer to 6LoWPAN interface.
   * @param[in]   err_code     Internally error code.
   *
   * @return      None.
   */
  static void app_notify_error(iot_interface_t * p_interface,
                               uint32_t          err_code)
  {
      ble_6lowpan_event_t event;
  
      event.event_id     = BLE_6LO_EVT_ERROR;
      event.event_result = err_code;
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      m_event_handler(p_interface, &event);
  
      BLE_6LOWPAN_MUTEX_LOCK();
  }
  
  
  /**@brief Function for notifying application of the new packet received.
   *
   * @param[in]   p_interface   Pointer to iot interface.
   * @param[in]   p_packet      Pointer to decompressed IPv6 packet.
   * @param[in]   packet_len    Length of IPv6 packet.
   * @param[in]   result        Processing result of packet. Could be NRF_SUCCESS, or
   *                            NRF_ERROR_NOT_FOUND if context identifier is unknown.
   * @param[in]  p_rx_contexts  Received contexts if any.
   *
   * @return      None.
   */
  static void app_notify_rx_data(iot_interface_t  * p_interface,
                                 uint8_t          * p_packet,
                                 uint16_t           packet_len,
                                 uint32_t           result,
                                 iot_context_id_t * p_rx_contexts)
  {
      ble_6lowpan_event_t event;
  
      event.event_id                               = BLE_6LO_EVT_INTERFACE_DATA_RX;
      event.event_result                           = result;
      event.event_param.rx_event_param.p_packet    = p_packet;
      event.event_param.rx_event_param.packet_len  = packet_len;
      event.event_param.rx_event_param.rx_contexts = *(p_rx_contexts);
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      m_event_handler(p_interface, &event);
  
      BLE_6LOWPAN_MUTEX_LOCK();
  }
  
  
  /**@brief Function for notifying application of the new interface established.
   *
   * @param[in]   p_interface  Pointer to new iot interface.
   *
   * @return      None.
   */
  static void app_notify_interface_add(iot_interface_t * p_interface)
  {
      ble_6lowpan_event_t event;
  
      event.event_id     = BLE_6LO_EVT_INTERFACE_ADD;
      event.event_result = NRF_SUCCESS;
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      m_event_handler(p_interface, &event);
  
      BLE_6LOWPAN_MUTEX_LOCK();
  }
  
  
  /**@brief Function for notifying application of the interface disconnection.
   *
   * @param[in]   p_interface  Pointer to removed iot interface.
   *
   * @return      None.
   */
  static void app_notify_interface_delete(iot_interface_t * p_interface)
  {
      ble_6lowpan_event_t event;
  
      event.event_id     = BLE_6LO_EVT_INTERFACE_DELETE;
      event.event_result = NRF_SUCCESS;
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      m_event_handler(p_interface, &event);
  
      BLE_6LOWPAN_MUTEX_LOCK();
  }
  
  
  /**@brief Function for initialize transmit FIFO.
   *
   * @param[in]   p_fifo  Pointer to transmit FIFO instance.
   *
   * @return      None.
   */
  static void tx_fifo_init(tx_fifo_t * p_fifo)
  {
      memset(p_fifo->packets, 0, BLE_6LOWPAN_TX_FIFO_SIZE * sizeof (tx_packet_t));
      p_fifo->read_pos  = 0;
      p_fifo->write_pos = 0;
  }
  
  
  /**@brief Function for putting new packet to transmit FIFO.
   *
   * @param[in]   p_fifo   Pointer to transmit FIFO instance.
   * @param[in]   p_packet Pointer to new packet.
   *
   * @return      NRF_SUCCESS on success, otherwise NRF_ERROR_NO_MEM error.
   */
  static uint32_t tx_fifo_put(tx_fifo_t * p_fifo, tx_packet_t * p_packet)
  {
      uint32_t err_code = BLE_6LOWPAN_TX_FIFO_FULL;
  
      // To prevent "The order of volatile accesses is undefined in this statement"
      // in subsequent conditional statement.
      uint32_t write_pos = p_fifo->write_pos;
      uint32_t read_pos  = p_fifo->read_pos;
  
      if ((write_pos - read_pos) <= TX_FIFO_MASK)
      {
          p_fifo->packets[p_fifo->write_pos & TX_FIFO_MASK].p_data      = p_packet->p_data;
          p_fifo->packets[p_fifo->write_pos & TX_FIFO_MASK].data_len    = p_packet->data_len;
          p_fifo->packets[p_fifo->write_pos & TX_FIFO_MASK].p_mem_block = p_packet->p_mem_block;
  
          p_fifo->write_pos++;
  
          err_code = NRF_SUCCESS;
      }
  
      return err_code;
  }
  
  
  /**@brief Function for popping currently processed packet in transmit FIFO.
   *        It releases element on FIFO only when processing of the element is done.
   *
   * @param[in]   p_fifo   Pointer to transmit FIFO instance.
   *
   * @return      None.
   */
  static void tx_fifo_release(tx_fifo_t * p_fifo)
  {
      p_fifo->read_pos++;
  }
  
  
  /**@brief Function for reading front element of transmit FIFO.
   *        After finish processing element in queue, it must be
   *        released using tx_fifo_release function.
   *
   * @param[in]   p_fifo    Pointer to transmit FIFO instance.
   * @param[in]   pp_packet Pointer to front packet.
   *
   * @return      NRF_SUCCESS on success, otherwise NRF_ERROR_NO_MEM error.
   */
  static uint32_t tx_fifo_get(tx_fifo_t * p_fifo, tx_packet_t * * pp_packet)
  {
      uint32_t err_code = NRF_ERROR_NOT_FOUND;
  
      // To prevent "The order of volatile accesses is undefined in this statement"
      // in subsequent conditional statement.
      uint32_t write_pos = p_fifo->write_pos;
      uint32_t read_pos  = p_fifo->read_pos;
  
      if ((write_pos - read_pos) != 0)
      {
          *pp_packet = &p_fifo->packets[p_fifo->read_pos & TX_FIFO_MASK];
          err_code   = NRF_SUCCESS;
      }
  
      return err_code;
  }
  
  
  /**@brief Function for searching transport interface by given IPSP handle.
   *
   * @param[in]   p_ipsp_handle Pointer to IPSP handle.
   *
   * @return      Transport interface related with IPSP handle, or NULL if not found.
   */
  static transport_instance_t * interface_get_by_handle(const ble_ipsp_handle_t * p_ipsp_handle)
  {
      uint32_t index;
  
      for (index = 0; index < BLE_6LOWPAN_MAX_INTERFACE; index++)
      {
          if (m_instances[index].handle.cid         == p_ipsp_handle->cid &&
              m_instances[index].handle.conn_handle == p_ipsp_handle->conn_handle)
          {
              return &m_instances[index];
          }
      }
  
      return NULL;
  }
  
  
  /**@brief Function for initializing transport instance.
   *
   * @param[in]   index  Index of instance.
   *
   * @return      None.
   */
  static void instance_init(uint32_t index)
  {
      memset(&m_instances[index], 0, sizeof (transport_instance_t));
      m_instances[index].handle.cid            = BLE_L2CAP_CID_INVALID;
      m_instances[index].p_tx_cur_packet       = NULL;
      m_instances[index].interface.p_transport = (void *) index;
  }
  
  
  /**@brief Function for resetting specific 6lowpan interface.
   *
   * @param[in]   p_interface  Pointer to transport interface.
   *
   * @return      None.
   */
  static void interface_reset(transport_instance_t * p_instance)
  {
      uint32_t index;
      uint32_t instance_id = (uint32_t) p_instance->interface.p_transport;
  
      // Free all queued packets.
      for (index = 0; index < BLE_6LOWPAN_TX_FIFO_SIZE; index++)
      {
          if (m_instances[instance_id].tx_fifo.packets[index].p_mem_block != NULL)
          {
              nrf_free(m_instances[instance_id].tx_fifo.packets[index].p_mem_block);
          }
      }
  
      instance_init (instance_id);
  }
  
  
  /**@brief Function for adding new transport instance.
   *
   * @param[in]   p_peer_addr   Pointer EUI-64 of peer address.
   * @param[in]   p_ipsp_handle Pointer IPSP handle, related with L2CAP CoC channel.
   * @param[out]  pp_instance   Pointer to added transport instances, if operation succeeded.
   *
   * @return      NRF_SUCCESS on success, otherwise NRF_ERROR_NO_MEM error.
   */
  static uint32_t interface_add(const eui64_t             * p_peer_addr,
                                const ble_ipsp_handle_t   * p_ipsp_handle,
                                transport_instance_t     ** pp_instance)
  {
      uint32_t index;
  
      for (index = 0; index < BLE_6LOWPAN_MAX_INTERFACE; index++)
      {
          if (m_instances[index].handle.cid == BLE_L2CAP_CID_INVALID)
          {
              m_instances[index].handle.cid                          = p_ipsp_handle->cid;
              m_instances[index].handle.conn_handle                  = p_ipsp_handle->conn_handle;
              m_instances[index].interface.tx_contexts.src_cntxt_id  = IPV6_CONTEXT_IDENTIFIER_NONE;
              m_instances[index].interface.tx_contexts.dest_cntxt_id = IPV6_CONTEXT_IDENTIFIER_NONE;
  
              memcpy(&m_instances[index].interface.peer_addr, p_peer_addr, sizeof (eui64_t));
              memcpy(&m_instances[index].interface.local_addr, &m_ll_addr, sizeof (eui64_t));
  
              // Initialize TX FIFO.
              tx_fifo_init(&m_instances[index].tx_fifo);
  
              *pp_instance = &m_instances[index];
  
              return NRF_SUCCESS;
          }
      }
  
      return NRF_ERROR_NO_MEM;
  }
  
  
  /**@brief Function for checking if any transmission is currently processing on specific interface.
   *        Current version of L2CAP CoC in SoftDevice has limitation to send one packet at same
   *        time.
   *
   * @param[in]   p_instance  Pointer to transport instance.
   *
   * @return      TRUE if interface not sending any packets, FALSE if busy.
   */
  static bool tx_is_free(transport_instance_t * p_instance)
  {
      return (NULL == p_instance->p_tx_cur_packet);
  }
  
  
  /**@brief Function uses for indicating transmission complete on specific interface.
   *
   * @param[in]   p_instance  Pointer to transport instance.
   *
   * @return      None.
   */
  static void tx_complete(transport_instance_t * p_instance)
  {
      BLE_6LOWPAN_TRC("[CID 0x%04X]: Transmission complete.",
                      p_instance->handle.cid);
  
      // Free the transmit buffer.
      nrf_free(p_instance->p_tx_cur_packet->p_mem_block);
      p_instance->p_tx_cur_packet = NULL;
  
      // Release last processed packet.
      tx_fifo_release(&p_instance->tx_fifo);
  }
  
  
  /**@brief Function for sending front packet from transmit FIFO on specific interface.
   *
   * @param[in]   p_instance  Pointer to transport instance.
   *
   * @return      None.
   */
  static void tx_send(transport_instance_t * p_instance)
  {
      uint32_t err_code = NRF_SUCCESS;
  
      if (NRF_SUCCESS == tx_fifo_get(&p_instance->tx_fifo,
                                     &p_instance->p_tx_cur_packet))
      {
          err_code = ble_ipsp_send(&p_instance->handle,
                                   p_instance->p_tx_cur_packet->p_data,
                                   p_instance->p_tx_cur_packet->data_len);
  
          if (NRF_SUCCESS != err_code)
          {
              BLE_6LOWPAN_TRC("Cannot send the packet, error = 0x%08lX", err_code);
  
              app_notify_error(&p_instance->interface, err_code);
  
              tx_complete(p_instance);
  
              // Try to send another packet.
              tx_send(p_instance);
          }
      }
  }
  
  /**@brief Callback registered with IPSP to receive asynchronous events from the module.
   *
   * @param[in]   p_handle  Pointer to IPSP handle.
   * @param[in]   p_evt     Pointer to specific event, generated by IPSP module.
   *
   * @return      NRF_SUCCESS on success, otherwise NRF_ERROR_NO_MEM error.
   */
  static uint32_t ipsp_evt_handler(ble_ipsp_handle_t const * p_handle, ble_ipsp_evt_t const * p_evt)
  {
      BLE_6LOWPAN_ENTRY();
  
      VERIFY_MODULE_IS_INITIALIZED();
  
      uint32_t               mem_size;
      uint16_t               buff_len;
      eui64_t                peer_addr;
      iot_context_id_t       rx_contexts;
      uint32_t               retval       = NRF_SUCCESS;
      transport_instance_t * p_instance   = NULL;
      uint8_t              * p_buff       = NULL;
  
      BLE_6LOWPAN_MUTEX_LOCK();
  
      p_instance = interface_get_by_handle(p_handle);
  
      switch (p_evt->evt_id)
      {
          case BLE_IPSP_EVT_CHANNEL_CONNECTED:
          {
              BLE_6LOWPAN_TRC("[CID 0x%04X]: >> BLE_IPSP_EVT_CHANNEL_CONNECTED",
                              p_handle->cid);
              BLE_6LOWPAN_TRC("New channel established.");
  
              if (p_instance == NULL)
              {
                  IPV6_EUI64_CREATE_FROM_EUI48(peer_addr.identifier,
                                          p_evt->p_evt_param->p_peer->addr,
                                          p_evt->p_evt_param->p_peer->addr_type);
  
                  // Add interface to internal table.
                  retval = interface_add(&peer_addr, p_handle, &p_instance);
  
                  if (NRF_SUCCESS == retval)
                  {
                      BLE_6LOWPAN_TRC("Added new IPSP interface.");
  
                      // Notify application.
                      app_notify_interface_add(&p_instance->interface);
                  }
                  else
                  {
                      BLE_6LOWPAN_ERR("Cannot add new interface. Table is full.");
                      UNUSED_VARIABLE(ble_ipsp_disconnect(p_handle));
                  }
              }
              else
              {
                  // Got a connection event, when already connected.
                  UNUSED_VARIABLE(ble_ipsp_disconnect(p_handle));
              }
  
              break;
          }
  
          case BLE_IPSP_EVT_CHANNEL_DISCONNECTED:
          {
              BLE_6LOWPAN_TRC("[CID 0x%04X]: >> BLE_IPSP_EVT_CHANNEL_DISCONNECTED",
                              p_handle->cid);
              BLE_6LOWPAN_TRC("Channel disconnection.");
  
              if (NULL != p_instance)
              {
                  BLE_6LOWPAN_TRC("Removed IPSP interface.");
  
                  // Notify application.
                  app_notify_interface_delete(&p_instance->interface);
  
                  // Remove interface from internal table.
                  interface_reset(p_instance);
              }
  
              break;
          }
  
          case BLE_IPSP_EVT_CHANNEL_DATA_RX:
          {
  
              if (NULL != p_instance)
              {
                  const uint16_t packet_len = MIN(p_evt->p_evt_param->p_l2cap_evt->params.rx.sdu_buf.len,
                                                  p_evt->p_evt_param->p_l2cap_evt->params.rx.sdu_len);
  
                  BLE_6LOWPAN_TRC("[CID 0x%04X]: >> BLE_IPSP_EVT_CHANNEL_DATA_RX",
                                  p_handle->cid);
  
                  BLE_6LOWPAN_DUMP(p_evt->p_evt_param->p_l2cap_evt->params.rx.sdu_buf.p_data,
                                   packet_len);
  
                  BLE_6LOWPAN_TRC("Processing received data.");
  
                  mem_size = packet_len + IPHC_MAX_COMPRESSED_DIFF;
  
                  // Try to allocate memory for incoming data.
                  retval = nrf_mem_reserve(&p_buff, &mem_size);
  
                  if (retval == NRF_SUCCESS)
                  {
                      // Decompress IPHC data into IPv6 packet.
                      retval = iphc_decode(&p_instance->interface,
                                           p_buff,
                                           &buff_len,
                                           p_evt->p_evt_param->p_l2cap_evt->params.rx.sdu_buf.p_data,
                                           packet_len,
                                           &rx_contexts);
  
                      // Do not discard if packet decompressed successfully,
                      // otherwise error in Context based decompression.
                      if (retval == NRF_SUCCESS || retval == BLE_6LOWPAN_CID_NOT_FOUND)
                      {
                          if ((p_evt->evt_result == NRF_ERROR_BLE_IPSP_RX_PKT_TRUNCATED) &&
                              (retval == NRF_SUCCESS))
                          {
                              // Inform the application that the packet is truncated.
                              retval = NRF_ERROR_BLE_IPSP_RX_PKT_TRUNCATED;
                          }
  
                          BLE_6LOWPAN_TRC("Decompressed packet:");
                          BLE_6LOWPAN_DUMP(p_buff, buff_len);
  
                          // Notify application.
                          app_notify_rx_data(&p_instance->interface,
                                             p_buff,
                                             buff_len,
                                             retval,
                                             &rx_contexts);
                      }
                      else
                      {
                          BLE_6LOWPAN_ERR("Decompression failed!");
  
                          nrf_free(p_buff);
                      }
                  }
                  else
                  {
                      BLE_6LOWPAN_ERR(
                          "No buffer in memory pool available, packet dropped!");
                  }
              }
              else
              {
                  BLE_6LOWPAN_ERR("Got data to unknown interface!");
              }
  
              break;
          }
  
          case BLE_IPSP_EVT_CHANNEL_DATA_TX_COMPLETE:
          {
              BLE_6LOWPAN_TRC("[CID 0x%04X]: >> BLE_IPSP_EVT_CHANNEL_DATA_TX_COMPLETE",
                              p_handle->cid);
  
              // Free TX buffer.
              tx_complete(p_instance);
  
              // Try to send another packet.
              tx_send(p_instance);
  
              break;
          }
  
          default:
              break;
      }
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      BLE_6LOWPAN_EXIT();
  
      return retval;
  }
  
  /******************************************************************************
   * @name 6LoWPAN API functions
   *****************************************************************************/
  
  uint32_t ble_6lowpan_init(const ble_6lowpan_init_t * p_init)
  {
      BLE_6LOWPAN_ENTRY();
  
      uint32_t        index;
      uint32_t        retval = NRF_SUCCESS;
      ble_ipsp_init_t ipsp_init_params;
  
      NULL_PARAM_CHECK(p_init);
      NULL_PARAM_CHECK(p_init->p_eui64);
      NULL_PARAM_CHECK(p_init->event_handler);
  
      // Check if the transmit FIFO size is a power of two.
      if (!IS_POWER_OF_TWO(BLE_6LOWPAN_TX_FIFO_SIZE))
      {
          return NRF_ERROR_INVALID_LENGTH | BLE_6LOWPAN_ERR_BASE;
      }
  
      SDK_MUTEX_INIT(m_6lowpan_mutex);
  
      BLE_6LOWPAN_MUTEX_LOCK();
  
      // Store EUI64 in internal variable.
      memcpy(m_ll_addr.identifier, p_init->p_eui64->identifier, EUI_64_ADDR_SIZE);
  
      // Set application event handler.
      m_event_handler = p_init->event_handler;
  
      // Clear transport interfaces table.
      for (index = 0; index < BLE_6LOWPAN_MAX_INTERFACE; index++)
      {
          instance_init(index);
      }
  
      // IPSP module initialization.
      ipsp_init_params.evt_handler = ipsp_evt_handler;
  
      // Initialize the IPSP module.
      retval = ble_ipsp_init(&ipsp_init_params);
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      BLE_6LOWPAN_EXIT();
  
      return retval;
  }
  
  
  uint32_t  ble_6lowpan_interface_disconnect(const iot_interface_t * p_interface)
  {
      BLE_6LOWPAN_ENTRY();
  
      VERIFY_MODULE_IS_INITIALIZED();
  
      NULL_PARAM_CHECK(p_interface);
  
      uint32_t               retval;
      transport_instance_t * p_instance = &m_instances[(uint32_t)p_interface->p_transport];
  
      BLE_6LOWPAN_MUTEX_LOCK();
  
      retval = ble_ipsp_disconnect(&p_instance->handle);
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      BLE_6LOWPAN_EXIT();
  
      return retval;
  }
  
  
  uint32_t ble_6lowpan_interface_send(const iot_interface_t * p_interface,
                                      const uint8_t         * p_packet,
                                      uint16_t                packet_len)
  {
      BLE_6LOWPAN_ENTRY();
  
      VERIFY_MODULE_IS_INITIALIZED();
  
      NULL_PARAM_CHECK(p_packet);
      NULL_PARAM_CHECK(p_interface);
      PACKET_LENGTH_CHECK(packet_len);
  
      uint32_t               retval        = NRF_SUCCESS;
      uint8_t              * p_output_buff = NULL;
      uint16_t               output_len;
      tx_packet_t            tx_packet;
      transport_instance_t * p_instance = &m_instances[(uint32_t)p_interface->p_transport];
  
      BLE_6LOWPAN_MUTEX_LOCK();
  
      BLE_6LOWPAN_TRC("Uncompressed packet:");
      BLE_6LOWPAN_DUMP((uint8_t *)p_packet, packet_len);
  
      // Encode IP packet into IPHC.
      retval = iphc_encode(p_interface,
                           &p_output_buff,
                           &output_len,
                           p_packet,
                           packet_len);
  
      if (NRF_SUCCESS == retval)
      {
          BLE_6LOWPAN_TRC("Successfully compressed packet.");
  
          tx_packet.p_data      = p_output_buff;
          tx_packet.data_len    = output_len;
          tx_packet.p_mem_block = (uint8_t *)p_packet;
  
          retval = tx_fifo_put(&p_instance->tx_fifo, &tx_packet);
  
          if (NRF_SUCCESS == retval)
          {
              BLE_6LOWPAN_TRC("Compressed packet:");
              BLE_6LOWPAN_DUMP(p_output_buff, output_len);
  
              // Send packet immediately if transport interface is not busy.
              if (tx_is_free(p_instance))
              {
                  tx_send(p_instance);
              }
          }
          else
          {
              BLE_6LOWPAN_ERR("No place in TX queue!");
          }
      }
      else
      {
          BLE_6LOWPAN_ERR("Error while compression!");
      }
  
      BLE_6LOWPAN_MUTEX_UNLOCK();
  
      BLE_6LOWPAN_EXIT();
  
      return retval;
  }
  