uint32_t pstorage_load(uint8_t *           p_dest,
                       pstorage_handle_t * p_src,
                       pstorage_size_t     size,
                       pstorage_size_t     offset)
{
    VERIFY_MODULE_INITIALIZED();
    NULL_PARAM_CHECK(p_src);
    NULL_PARAM_CHECK(p_dest);
    MODULE_ID_RANGE_CHECK (p_src);
    BLOCK_ID_RANGE_CHECK(p_src);
    SIZE_CHECK(p_src,size);
    OFFSET_CHECK(p_src,offset,size);

    // Verify word alignment.
    if ((!is_word_aligned(p_dest)) || (!is_word_aligned((void *)(uint32_t)offset)))
    {
        return NRF_ERROR_INVALID_ADDR;
    }

    memcpy(p_dest, (((uint8_t *)p_src->block_id) + offset), size);

    app_notify(p_src, p_dest, PSTORAGE_LOAD_OP_CODE, size, NRF_SUCCESS);

    return NRF_SUCCESS;
}

uint32_t iot_context_manager_get_by_cid(const iot_interface_t * p_interface, 
                                        uint8_t           context_id, 
                                        iot_context_t  ** pp_context)
{
    VERIFY_MODULE_IS_INITIALIZED();
    NULL_PARAM_CHECK(p_interface);
    NULL_PARAM_CHECK(pp_context);
    VERIFY_CID_VALUE(context_id);    

    uint32_t err_code;

    CM_TRC("[CONTEXT_MANAGER]: >> iot_context_manager_get_by_cid\r\n");

    CM_MUTEX_LOCK();

    const uint32_t table_id = context_table_find(p_interface);

    if (table_id != IOT_CONTEXT_MANAGER_MAX_TABLES)
    {
        err_code = context_find_by_cid(table_id, context_id, pp_context);
    }
    else
    {
        // No free context table found.
        CM_TRC("[CONTEXT_MANAGER]: No context table found.\r\n");
        err_code = (NRF_ERROR_NOT_FOUND | IOT_CONTEXT_MANAGER_ERR_BASE);
    }

    CM_MUTEX_UNLOCK();

    CM_TRC("[CONTEXT_MANAGER]: << iot_context_manager_get_by_cid\r\n");

    return err_code;
}

uint32_t pstorage_update(pstorage_handle_t * p_dest,
                         uint8_t           * p_src,
                         pstorage_size_t     size,
                         pstorage_size_t     offset)
{
    VERIFY_MODULE_INITIALIZED();
    NULL_PARAM_CHECK(p_src);
    NULL_PARAM_CHECK(p_dest);
    MODULE_ID_RANGE_CHECK(p_dest);
    BLOCK_ID_RANGE_CHECK(p_dest);
    SIZE_CHECK(p_dest, size);
    OFFSET_CHECK(p_dest, offset, size);

    // Verify word alignment.
    if ((!is_word_aligned(p_src)) || (!is_word_aligned((void *)(uint32_t)offset)))
    {
        return NRF_ERROR_INVALID_ADDR;
    }

    if ((!is_word_aligned((uint32_t *)p_dest->block_id)))
    {
        return NRF_ERROR_INVALID_ADDR;
    }

    return cmd_queue_enqueue(PSTORAGE_UPDATE_OP_CODE, p_dest, p_src, size, offset);
}

uint32_t pstorage_store(pstorage_handle_t * p_dest,
                        uint8_t           * p_src,
                        pstorage_size_t     size,
                        pstorage_size_t     offset)
{
    VERIFY_MODULE_INITIALIZED();
    NULL_PARAM_CHECK(p_src);
    NULL_PARAM_CHECK(p_dest);
    MODULE_ID_RANGE_CHECK(p_dest);
    BLOCK_ID_RANGE_CHECK(p_dest);
    SIZE_CHECK(p_dest, size);
    OFFSET_CHECK(p_dest, offset, size);

    // Verify word alignment.
    if ((!is_word_aligned(p_src)) || (!is_word_aligned((void *)(uint32_t)offset)))
    {
        return NRF_ERROR_INVALID_ADDR;
    }

    uint32_t storage_addr = p_dest->block_id + offset;

    uint32_t retval = ble_flash_block_write((uint32_t *)storage_addr,
                                            (uint32_t *)p_src,
                                            (size /sizeof(uint32_t)));

    app_notify(p_dest, p_src, PSTORAGE_STORE_OP_CODE, size, retval);
    
    return retval;
}

uint32_t ipv6_send(const iot_interface_t * p_interface, iot_pbuffer_t * p_packet)
{
    VERIFY_MODULE_IS_INITIALIZED();

    NULL_PARAM_CHECK(p_packet);
    NULL_PARAM_CHECK(p_interface);

    uint32_t err_code;

    IPV6_MUTEX_LOCK();

    IPV6_ENTRY();

    err_code = ble_6lowpan_interface_send(p_interface,
                                          p_packet->p_payload,
                                          p_packet->length);

    if (err_code != NRF_SUCCESS)
    {
        IPV6_ERR("Cannot send packet!");
    }

    // Free pbuffer, without freeing memory.
    UNUSED_VARIABLE(iot_pbuffer_free(p_packet, false));

    IPV6_EXIT();

    IPV6_MUTEX_UNLOCK();

    return err_code;
}

uint32_t coap_message_create(coap_message_t * p_message, coap_message_conf_t * p_init_config)
{
    NULL_PARAM_CHECK(p_message);
    NULL_PARAM_CHECK(p_init_config);
    
    // Setting default value for version.
    p_message->header.version = COAP_VERSION;
    
    // Copy values from the init config.
    p_message->header.type       = p_init_config->type;
    p_message->header.token_len  = p_init_config->token_len;
    p_message->header.code       = p_init_config->code;
    p_message->header.id         = p_init_config->id;
    p_message->response_callback = p_init_config->response_callback;
    p_message->p_arg             = NULL;
    
    if (p_init_config->port.port_number == 0)
    {
        return (NRF_ERROR_INVALID_PARAM | IOT_COAP_ERR_BASE);
    }
    memcpy(&p_message->port, &p_init_config->port, sizeof(coap_port_t));
    
    memcpy(p_message->token, p_init_config->token, sizeof(p_init_config->token));
    return NRF_SUCCESS;
}

uint32_t coap_opt_uint_encode(uint8_t * p_encoded, uint16_t * p_length, uint32_t data)
{
    NULL_PARAM_CHECK(p_encoded);
    NULL_PARAM_CHECK(p_length);
    
    uint16_t byte_index = 0;
    
    if (data <= UINT8_MAX)
    {
        if (*p_length < sizeof(uint8_t))
        {
            return (NRF_ERROR_DATA_SIZE | IOT_COAP_ERR_BASE);
        }
        
        p_encoded[byte_index++] = (uint8_t)data;
    }
    else if (data <= UINT16_MAX)
    {
        if (*p_length < sizeof(uint16_t))
        {
            return (NRF_ERROR_DATA_SIZE | IOT_COAP_ERR_BASE);
        }
        
        p_encoded[byte_index++] = (uint8_t)((data & 0xFF00) >> 8);
        p_encoded[byte_index++] = (uint8_t)(data & 0x00FF);
    }
    else
    {
        if (*p_length < sizeof(uint32_t))

uint32_t ipv6_address_find_best_match(iot_interface_t   ** pp_interface,
                                      ipv6_addr_t       *  p_addr_r,
                                      const ipv6_addr_t *  p_addr_f)
{
    VERIFY_MODULE_IS_INITIALIZED();

    NULL_PARAM_CHECK(p_addr_f);
    NULL_PARAM_CHECK(pp_interface);

    uint32_t      index;
    uint32_t      err_code;
    uint32_t      addr_index;
    uint32_t      match_temp  = 0;
    uint32_t      match_best  = 0;
    ipv6_addr_t * p_best_addr = NULL;

    IPV6_MUTEX_LOCK();

    err_code = interface_find(pp_interface, p_addr_f);

    if (err_code == NRF_SUCCESS && p_addr_r)
    {
        uint32_t interface_id = (uint32_t)(*pp_interface)->p_upper_stack;

        for (index = 0; index < IPV6_MAX_ADDRESS_PER_INTERFACE; index++)
        {
            addr_index = m_interfaces[interface_id].addr_range[index];

            if (addr_index != IPV6_INVALID_ADDR_INDEX)
            {
                if (m_address_table[addr_index].state == IPV6_ADDR_STATE_PREFERRED)
                {
                    match_temp = addr_bit_equal(p_addr_f, &m_address_table[addr_index].addr);

                    if (match_temp >= match_best)
                    {
                        match_best  = match_temp;
                        p_best_addr = &m_address_table[addr_index].addr;
                    }
                }
            }
        }

        // No address found.
        if (p_best_addr == NULL)
        {
            // Set undefined :: address.
            IPV6_ADDRESS_INITIALIZE(p_addr_r);
        }
        else
        {
            memcpy(p_addr_r->u8, p_best_addr->u8, IPV6_ADDR_SIZE);
        }
    }

    IPV6_MUTEX_UNLOCK();

    return err_code;
}

uint32_t ipv6_init(const ipv6_init_t * p_init)
{
    uint32_t           index;
    uint32_t           err_code;
    ble_6lowpan_init_t init_params;

    NULL_PARAM_CHECK(p_init);
    NULL_PARAM_CHECK(p_init->p_eui64);
    NULL_PARAM_CHECK(p_init->event_handler);

    SDK_MUTEX_INIT(m_ipv6_mutex);
    IPV6_MUTEX_LOCK();

    IPV6_ENTRY();

    // Initialize related modules.
    UNUSED_VARIABLE(nrf_mem_init());
    UNUSED_VARIABLE(iot_pbuffer_init());

    // Initialize submodules of IPv6 stack.
    UNUSED_VARIABLE(udp_init());
    UNUSED_VARIABLE(icmp6_init());

    // Initialize context manager.
    UNUSED_VARIABLE(iot_context_manager_init());

    IPV6_ADDRESS_INITIALIZE(IPV6_ADDR_ANY);

    // Set application event handler.
    m_event_handler = p_init->event_handler;

    // Clear number of interfaces.
    m_interfaces_count = 0;

    // Clear network interfaces.
    for (index = 0; index < IPV6_MAX_INTERFACE; index++)
    {
        interface_reset(&m_interfaces[index]);
    }

    // Clear all addresses.
    for (index = 0; index < IPV6_MAX_ADDRESS_COUNT; index++)
    {
        addr_free(index, false);
    }

    // 6LoWPAN module initialization.
    init_params.p_eui64       = p_init->p_eui64;
    init_params.event_handler = ble_6lowpan_evt_handler;

    err_code = ble_6lowpan_init(&init_params);

    IPV6_EXIT();

    IPV6_MUTEX_UNLOCK();

    return err_code;
}

uint32_t pstorage_register(pstorage_module_param_t * p_module_param,
                           pstorage_handle_t       * p_block_id)
{
    uint16_t page_count;
    uint32_t total_size;

    VERIFY_MODULE_INITIALIZED();
    NULL_PARAM_CHECK(p_module_param);
    NULL_PARAM_CHECK(p_block_id);
    NULL_PARAM_CHECK(p_module_param->cb);
    BLOCK_SIZE_CHECK(p_module_param->block_size);    
    BLOCK_COUNT_CHECK(p_module_param->block_count, p_module_param->block_size);

    // Block size should be a multiple of word size.
    if (!((p_module_param->block_size % sizeof(uint32_t)) == 0))
    {
        return NRF_ERROR_INVALID_PARAM;
    }

    if (m_next_app_instance == PSTORAGE_MAX_APPLICATIONS)
    {
        return NRF_ERROR_NO_MEM;
    }

    p_block_id->module_id = m_next_app_instance;
    p_block_id->block_id  = m_next_page_addr;

    m_app_table[m_next_app_instance].base_id     = p_block_id->block_id;
    m_app_table[m_next_app_instance].cb          = p_module_param->cb;
    m_app_table[m_next_app_instance].block_size  = p_module_param->block_size;
    m_app_table[m_next_app_instance].block_count = p_module_param->block_count;

    // Calculate number of flash pages allocated for the device.
    page_count = 0;
    total_size = p_module_param->block_size * p_module_param->block_count;
    do
    {
        page_count++;
        if (total_size > PSTORAGE_FLASH_PAGE_SIZE)
        {
            total_size -= PSTORAGE_FLASH_PAGE_SIZE;
        }
        else
        {
            total_size = 0;
        }
        m_next_page_addr += PSTORAGE_FLASH_PAGE_SIZE;
    }
    while (total_size > 0);

    m_app_table[m_next_app_instance].num_of_pages = page_count;
    m_next_app_instance++;

    return NRF_SUCCESS;
}

uint32_t coap_transport_write(const coap_port_t    * p_port,
                              const coap_remote_t  * p_remote,
                              const uint8_t        * p_data,
                              uint16_t               datalen)
{

    err_t err = NRF_ERROR_NOT_FOUND;
    uint32_t index;

    NULL_PARAM_CHECK(p_port);
    NULL_PARAM_CHECK(p_remote);
    NULL_PARAM_CHECK(p_data);

	//Search for the corresponding port.
    for (index = 0; index < COAP_PORT_COUNT; index++)
    {
        if (m_port_table[index].port_number == p_port->port_number)
        {
		    //Allocate Buffer to send the data on port.
            struct pbuf * lwip_buffer = pbuf_alloc(PBUF_TRANSPORT, datalen, PBUF_RAM);

            if (NULL != lwip_buffer)
            {
			    //Make a copy of the data onto the buffer.
                memcpy(lwip_buffer->payload, p_data, datalen);

                COAP_MUTEX_UNLOCK();

				//Send on UDP port.
                err = udp_sendto(m_port_table[index].p_socket,
                                 lwip_buffer,
                                 (ip6_addr_t *)p_remote->addr,
                                 p_remote->port_number);

                COAP_MUTEX_LOCK();


                if (err != ERR_OK)
                {
				    //Free the allocated buffer as send procedure has failed.
                    err = NRF_ERROR_INTERNAL;
                }
                UNUSED_VARIABLE(pbuf_free(lwip_buffer));
            }
            else
            {
			    //Buffer allocation failed, cannot send data.
                err = NRF_ERROR_NO_MEM;
            }
            break;
        }
    }
    return err;
}

uint32_t pstorage_clear(pstorage_handle_t * p_dest, pstorage_size_t size)
{
    uint32_t page_addr;
    uint32_t retval;
    uint16_t page_count;

    VERIFY_MODULE_INITIALIZED();
    NULL_PARAM_CHECK(p_dest);
    MODULE_ID_RANGE_CHECK(p_dest);

    page_addr = p_dest->block_id / BLE_FLASH_PAGE_SIZE;

    retval = NRF_SUCCESS;

    for (page_count = 0; page_count < m_app_table[p_dest->module_id].no_of_pages; page_count++)
    {
        retval = ble_flash_page_erase(page_addr);
        page_addr++;
        if (retval != NRF_SUCCESS)
        {
            break;
        }
    }
    app_notify(p_dest, NULL, PSTORAGE_CLEAR_OP_CODE, size, retval);
    return retval;
}

ssize_t recv(int sock, void * p_buf, size_t buf_size, int flags)
{
    VERIFY_MODULE_IS_INITIALIZED();
    VERIFY_SOCKET_ID(sock);
    NULL_PARAM_CHECK(p_buf);

    SOCKET_MUTEX_LOCK();
    socket_entry_t * p_socket_entry = &m_socket_table[sock];
    SOCKET_MUTEX_UNLOCK();

    ssize_t ret = -1;
    if (p_socket_entry->state == STATE_CONNECTED)
    {
        uint32_t recv_size = 0;
        uint32_t err_code = socket_recv(&p_socket_entry->handle,
                                        p_buf,
                                        buf_size,
                                        &recv_size,
                                        flags);
        if (err_code == NRF_SUCCESS)
        {
            ret = (ssize_t) recv_size;
        }
        socket_set_errno(err_code);
    }
    else
    {
        set_errno(ENOTCONN);
    }
    return ret;
}

int
connect(int sock, const void * p_addr, socklen_t addrlen)
{
    VERIFY_MODULE_IS_INITIALIZED();
    VERIFY_SOCKET_ID(sock);
    NULL_PARAM_CHECK(p_addr);

    SOCKET_MUTEX_LOCK();
    socket_entry_t * p_socket_entry = &m_socket_table[sock];
    SOCKET_MUTEX_UNLOCK();

    int ret = -1;
    if (p_socket_entry->state == STATE_OPEN)
    {
        uint32_t err_code = p_socket_entry->handler->connect_handler(&p_socket_entry->handle,
                                                                     p_addr,
                                                                     addrlen);
        if (err_code == NRF_SUCCESS)
        {
            p_socket_entry->state = STATE_CONNECTED;
            ret = 0;
        }
        socket_set_errno(err_code);
    }
    else if (p_socket_entry->state == STATE_CONNECTED)
    {
        set_errno(EISCONN);
    }
    else if (p_socket_entry->state == STATE_CLOSED)
    {
        set_errno(EBADF);
    }
    return ret;
}

uint32_t coap_queue_add(coap_queue_item_t * item)
{
    NULL_PARAM_CHECK(item);
    
    if (m_message_queue_count >= COAP_MESSAGE_QUEUE_SIZE)
    {
       return (NRF_ERROR_NO_MEM | IOT_COAP_ERR_BASE);
    }
    else 
    {
        for (uint8_t i = 0; i < COAP_MESSAGE_QUEUE_SIZE; i++)
        {
            if (m_queue[i].p_buffer == NULL)
            {
                // Free spot in message queue. Add message here...                
                memcpy(&m_queue[i], item, sizeof(coap_queue_item_t));
                
                m_message_queue_count++;

                return NRF_SUCCESS;
            }
        }
        
    }
    return (NRF_ERROR_DATA_SIZE | IOT_COAP_ERR_BASE);
}

uint32_t pstorage_clear(pstorage_handle_t * p_dest, pstorage_size_t size)
{
    uint32_t retval;

    VERIFY_MODULE_INITIALIZED();
    NULL_PARAM_CHECK(p_dest);
    MODULE_ID_RANGE_CHECK(p_dest);
    BLOCK_ID_RANGE_CHECK(p_dest);

    if ((!is_word_aligned((uint32_t *)p_dest->block_id)))
    {
        return NRF_ERROR_INVALID_ADDR;
    }

    if (
        !(
            ((p_dest->block_id - m_app_table[p_dest->module_id].base_id) %
             m_app_table[p_dest->module_id].block_size) == 0
            )
        )
    {
        return NRF_ERROR_INVALID_PARAM;
    }

    retval = cmd_queue_enqueue(PSTORAGE_CLEAR_OP_CODE, p_dest, NULL, size, 0);

    return retval;
}

uint32_t iot_context_manager_table_free(const iot_interface_t * p_interface)
{
    VERIFY_MODULE_IS_INITIALIZED();
    NULL_PARAM_CHECK(p_interface);

    uint32_t err_code = NRF_SUCCESS;

    CM_ENTRY();

    SDK_MUTEX_INIT(m_iot_context_manager_mutex);

    CM_MUTEX_LOCK();

    const uint32_t table_id = context_table_find(p_interface);

    if (table_id != IOT_CONTEXT_MANAGER_MAX_TABLES)
    {
        // Clear context table.
        CM_TRC("Found context table assigned to interface.");
        context_table_init(table_id);
    }
    else
    {
        // No free context table found.
        CM_ERR("No context table found.");
        err_code = (NRF_ERROR_NOT_FOUND | IOT_CONTEXT_MANAGER_ERR_BASE);
    }

    CM_MUTEX_UNLOCK();

    CM_EXIT();

    return err_code;
}

uint32_t iot_context_manager_table_alloc(const iot_interface_t * p_interface)
{
    VERIFY_MODULE_IS_INITIALIZED();
    NULL_PARAM_CHECK(p_interface);

    uint32_t err_code = NRF_SUCCESS;

    CM_ENTRY();

    CM_MUTEX_LOCK();

    const uint32_t table_id = context_table_find(NULL);

    if (table_id != IOT_CONTEXT_MANAGER_MAX_TABLES)
    {
        // Found a free context table and assign to it.
        CM_TRC("Assigned new context table.");
        m_context_table[table_id].p_interface = (iot_interface_t *)p_interface;
    }
    else
    {
        // No free context table found.
        CM_ERR("No context table found.");
        err_code = (NRF_ERROR_NO_MEM | IOT_CONTEXT_MANAGER_ERR_BASE);
    }

    CM_MUTEX_UNLOCK();

    CM_EXIT();

    return err_code;
}

uint32_t lwm2m_coap_handler_object_delete(lwm2m_object_prototype_t * p_object)
{
    LWM2M_TRC("[LWM2M][CoAP      ]: lwm2m_coap_handler_object_delete\r\n");

    NULL_PARAM_CHECK(p_object);
    
    LWM2M_MUTEX_LOCK();
    
    for (int i = 0; i < m_num_objects; ++i)
    {
        if ( m_objects[i]->object_id == p_object->object_id)
        {
            // Move current last entry into this index position, and trim down the length.
            // If this is the last element, it cannot be accessed because the m_num_objects
            // count is 0.
            m_objects[i] = m_objects[m_num_objects - 1];
            --m_num_objects;
            
            LWM2M_MUTEX_UNLOCK();
            
            return NRF_SUCCESS;
        }
    }
    
    LWM2M_MUTEX_UNLOCK();

    return NRF_ERROR_NOT_FOUND;
}

uint32_t ipv6_address_remove(const iot_interface_t * p_interface,
                             const ipv6_addr_t     * p_addr)
{
    VERIFY_MODULE_IS_INITIALIZED();

    NULL_PARAM_CHECK(p_addr);
    NULL_PARAM_CHECK(p_interface);

    uint32_t    index;
    uint32_t    err_code;
    uint32_t    addr_index;

    IPV6_MUTEX_LOCK();

    IPV6_ENTRY();

    uint32_t interface_id = (uint32_t)p_interface->p_upper_stack;

    err_code = (IOT_IPV6_ERR_BASE | NRF_ERROR_NOT_FOUND);

    for (index = 0; index < IPV6_MAX_ADDRESS_PER_INTERFACE; index++)
    {
        addr_index = m_interfaces[interface_id].addr_range[index];

        if (addr_index != IPV6_INVALID_ADDR_INDEX)
        {
            if (0 == IPV6_ADDRESS_CMP(&m_address_table[addr_index].addr, p_addr))
            {
                m_interfaces[interface_id].addr_range[index] = IPV6_INVALID_ADDR_INDEX;

                // Remove address if no reference to interface found.
                addr_free(index, true);

                err_code = NRF_SUCCESS;

                break;
            }
        }
    }

    IPV6_EXIT();

    IPV6_MUTEX_UNLOCK();

    return err_code;
}