diff options
Diffstat (limited to 'thirdparty/nRF5_SDK_15.0.0_a53641a/components/iot/ble_6lowpan/ble_6lowpan.c')
| -rw-r--r-- | thirdparty/nRF5_SDK_15.0.0_a53641a/components/iot/ble_6lowpan/ble_6lowpan.c | 1978 |
1 files changed, 1978 insertions, 0 deletions
diff --git a/thirdparty/nRF5_SDK_15.0.0_a53641a/components/iot/ble_6lowpan/ble_6lowpan.c b/thirdparty/nRF5_SDK_15.0.0_a53641a/components/iot/ble_6lowpan/ble_6lowpan.c new file mode 100644 index 0000000..b7bf6fa --- /dev/null +++ b/thirdparty/nRF5_SDK_15.0.0_a53641a/components/iot/ble_6lowpan/ble_6lowpan.c @@ -0,0 +1,1978 @@ +/** + * 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; +} |