Initial commit

.gitignore

@@ -0,0 +1,53 @@
+# Archives
+*.rar
+*.zip
+*.7z
+*.tar
+*.tar.gz
+*.tgz
+
+# Claude AI
+.claude/
+docs/
+
+# Compiled binaries
+*.o
+*.hex
+*.bin
+*.elf
+*.map
+*.lst
+
+# Build directories
+_build/
+build/
+output/
+
+# IDE/Editor
+.vscode/
+*.swp
+*.swo
+*~
+*.uvguix.*
+JLinkLog.txt
+*.crf
+*.d
+*.htm
+*.lnp
+*.dep
+*.axf
+
+# Segger/J-Link
+*.jlink
+
+# nRF specific
+sdk_config.h.bak
+
+# Windows
+Thumbs.db
+desktop.ini
+.DS_Store
+
+# Temporary files
+*.tmp
+*.bak

README.md

@@ -0,0 +1,100 @@
+# Medithings VesiScan BASIC
+
+nRF52840 기반 초음파(Piezo) 방광 모니터링 패치 펌웨어
+
+## Version
+VBTFW0111
+VesiScan-Basic Tester Ver1.2.2
+
+## Overview
+
+VesiScan BASIC은 피에조 초음파 트랜스듀서와 IMU 센서를 이용하여 방광 상태를 측정하고, BLE를 통해 모바일 클라이언트로 데이터를 전송하는 웨어러블 패치 디바이스 펌웨어입니다.
+
+## Hardware
+
+| 구분 | 칩/센서 | 사양 | 인터페이스 |
+|------|--------|------|-----------|
+| MCU | nRF52840 | ARM Cortex-M4, SoftDevice S140 | - |
+| Piezo TX | MD1822K6-G + TC7920K6-G | MOSFET Driver, +/-20V | GPIO (Timer+PPI) |
+| Piezo MUX | 8ch MUX (MUXA/MUXB) | 채널 선택 (CH0~CH7) | GPIO |
+| Echo AFE | ADA2200 | Lock-in Amplifier | SPI |
+| Echo ADC | ADC121S051 | 12-bit, Echo 신호 샘플링 | SPI |
+| DAC | MCP4725 | 12-bit, 바이어스 전압 | I2C (SW) |
+| DigiPot | AD5272 | 가변저항, AGC 제어 | I2C (SW) |
+| IMU | ICM42670P | 6축 가속도/자이로 | I2C (HW) |
+
+### Piezo 주파수 옵션
+
+| freq | 주파수 |
+|------|--------|
+| 0 | 1.8 MHz |
+| 1 (기본) | 2.1 MHz |
+| 2 | 2.0 MHz |
+| 3 | 1.7 MHz |
+| 4 | 2.2 MHz |
+
+### 8채널 MUX 매핑
+
+| Channel | MUX | SEL1 | SEL0 | EN_MUXA | EN_MUXB |
+|---------|-----|------|------|---------|---------|
+| CH0 | A0 | 0 | 0 | 1 | 0 |
+| CH1 | A2 | 0 | 1 | 1 | 0 |
+| CH2 | A1 | 1 | 0 | 1 | 0 |
+| CH3 | A3 | 1 | 1 | 1 | 0 |
+| CH4 | B0 | 1 | 1 | 0 | 1 |
+| CH5 | B1 | 0 | 1 | 0 | 1 |
+
+### Echo 수신 체인
+
+```
+Piezo → MUX(8ch) → ADA2200(AFE) → ADC121S051 → nRF52840
+                                      ↑
+                               MCP4725(Bias) + AD5272(AGC)
+```
+
+### IMU 사양
+
+| 항목 | 설정 |
+|------|------|
+| 센서 | ICM42670P (6축: 3축 가속도 + 3축 자이로) |
+| 인터페이스 | I2C (HW) |
+| 모드 | Low-Noise Mode |
+| 해상도 | 16-bit |
+
+## Build
+
+- **IDE**: Keil uVision5 (ARM5)
+- **SDK**: nRF5 SDK + SoftDevice S140
+
+## Project Structure
+
+```text
+project/
+|-- ble_peripheral/
+|   `-- ble_app_bladder_patch/
+|       |-- main.c / main.h                  # Application entry, BLE init, main loop
+|       |-- command/                         # Command parser, command table, handlers
+|       |   `-- handlers/                    # Device / sensor / piezo command handlers
+|       |-- measurement/                     # Sensor and acquisition modules
+|       |   |-- piezo/                       # Piezo TX driver and mux channel control
+|       |   |-- adc121s051/                  # Echo ADC sampling / acquisition
+|       |   |-- imu/                         # ICM42670P support and motion libraries
+|       |   |-- battery/                     # Battery voltage measurement
+|       |   `-- temperature/                 # TMP235-Q1 temperature sensing
+|       |-- hal/
+|       |   |-- i2c/                         # Shared I2C manager
+|       |   `-- fds/                         # Flash data storage helpers
+|       |-- system/
+|       |   |-- power/                       # Power control
+|       |   |-- led/                         # LED control
+|       |   |-- security/                    # BLE security / bonding
+|       |   `-- main_timer.c                 # Periodic timing / scheduler logic
+|       |-- pca10056/s140/arm5_no_packs/     # Keil project files and local build output
+|       `-- download/                        # Flashing / DFU scripts and generated images
+`-- dfu/
+    `-- secure_bootloader/                   # BLE DFU bootloader project
+```
+
+## License
+
+Proprietary - Medithings Co., Ltd.

components/ble/ble_advertising/ble_advertising.c

@@ -0,0 +1,813 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include  "sdk_common.h"
+#if NRF_MODULE_ENABLED(BLE_ADVERTISING)
+#include "ble_advdata.h"
+#include "ble_advertising.h"
+#include "nrf_soc.h"
+#include "nrf_log.h"
+#include "sdk_errors.h"
+#include "nrf_sdh_ble.h"
+
+#define BLE_ADV_MODES (5) /**< Total number of possible advertising modes. */
+
+
+/**@brief Function for checking if the whitelist is in use.
+ *
+ * @param[in] p_advertising Advertising module instance.
+ */
+static bool whitelist_has_entries(ble_advertising_t * const p_advertising)
+{
+    return p_advertising->whitelist_in_use;
+}
+
+
+/**@brief Function for checking if an address is valid.
+ *
+ * @param[in] p_addr Pointer to a bluetooth address.
+ */
+static bool addr_is_valid(uint8_t const * const p_addr)
+{
+    for (uint32_t i = 0; i < BLE_GAP_ADDR_LEN; i++)
+    {
+        if (p_addr[i] != 0)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+/**@brief Function for checking the next advertising mode.
+ *
+ * @param[in] adv_mode Current advertising mode.
+ */
+static ble_adv_mode_t adv_mode_next_get(ble_adv_mode_t adv_mode)
+{
+    return (ble_adv_mode_t)((adv_mode + 1) % BLE_ADV_MODES);
+}
+
+
+/**@brief Function for handling the Connected event.
+ *
+ * @param[in] p_ble_evt Event received from the BLE stack.
+ */
+static void on_connected(ble_advertising_t * const p_advertising, ble_evt_t const * p_ble_evt)
+{
+    if (p_ble_evt->evt.gap_evt.params.connected.role == BLE_GAP_ROLE_PERIPH)
+    {
+        p_advertising->current_slave_link_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+    }
+}
+
+
+/**@brief Function for handling the Disconnected event.
+ *
+ * @param[in] p_advertising Advertising module instance.
+ * @param[in] p_ble_evt Event received from the BLE stack.
+ */
+static void on_disconnected(ble_advertising_t * const p_advertising, ble_evt_t const * p_ble_evt)
+{
+    uint32_t ret;
+
+    p_advertising->whitelist_temporarily_disabled = false;
+
+    if (p_ble_evt->evt.gap_evt.conn_handle == p_advertising->current_slave_link_conn_handle &&
+        p_advertising->adv_modes_config.ble_adv_on_disconnect_disabled == false)
+    {
+       ret = ble_advertising_start(p_advertising, BLE_ADV_MODE_DIRECTED_HIGH_DUTY);
+       if ((ret != NRF_SUCCESS) && (p_advertising->error_handler != NULL))
+       {
+           p_advertising->error_handler(ret);
+       }
+    }
+}
+
+
+/**@brief Function for handling the Timeout event.
+ *
+ * @param[in] p_advertising Advertising module instance.
+ * @param[in] p_ble_evt Event received from the BLE stack.
+ */
+static void on_terminated(ble_advertising_t * const p_advertising, ble_evt_t const * p_ble_evt)
+{
+    ret_code_t ret;
+
+    if (p_ble_evt->header.evt_id != BLE_GAP_EVT_ADV_SET_TERMINATED)
+    {
+        // Nothing to do.
+        return;
+    }
+
+    if (  p_ble_evt->evt.gap_evt.params.adv_set_terminated.reason == BLE_GAP_EVT_ADV_SET_TERMINATED_REASON_TIMEOUT
+        ||p_ble_evt->evt.gap_evt.params.adv_set_terminated.reason == BLE_GAP_EVT_ADV_SET_TERMINATED_REASON_LIMIT_REACHED)
+    {
+        // Start advertising in the next mode.
+        ret = ble_advertising_start(p_advertising, adv_mode_next_get(p_advertising->adv_mode_current));
+
+        if ((ret != NRF_SUCCESS) && (p_advertising->error_handler != NULL))
+        {
+            p_advertising->error_handler(ret);
+        }
+    }
+}
+
+
+/**@brief Get the next available advertising mode.
+ *
+ * @param[in] p_advertising Advertising module instance.
+ * @param[in] adv_mode Requested advertising mode.
+ *
+ * @returns adv_mode if possible, or the best available mode if not.
+ */
+static ble_adv_mode_t adv_mode_next_avail_get(ble_advertising_t * const p_advertising,
+                                              ble_adv_mode_t            adv_mode)
+{
+    bool peer_addr_is_valid = addr_is_valid(p_advertising->peer_address.addr);
+
+    // If a mode is disabled, continue to the next mode.
+
+    switch (adv_mode)
+    {
+        case BLE_ADV_MODE_DIRECTED_HIGH_DUTY:
+            if (   (p_advertising->adv_modes_config.ble_adv_directed_high_duty_enabled)
+                && (!p_advertising->adv_modes_config.ble_adv_extended_enabled)
+                && (peer_addr_is_valid))
+            {
+                return BLE_ADV_MODE_DIRECTED_HIGH_DUTY;
+            }
+            // Fallthrough.
+
+        case BLE_ADV_MODE_DIRECTED:
+            if ((p_advertising->adv_modes_config.ble_adv_directed_enabled) && peer_addr_is_valid)
+            {
+                return BLE_ADV_MODE_DIRECTED;
+            }
+            // Fallthrough.
+
+        case BLE_ADV_MODE_FAST:
+            if (p_advertising->adv_modes_config.ble_adv_fast_enabled)
+            {
+                return BLE_ADV_MODE_FAST;
+            }
+            // Fallthrough.
+
+        case BLE_ADV_MODE_SLOW:
+            if (p_advertising->adv_modes_config.ble_adv_slow_enabled)
+            {
+                return BLE_ADV_MODE_SLOW;
+            }
+            // Fallthrough.
+
+        default:
+            return BLE_ADV_MODE_IDLE;
+    }
+}
+
+
+/**@brief Function for starting high duty directed advertising.
+ *
+ * @param[in]  p_advertising Advertising instance.
+ * @param[out] p_adv_params Advertising parameters.
+ *
+ * @return NRF_SUCCESS
+ */
+static ret_code_t set_adv_mode_directed_high_duty(ble_advertising_t * const p_advertising,
+                                                  ble_gap_adv_params_t    * p_adv_params)
+{
+    p_advertising->adv_evt    = BLE_ADV_EVT_DIRECTED_HIGH_DUTY;
+    p_advertising->p_adv_data = NULL;
+
+    p_adv_params->p_peer_addr     = &(p_advertising->peer_address);
+    p_adv_params->interval        = 0;
+    p_adv_params->properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_NONSCANNABLE_DIRECTED_HIGH_DUTY_CYCLE;
+    p_adv_params->duration        = BLE_GAP_ADV_TIMEOUT_HIGH_DUTY_MAX;
+
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function for starting directed slow advertising.
+ *
+ * @param[in]  p_advertising Advertising module instance.
+ * @param[out] p_adv_params Advertising parameters.
+ *
+ * @return NRF_SUCCESS
+ */
+static ret_code_t set_adv_mode_directed(ble_advertising_t * const p_advertising,
+                                        ble_gap_adv_params_t    * p_adv_params)
+{
+    p_advertising->adv_evt = BLE_ADV_EVT_DIRECTED;
+#if !defined (S112) && !defined(S312) && !defined(S113)
+    if (p_advertising->adv_modes_config.ble_adv_extended_enabled)
+    {
+        p_adv_params->properties.type = BLE_GAP_ADV_TYPE_EXTENDED_CONNECTABLE_NONSCANNABLE_DIRECTED;
+    }
+    else
+    {
+#endif // !defined (S112) && !defined(S312)
+        p_adv_params->properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_NONSCANNABLE_DIRECTED;
+#if !defined (S112) && !defined(S312) && !defined(S113)
+    }
+#endif // !defined (S112) && !defined(S312) && !defined(S113)
+    p_adv_params->duration = p_advertising->adv_modes_config.ble_adv_directed_timeout;
+
+    p_advertising->p_adv_data = NULL;
+
+    p_adv_params->p_peer_addr = &p_advertising->peer_address;
+    p_adv_params->interval    = p_advertising->adv_modes_config.ble_adv_directed_interval;
+
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function for indicating whether to use whitelist for advertising.
+ *
+ * @param[in]  p_advertising Advertising module instance.
+ *
+ * @return Whether to use whitelist.
+ */
+static bool use_whitelist(ble_advertising_t * const p_advertising)
+{
+    return((p_advertising->adv_modes_config.ble_adv_whitelist_enabled) &&
+           (!p_advertising->whitelist_temporarily_disabled) &&
+           (whitelist_has_entries(p_advertising)));
+}
+
+
+/**@brief Function for setting new advertising flags in the advertising parameters.
+ *
+ * @param[in]  p_advertising Advertising module instance.
+ * @param[in]  flags         New flags.
+ *
+ * @return Any error from @ref sd_ble_gap_adv_set_configure.
+ */
+static ret_code_t flags_set(ble_advertising_t * const p_advertising, uint8_t flags)
+{
+    uint8_t * p_flags = ble_advdata_parse(p_advertising->adv_data.adv_data.p_data,
+                                          p_advertising->adv_data.adv_data.len,
+                                          BLE_GAP_AD_TYPE_FLAGS);
+
+    if (p_flags != NULL)
+    {
+        *p_flags = flags;
+    }
+
+    return sd_ble_gap_adv_set_configure(&p_advertising->adv_handle, &p_advertising->adv_data, &p_advertising->adv_params);
+}
+
+
+/**@brief Function for starting fast advertising.
+ *
+ * @param[in]  p_advertising Advertising module instance.
+ * @param[out] p_adv_params Advertising parameters.
+ *
+ * @return NRF_SUCCESS or an error from @ref flags_set().
+ */
+static ret_code_t set_adv_mode_fast(ble_advertising_t * const p_advertising,
+                                    ble_gap_adv_params_t    * p_adv_params)
+{
+    ret_code_t ret;
+
+    p_adv_params->interval = p_advertising->adv_modes_config.ble_adv_fast_interval;
+    p_adv_params->duration = p_advertising->adv_modes_config.ble_adv_fast_timeout;
+
+#if !defined (S112) && !defined(S312) && !defined(S113)
+    if (p_advertising->adv_modes_config.ble_adv_extended_enabled)
+    {
+        p_advertising->adv_params.properties.type = BLE_GAP_ADV_TYPE_EXTENDED_CONNECTABLE_NONSCANNABLE_UNDIRECTED;
+    }
+    else
+    {
+#endif // !defined (S112) && !defined(S312) && !defined(S113)
+        p_advertising->adv_params.properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
+#if !defined (S112) && !defined(S312) && !defined(S113)
+    }
+#endif // !defined (S112) && !defined(S312) && !defined(S113)
+ 
+    if (use_whitelist(p_advertising))
+    {
+        p_adv_params->filter_policy = BLE_GAP_ADV_FP_FILTER_CONNREQ;
+
+        // Set correct flags.
+        ret = flags_set(p_advertising, BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED);
+        VERIFY_SUCCESS(ret);
+
+        p_advertising->adv_evt = BLE_ADV_EVT_FAST_WHITELIST;
+    }
+    else
+    {
+        p_advertising->adv_evt = BLE_ADV_EVT_FAST;
+    }
+    p_advertising->p_adv_data = &(p_advertising->adv_data);
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function for starting slow advertising.
+ *
+ * @param[in]  p_advertising Advertising module instance.
+ * @param[out] p_adv_params Advertising parameters.
+ *
+ * @return NRF_SUCCESS or an error from @ref flags_set().
+ */
+static ret_code_t set_adv_mode_slow(ble_advertising_t * const p_advertising,
+                                    ble_gap_adv_params_t    * p_adv_params)
+{
+    ret_code_t ret;
+
+    p_adv_params->interval = p_advertising->adv_modes_config.ble_adv_slow_interval;
+    p_adv_params->duration = p_advertising->adv_modes_config.ble_adv_slow_timeout;
+
+#if !defined (S112) && !defined(S312) && !defined(S113)
+    if (p_advertising->adv_modes_config.ble_adv_extended_enabled)
+    {
+        p_advertising->adv_params.properties.type = BLE_GAP_ADV_TYPE_EXTENDED_CONNECTABLE_NONSCANNABLE_UNDIRECTED;
+    }
+    else
+    {
+#endif // !defined (S112) && !defined(S312) && !defined(S113)
+        p_advertising->adv_params.properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
+#if !defined (S112) && !defined(S312) && !defined(S113)
+    }
+#endif // !defined (S112) && !defined(S312) && !defined(S113)
+
+    if (use_whitelist(p_advertising))
+    {
+        p_adv_params->filter_policy = BLE_GAP_ADV_FP_FILTER_CONNREQ;
+
+        // Set correct flags.
+        ret = flags_set(p_advertising, BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED);
+        VERIFY_SUCCESS(ret);
+
+        p_advertising->adv_evt = BLE_ADV_EVT_SLOW_WHITELIST;
+    }
+    else
+    {
+        p_advertising->adv_evt = BLE_ADV_EVT_SLOW;
+    }
+    p_advertising->p_adv_data = &(p_advertising->adv_data);
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function for checking if an advertising module configuration is legal.
+ *
+ * @details Advertising module can not be initialized if high duty directed advertising is used
+ *          together with extended advertising.
+ *
+ * @param[in] p_config Pointer to the configuration.
+ *
+ * @return True  If the configuration is valid.
+ * @return False If the configuration is invalid.
+ */
+static bool config_is_valid(ble_adv_modes_config_t const * const p_config)
+{
+    if ((p_config->ble_adv_directed_high_duty_enabled == true) &&
+        (p_config->ble_adv_extended_enabled == true))
+    {
+        return false;
+    }
+#if !defined (S140)
+    else if ( p_config->ble_adv_primary_phy == BLE_GAP_PHY_CODED ||
+              p_config->ble_adv_secondary_phy == BLE_GAP_PHY_CODED)
+    {
+        return false;
+    }
+#endif // !defined (S140)
+    else
+    {
+        return true;
+    }
+}
+
+
+/**@brief Function for getting the maximum size of the advertising data buffer.
+ *
+ * @param[in] p_advertising Advertising module instance.
+ *
+ * @returns The maximum size of the advertising data buffer.
+ */
+static uint16_t adv_set_data_size_max_get(ble_advertising_t const * const p_advertising)
+{
+    uint16_t adv_set_data_size_max;
+
+    if (p_advertising->adv_modes_config.ble_adv_extended_enabled == true)
+    {
+#ifdef BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED
+        adv_set_data_size_max = BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED;
+#else
+        adv_set_data_size_max = BLE_GAP_ADV_SET_DATA_SIZE_MAX;
+#endif // BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED
+    }
+    else
+    {
+        adv_set_data_size_max = BLE_GAP_ADV_SET_DATA_SIZE_MAX;
+    }
+
+    return adv_set_data_size_max;
+}
+
+
+void ble_advertising_conn_cfg_tag_set(ble_advertising_t * const p_advertising,
+                                      uint8_t                   ble_cfg_tag)
+{
+    p_advertising->conn_cfg_tag = ble_cfg_tag;
+}
+
+
+uint32_t ble_advertising_init(ble_advertising_t            * const p_advertising,
+                              ble_advertising_init_t const * const p_init)
+{
+    uint32_t ret;
+    if ((p_init == NULL) || (p_advertising == NULL))
+    {
+        return NRF_ERROR_NULL;
+    }
+    if (!config_is_valid(&p_init->config))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    p_advertising->adv_mode_current               = BLE_ADV_MODE_IDLE;
+    p_advertising->adv_modes_config               = p_init->config;
+    p_advertising->conn_cfg_tag                   = BLE_CONN_CFG_TAG_DEFAULT;
+    p_advertising->evt_handler                    = p_init->evt_handler;
+    p_advertising->error_handler                  = p_init->error_handler;
+    p_advertising->current_slave_link_conn_handle = BLE_CONN_HANDLE_INVALID;
+    p_advertising->p_adv_data                     = &p_advertising->adv_data;
+
+    memset(&p_advertising->peer_address, 0, sizeof(p_advertising->peer_address));
+
+    // Copy advertising data.
+    if (!p_advertising->initialized)
+    {
+        p_advertising->adv_handle = BLE_GAP_ADV_SET_HANDLE_NOT_SET;
+    }
+    p_advertising->adv_data.adv_data.p_data = p_advertising->enc_advdata[0];
+    p_advertising->adv_data.adv_data.len = adv_set_data_size_max_get(p_advertising);
+
+    ret = ble_advdata_encode(&p_init->advdata, p_advertising->enc_advdata[0], &p_advertising->adv_data.adv_data.len);
+    VERIFY_SUCCESS(ret);
+
+    p_advertising->adv_data.scan_rsp_data.p_data = p_advertising->enc_scan_rsp_data[0];
+    p_advertising->adv_data.scan_rsp_data.len = adv_set_data_size_max_get(p_advertising);
+
+    ret = ble_advdata_encode(&p_init->srdata,
+                              p_advertising->adv_data.scan_rsp_data.p_data,
+                             &p_advertising->adv_data.scan_rsp_data.len);
+    VERIFY_SUCCESS(ret);
+
+    // Configure a initial advertising configuration. The advertising data and and advertising
+    // parameters will be changed later when we call @ref ble_advertising_start, but must be set
+    // to legal values here to define an advertising handle.
+    p_advertising->adv_params.primary_phy     = BLE_GAP_PHY_1MBPS;
+    p_advertising->adv_params.duration        = p_advertising->adv_modes_config.ble_adv_fast_timeout;
+    p_advertising->adv_params.properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
+    p_advertising->adv_params.p_peer_addr     = NULL;
+    p_advertising->adv_params.filter_policy   = BLE_GAP_ADV_FP_ANY;
+    p_advertising->adv_params.interval        = p_advertising->adv_modes_config.ble_adv_fast_interval;
+
+    ret = sd_ble_gap_adv_set_configure(&p_advertising->adv_handle, NULL, &p_advertising->adv_params);
+    VERIFY_SUCCESS(ret);
+
+    p_advertising->initialized = true;
+    return ret;
+}
+
+
+/**@brief Function for checking that a phy define value matches one of the valid phys from the SD.
+ *
+ * @param[in]  PHY to be validated.
+ *
+ * @retval true  If the PHY value is valid (1mbit, 2mbit, coded).
+ * @retval false If the PHY value is invalid.
+ */
+static bool phy_is_valid(uint32_t const * const p_phy)
+{
+    if ((*p_phy) == BLE_GAP_PHY_1MBPS ||
+        (*p_phy) == BLE_GAP_PHY_2MBPS
+#if defined (S140)
+     || (*p_phy) == BLE_GAP_PHY_CODED
+#endif // !defined (S140)
+        )
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+
+uint32_t ble_advertising_start(ble_advertising_t * const p_advertising,
+                               ble_adv_mode_t            advertising_mode)
+{
+    uint32_t ret;
+
+    if (p_advertising->initialized == false)
+    {
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    p_advertising->adv_mode_current = advertising_mode;
+
+    memset(&p_advertising->peer_address, 0, sizeof(p_advertising->peer_address));
+
+    if (  ((p_advertising->adv_modes_config.ble_adv_directed_high_duty_enabled) && (p_advertising->adv_mode_current == BLE_ADV_MODE_DIRECTED_HIGH_DUTY))
+        ||((p_advertising->adv_modes_config.ble_adv_directed_enabled)           && (p_advertising->adv_mode_current == BLE_ADV_MODE_DIRECTED_HIGH_DUTY))
+        ||((p_advertising->adv_modes_config.ble_adv_directed_enabled)           && (p_advertising->adv_mode_current == BLE_ADV_MODE_DIRECTED))
+       )
+    {
+        if (p_advertising->evt_handler != NULL)
+        {
+            p_advertising->peer_addr_reply_expected = true;
+            p_advertising->evt_handler(BLE_ADV_EVT_PEER_ADDR_REQUEST);
+        }
+        else
+        {
+            p_advertising->peer_addr_reply_expected = false;
+        }
+    }
+
+    p_advertising->adv_mode_current = adv_mode_next_avail_get(p_advertising, advertising_mode);
+
+    // Fetch the whitelist.
+    if ((p_advertising->evt_handler != NULL) &&
+        (p_advertising->adv_mode_current == BLE_ADV_MODE_FAST || p_advertising->adv_mode_current == BLE_ADV_MODE_SLOW) &&
+        (p_advertising->adv_modes_config.ble_adv_whitelist_enabled) &&
+        (!p_advertising->whitelist_temporarily_disabled))
+    {
+        p_advertising->whitelist_in_use         = false;
+        p_advertising->whitelist_reply_expected = true;
+        p_advertising->evt_handler(BLE_ADV_EVT_WHITELIST_REQUEST);
+    }
+    else
+    {
+        p_advertising->whitelist_reply_expected = false;
+    }
+
+    // Initialize advertising parameters with default values.
+    memset(&p_advertising->adv_params, 0, sizeof(p_advertising->adv_params));
+
+    p_advertising->adv_params.properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
+
+    // Use 1MBIT as primary phy if no phy was selected.
+    if (phy_is_valid(&p_advertising->adv_modes_config.ble_adv_primary_phy))
+    {
+        p_advertising->adv_params.primary_phy = p_advertising->adv_modes_config.ble_adv_primary_phy;
+    }
+    else
+    {
+        p_advertising->adv_params.primary_phy = BLE_GAP_PHY_1MBPS;
+    }
+
+    if (p_advertising->adv_modes_config.ble_adv_extended_enabled)
+    {
+        // Use 1MBIT as secondary phy if no phy was selected.
+        if (phy_is_valid(&p_advertising->adv_modes_config.ble_adv_secondary_phy))
+        {
+            p_advertising->adv_params.secondary_phy = p_advertising->adv_modes_config.ble_adv_secondary_phy;
+        }
+        else
+        {
+            p_advertising->adv_params.secondary_phy = BLE_GAP_PHY_1MBPS;
+        }
+    }
+    p_advertising->adv_params.filter_policy = BLE_GAP_ADV_FP_ANY;
+
+    // Set advertising parameters and events according to selected advertising mode.
+    switch (p_advertising->adv_mode_current)
+    {
+        case BLE_ADV_MODE_DIRECTED_HIGH_DUTY:
+            ret = set_adv_mode_directed_high_duty(p_advertising, &p_advertising->adv_params);
+            break;
+
+        case BLE_ADV_MODE_DIRECTED:
+            ret = set_adv_mode_directed(p_advertising, &p_advertising->adv_params);
+            break;
+
+        case BLE_ADV_MODE_FAST:
+            ret = set_adv_mode_fast(p_advertising, &p_advertising->adv_params);
+            break;
+
+        case BLE_ADV_MODE_SLOW:
+            ret = set_adv_mode_slow(p_advertising, &p_advertising->adv_params);
+            break;
+
+        case BLE_ADV_MODE_IDLE:
+            p_advertising->adv_evt = BLE_ADV_EVT_IDLE;
+            break;
+
+        default:
+            break;
+    }
+
+    if (p_advertising->adv_mode_current != BLE_ADV_MODE_IDLE)
+    {
+
+        ret = sd_ble_gap_adv_set_configure(&p_advertising->adv_handle, p_advertising->p_adv_data, &p_advertising->adv_params);
+        if (ret != NRF_SUCCESS)
+        {
+            return ret;
+        }
+        ret = sd_ble_gap_adv_start(p_advertising->adv_handle, p_advertising->conn_cfg_tag);
+
+        if (ret != NRF_SUCCESS)
+        {
+            return ret;
+        }
+    }
+
+    if (p_advertising->evt_handler != NULL)
+    {
+        p_advertising->evt_handler(p_advertising->adv_evt);
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+void ble_advertising_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    ble_advertising_t * p_advertising = (ble_advertising_t *)p_context;
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            on_connected(p_advertising, p_ble_evt);
+            break;
+
+        // Upon disconnection, whitelist will be activated and direct advertising is started.
+        case BLE_GAP_EVT_DISCONNECTED:
+            on_disconnected(p_advertising, p_ble_evt);
+            break;
+
+        // Upon terminated advertising (time-out), the next advertising mode is started.
+        case BLE_GAP_EVT_ADV_SET_TERMINATED:
+            on_terminated(p_advertising, p_ble_evt);
+            break;
+
+        default:
+            break;
+    }
+}
+
+
+uint32_t ble_advertising_peer_addr_reply(ble_advertising_t * const p_advertising,
+                                         ble_gap_addr_t          * p_peer_address)
+{
+    if (!p_advertising->peer_addr_reply_expected)
+    {
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    p_advertising->peer_addr_reply_expected = false;
+
+    memcpy(&p_advertising->peer_address, p_peer_address, sizeof(p_advertising->peer_address));
+
+    return NRF_SUCCESS;
+}
+
+
+uint32_t ble_advertising_whitelist_reply(ble_advertising_t * const p_advertising,
+                                         ble_gap_addr_t    const * p_gap_addrs,
+                                         uint32_t               addr_cnt,
+                                         ble_gap_irk_t  const * p_gap_irks,
+                                         uint32_t               irk_cnt)
+{
+    if (!p_advertising->whitelist_reply_expected)
+    {
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    p_advertising->whitelist_reply_expected = false;
+    p_advertising->whitelist_in_use         = ((addr_cnt > 0) || (irk_cnt > 0));
+
+    return NRF_SUCCESS;
+}
+
+
+uint32_t ble_advertising_restart_without_whitelist(ble_advertising_t * const p_advertising)
+{
+    ret_code_t ret;
+
+    (void) sd_ble_gap_adv_stop(p_advertising->adv_handle);
+
+    p_advertising->whitelist_temporarily_disabled = true;
+    p_advertising->whitelist_in_use               = false;
+    p_advertising->adv_params.filter_policy       = BLE_GAP_ADV_FP_ANY;
+    // Set correct flags.
+    ret = flags_set(p_advertising, BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
+    VERIFY_SUCCESS(ret);
+
+    ret = ble_advertising_start(p_advertising, p_advertising->adv_mode_current);
+    if ((ret != NRF_SUCCESS) && (p_advertising->error_handler != NULL))
+    {
+        p_advertising->error_handler(ret);
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+void ble_advertising_modes_config_set(ble_advertising_t            * const p_advertising,
+                                      ble_adv_modes_config_t const * const p_adv_modes_config)
+{
+    p_advertising->adv_modes_config = *p_adv_modes_config;
+}
+
+
+ret_code_t ble_advertising_advdata_update(ble_advertising_t   * const p_advertising,
+                                          ble_advdata_t const * const p_advdata,
+                                          ble_advdata_t const * const p_srdata)
+{
+    VERIFY_PARAM_NOT_NULL(p_advertising);
+    if (p_advertising->initialized == false)
+    {
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    if ((p_advdata == NULL) && (p_srdata == NULL))
+    {
+        return NRF_ERROR_NULL;
+    }
+
+    ble_gap_adv_data_t new_adv_data;
+    memset(&new_adv_data, 0, sizeof(new_adv_data));
+
+    if (p_advdata != NULL)
+    {
+        new_adv_data.adv_data.p_data =
+            (p_advertising->p_adv_data->adv_data.p_data != p_advertising->enc_advdata[0]) ?
+             p_advertising->enc_advdata[0] : p_advertising->enc_advdata[1];
+        new_adv_data.adv_data.len = adv_set_data_size_max_get(p_advertising);
+
+        ret_code_t ret = ble_advdata_encode(p_advdata,
+                                            new_adv_data.adv_data.p_data,
+                                            &new_adv_data.adv_data.len);
+        VERIFY_SUCCESS(ret);
+    }
+
+    if (p_srdata != NULL)
+    {
+        new_adv_data.scan_rsp_data.p_data =
+            (p_advertising->p_adv_data->scan_rsp_data.p_data != p_advertising->enc_scan_rsp_data[0]) ?
+             p_advertising->enc_scan_rsp_data[0] : p_advertising->enc_scan_rsp_data[1];
+        new_adv_data.scan_rsp_data.len = adv_set_data_size_max_get(p_advertising);
+
+        ret_code_t ret = ble_advdata_encode(p_srdata,
+                                            new_adv_data.scan_rsp_data.p_data,
+                                            &new_adv_data.scan_rsp_data.len);
+        VERIFY_SUCCESS(ret);
+    }
+
+    memcpy(&p_advertising->adv_data, &new_adv_data, sizeof(p_advertising->adv_data));
+    p_advertising->p_adv_data = &p_advertising->adv_data;
+
+    return sd_ble_gap_adv_set_configure(&p_advertising->adv_handle,
+                                        p_advertising->p_adv_data,
+                                        NULL);
+}
+
+
+#endif // NRF_MODULE_ENABLED(BLE_ADVERTISING)

components/ble/ble_advertising/ble_advertising.h

@@ -0,0 +1,355 @@
+/**
+ * Copyright (c) 2015 - 2021, 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_advertising Advertising Module
+ * @{
+ * @ingroup  ble_sdk_lib
+ * @brief    Module for handling connectable BLE advertising.
+ *
+ * @details  The Advertising Module handles connectable advertising for your application. It can
+ *           be configured with advertising modes to suit most typical use cases.
+ *           Your main application can react to changes in advertising modes
+ *           if an event handler is provided.
+ *
+ * @note     The Advertising Module supports only applications with a single peripheral link.
+ *
+ */
+
+#ifndef BLE_ADVERTISING_H__
+#define BLE_ADVERTISING_H__
+
+#include <stdint.h>
+#include "nrf_error.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "ble_gattc.h"
+#include "ble_advdata.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief   Macro for defining a ble_advertising instance.
+ *
+ * @param   _name   Name of the instance.
+ * @hideinitializer
+ */
+#define BLE_ADVERTISING_DEF(_name)                                                                  \
+static ble_advertising_t _name;                                                                     \
+NRF_SDH_BLE_OBSERVER(_name ## _ble_obs,                                                             \
+                     BLE_ADV_BLE_OBSERVER_PRIO,                                                     \
+                     ble_advertising_on_ble_evt, &_name)
+
+
+/**@brief   Advertising modes. */
+typedef enum
+{
+    BLE_ADV_MODE_IDLE,               /**< Idle; no connectable advertising is ongoing. */
+    BLE_ADV_MODE_DIRECTED_HIGH_DUTY, /**< Directed advertising (high duty cycle) attempts to connect to the most recently disconnected peer. */
+    BLE_ADV_MODE_DIRECTED,           /**< Directed advertising (low duty cycle) attempts to connect to the most recently disconnected peer. */
+    BLE_ADV_MODE_FAST,               /**< Fast advertising will connect to any peer device, or filter with a whitelist if one exists. */
+    BLE_ADV_MODE_SLOW,               /**< Slow advertising is similar to fast advertising. By default, it uses a longer advertising interval and time-out than fast advertising. However, these options are defined by the user. */
+} ble_adv_mode_t;
+
+/**@brief   Advertising events.
+ *
+ * @details These events are propagated to the main application if a handler was provided during
+ *          initialization of the Advertising Module. Events for modes that are not used can be
+ *          ignored. Similarly, BLE_ADV_EVT_WHITELIST_REQUEST and BLE_ADV_EVT_PEER_ADDR_REQUEST
+ *          can be ignored if whitelist and direct advertising is not used.
+ */
+typedef enum
+{
+    BLE_ADV_EVT_IDLE,                /**< Idle; no connectable advertising is ongoing.*/
+    BLE_ADV_EVT_DIRECTED_HIGH_DUTY,  /**< Direct advertising mode has started. */
+    BLE_ADV_EVT_DIRECTED,            /**< Directed advertising (low duty cycle) has started. */
+    BLE_ADV_EVT_FAST,                /**< Fast advertising mode has started. */
+    BLE_ADV_EVT_SLOW,                /**< Slow advertising mode has started. */
+    BLE_ADV_EVT_FAST_WHITELIST,      /**< Fast advertising mode using the whitelist has started. */
+    BLE_ADV_EVT_SLOW_WHITELIST,      /**< Slow advertising mode using the whitelist has started. */
+    BLE_ADV_EVT_WHITELIST_REQUEST,   /**< Request a whitelist from the main application. For whitelist advertising to work, the whitelist must be set when this event occurs. */
+    BLE_ADV_EVT_PEER_ADDR_REQUEST    /**< Request a peer address from the main application. For directed advertising to work, the peer address must be set when this event occurs. */
+} ble_adv_evt_t;
+
+/**@brief   Options for the different advertisement modes.
+ *
+ * @details This structure is used to enable or disable advertising modes and to configure time-out
+ *          periods and advertising intervals.
+ */
+typedef struct
+{
+    bool     ble_adv_on_disconnect_disabled;     /**< Enable or disable automatic return to advertising upon disconnecting.*/
+    bool     ble_adv_whitelist_enabled;          /**< Enable or disable use of the whitelist. */
+    bool     ble_adv_directed_high_duty_enabled; /**< Enable or disable high duty direct advertising mode. Can not be used together with extended advertising. */
+    bool     ble_adv_directed_enabled;           /**< Enable or disable direct advertising mode. */
+    bool     ble_adv_fast_enabled;               /**< Enable or disable fast advertising mode. */
+    bool     ble_adv_slow_enabled;               /**< Enable or disable slow advertising mode. */
+    uint32_t ble_adv_directed_interval;          /**< Advertising interval for directed advertising. */
+    uint32_t ble_adv_directed_timeout;           /**< Time-out (number of tries) for direct advertising. */
+    uint32_t ble_adv_fast_interval;              /**< Advertising interval for fast advertising. */
+    uint32_t ble_adv_fast_timeout;               /**< Time-out (in units of 10ms) for fast advertising. */
+    uint32_t ble_adv_slow_interval;              /**< Advertising interval for slow advertising. */
+    uint32_t ble_adv_slow_timeout;               /**< Time-out (in units of 10ms) for slow advertising. */
+    bool     ble_adv_extended_enabled;           /**< Enable or disable extended advertising. */
+    uint32_t ble_adv_secondary_phy;              /**< PHY for the secondary (extended) advertising @ref BLE_GAP_PHYS (BLE_GAP_PHY_1MBPS, BLE_GAP_PHY_2MBPS or BLE_GAP_PHY_CODED). */
+    uint32_t ble_adv_primary_phy;                /**< PHY for the primary advertising. @ref BLE_GAP_PHYS (BLE_GAP_PHY_1MBPS, BLE_GAP_PHY_2MBPS or BLE_GAP_PHY_CODED). */
+} ble_adv_modes_config_t;
+
+/**@brief   BLE advertising event handler type. */
+typedef void (*ble_adv_evt_handler_t) (ble_adv_evt_t const adv_evt);
+
+/**@brief   BLE advertising error handler type. */
+typedef void (*ble_adv_error_handler_t) (uint32_t nrf_error);
+
+typedef struct
+{
+    bool                    initialized;
+    bool                    advertising_start_pending;                        /**< Flag to keep track of ongoing operations in flash. */
+    ble_adv_mode_t          adv_mode_current;                                 /**< Variable to keep track of the current advertising mode. */
+    ble_adv_modes_config_t  adv_modes_config;                                 /**< Struct to keep track of disabled and enabled advertising modes, as well as time-outs and intervals.*/
+    uint8_t                 conn_cfg_tag;                                     /**< Variable to keep track of what connection settings will be used if the advertising results in a connection. */
+
+    ble_adv_evt_t           adv_evt;                                          /**< Advertising event propogated to the main application. The event is either a transaction to a new advertising mode, or a request for whitelist or peer address. */
+    ble_adv_evt_handler_t   evt_handler;                                      /**< Handler for the advertising events. Can be initialized as NULL if no handling is implemented on in the main application. */
+    ble_adv_error_handler_t error_handler;                                    /**< Handler for the advertising error events. */
+
+    ble_gap_adv_params_t    adv_params;                                       /**< GAP advertising parameters. */
+    uint8_t                 adv_handle;                                       /**< Handle for the advertising set. */
+
+#ifdef BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED
+    uint8_t                 enc_advdata[2][BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED];       /**< Advertising data sets in encoded form. Current and swap buffer. */
+    uint8_t                 enc_scan_rsp_data[2][BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED]; /**< Scan response data sets in encoded form. Current and swap buffer. */
+#else
+    uint8_t                 enc_advdata[2][BLE_GAP_ADV_SET_DATA_SIZE_MAX];       /**< Current advertising data in encoded form. */
+    uint8_t                 enc_scan_rsp_data[2][BLE_GAP_ADV_SET_DATA_SIZE_MAX]; /**< Current scan response data in encoded form. */
+#endif // BLE_GAP_ADV_SET_DATA_SIZE_EXTENDED_CONNECTABLE_MAX_SUPPORTED
+
+    ble_gap_adv_data_t      adv_data;                                         /**< Advertising data. */
+    ble_gap_adv_data_t     *p_adv_data;                                       /**< Will be set to point to @ref ble_advertising_t::adv_data for undirected advertising, and will be set to NULL for directed advertising. */
+
+    uint16_t                current_slave_link_conn_handle;                   /**< Connection handle for the active link. */
+    ble_gap_addr_t          peer_address;                                     /**< Address of the most recently connected peer, used for direct advertising. */
+    bool                    peer_addr_reply_expected;                         /**< Flag to verify that peer address is only set when requested. */
+    bool                    whitelist_temporarily_disabled;                   /**< Flag to keep track of temporary disabling of the whitelist. */
+    bool                    whitelist_reply_expected;                         /**< Flag to verify that the whitelist is only set when requested. */
+    bool                    whitelist_in_use;                                 /**< This module needs to be aware of whether or not a whitelist has been set (e.g. using the Peer Manager) in order to start advertising with the proper advertising params (filter policy). */
+} ble_advertising_t;
+
+typedef struct
+{
+    uint32_t interval;
+    uint32_t timeout;
+    bool     enabled;
+} ble_adv_mode_config_t;
+
+/**@brief     Initialization parameters for the Advertising Module.
+ * @details This structure is used to pass advertising options, advertising data,
+ *          and an event handler to the Advertising Module during initialization.
+ */
+typedef struct
+{
+    ble_advdata_t           advdata;       /**< Advertising data: name, appearance, discovery flags, and more. */
+    ble_advdata_t           srdata;        /**< Scan response data: Supplement to advertising data. */
+    ble_adv_modes_config_t  config;        /**< Select which advertising modes and intervals will be utilized.*/
+    ble_adv_evt_handler_t   evt_handler;   /**< Event handler that will be called upon advertising events. */
+    ble_adv_error_handler_t error_handler; /**< Error handler that will propogate internal errors to the main applications. */
+} ble_advertising_init_t;
+
+
+/**@brief   Function for handling BLE events.
+ *
+ * @details This function must be called from the BLE stack event dispatcher for
+ *          the module to handle BLE events that are relevant for the Advertising Module.
+ *
+ * @param[in] p_ble_evt     BLE stack event.
+ * @param[in] p_adv         Advertising Module instance.
+ */
+void ble_advertising_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_adv);
+
+
+/**@brief   Function for initializing the Advertising Module.
+ *
+ * @details Encodes the required advertising data and passes it to the stack.
+ *          Also builds a structure to be passed to the stack when starting advertising.
+ *          Most of the supplied data is copied into a local structure where it is manipulated
+ *          depending on what advertising modes are started in @ref ble_advertising_start.
+ *          The exception is advdata_t::uuids_more_available, advdata_t::uuids_complete, and
+ *          advdata_t::uuids_solicited which are stored as pointers. The main application must
+ *          store these UUIDs.
+ *
+ * @param[out] p_advertising Advertising Module instance. This structure must be supplied by
+ *                           the application. It is initialized by this function and will later
+ *                           be used to identify this particular module instance.
+ * @param[in] p_init         Information needed to initialize the module.
+ *
+ * @retval NRF_SUCCESS             If initialization was successful.
+ * @retval NRF_ERROR_INVALID_PARAM If the advertising configuration in \p p_init is invalid.
+ * @return If functions from other modules return errors to this function, the @ref nrf_error are propagated.
+ */
+uint32_t ble_advertising_init(ble_advertising_t            * const p_advertising,
+                              ble_advertising_init_t const * const p_init);
+
+
+ /**@brief  Function for changing the connection settings tag that will be used for upcoming connections.
+ *
+ * @details See @ref sd_ble_cfg_set for more details about changing connection settings. If this
+ *          function is never called, @ref BLE_CONN_CFG_TAG_DEFAULT will be used.
+ *
+ * @param[in] p_advertising Advertising Module instance.
+ * @param[in] ble_cfg_tag Configuration for the connection settings (see @ref sd_ble_cfg_set).
+ */
+void ble_advertising_conn_cfg_tag_set(ble_advertising_t * const p_advertising, uint8_t ble_cfg_tag);
+
+/**@brief   Function for starting advertising.
+ *
+ * @details You can start advertising in any of the advertising modes that you enabled
+ *          during initialization.
+ *
+ * @param[in] p_advertising    Advertising Module instance.
+ * @param[in] advertising_mode Advertising mode.
+ *
+ * @retval @ref NRF_SUCCESS On success, else an error code indicating reason for failure.
+ * @retval @ref NRF_ERROR_INVALID_STATE If the module is not initialized.
+ */
+uint32_t ble_advertising_start(ble_advertising_t * const p_advertising,
+                               ble_adv_mode_t            advertising_mode);
+
+
+/**@brief   Function for setting the peer address.
+ *
+ * @details The peer address must be set by the application upon receiving a
+ *          @ref BLE_ADV_EVT_PEER_ADDR_REQUEST event. Without the peer address, the directed
+ *          advertising mode will not be run.
+ *
+ * @param[in] p_advertising Advertising Module instance.
+ * @param[in] p_peer_addr   Pointer to a peer address.
+ *
+ * @retval @ref NRF_SUCCESS Successfully stored the peer address pointer in the Advertising Module.
+ * @retval @ref NRF_ERROR_INVALID_STATE If a reply was not expected.
+ */
+uint32_t ble_advertising_peer_addr_reply(ble_advertising_t * const p_advertising,
+                                         ble_gap_addr_t          * p_peer_addr);
+
+
+/**@brief   Function for setting a whitelist.
+ *
+ * @details The whitelist must be set by the application upon receiving a
+ *          @ref BLE_ADV_EVT_WHITELIST_REQUEST event. Without the whitelist, the whitelist
+ *          advertising for fast and slow modes will not be run.
+ *
+ * @param[in] p_advertising Advertising Module instance.
+ * @param[in] p_gap_addrs   The list of GAP addresses to whitelist.
+ * @param[in] addr_cnt      The number of GAP addresses to whitelist.
+ * @param[in] p_gap_irks    The list of peer IRK to whitelist.
+ * @param[in] irk_cnt       The number of peer IRK to whitelist.
+ *
+ * @retval @ref NRF_SUCCESS             If the operation was successful.
+ * @retval @ref NRF_ERROR_INVALID_STATE If a call to this function was made without a
+ *                                      BLE_ADV_EVT_WHITELIST_REQUEST event being received.
+ */
+uint32_t ble_advertising_whitelist_reply(ble_advertising_t * const p_advertising,
+                                         ble_gap_addr_t const    * p_gap_addrs,
+                                         uint32_t                  addr_cnt,
+                                         ble_gap_irk_t const     * p_gap_irks,
+                                         uint32_t                  irk_cnt);
+
+
+/**@brief   Function for disabling whitelist advertising.
+ *
+ * @details This function temporarily disables whitelist advertising.
+ *          Calling this function resets the current time-out countdown.
+ *
+ * @param[in]  p_advertising Advertising Module instance.
+ *
+ * @retval @ref NRF_SUCCESS On success, else an error message propogated from the Softdevice.
+ */
+uint32_t ble_advertising_restart_without_whitelist(ble_advertising_t * const p_advertising);
+
+
+/**@brief   Function for changing advertising modes configuration.
+ *
+ * @details This function can be called if you wish to reconfigure the advertising modes that the
+ *          Advertising Module will cycle through. Enable or disable modes as listed in
+ *          @ref ble_adv_mode_t; or change the duration of the advertising and use of whitelist.
+ *
+ *          Keep in mind that @ref ble_adv_modes_config_t is also supplied when calling
+ *          @ref ble_advertising_init. Calling @ref ble_advertising_modes_config_set
+ *          is only necessary if your application requires this behaviour to change.
+ *
+ * @param[in]  p_advertising      Advertising Module instance.
+ * @param[in]  p_adv_modes_config Struct to keep track of disabled and enabled advertising modes,
+ *                                as well as time-outs and intervals.
+ */
+void ble_advertising_modes_config_set(ble_advertising_t            * const p_advertising,
+                                      ble_adv_modes_config_t const * const p_adv_modes_config);
+
+
+/**@brief   Function for updating advertising data.
+ *
+ * @details This function can be called if you wish to reconfigure the advertising data The update
+ *          will be effective even if advertising has already been started.
+ *
+ * @param[in]  p_advertising Advertising Module instance.
+ * @param[in]  p_advdata     Pointer to the structure for specifying the content of advertising data.
+ *                           Or null if there should be no advertising data.
+ * @param[in]  p_srdata      Pointer to the structure for specifying the content of scan response data.
+ *                           Or null if there should be no advertising data.
+ *
+ * @retval @ref NRF_ERROR_NULL          If advertising instance was null.
+ *                                      If both \p p_advdata and \p p_srdata are null.
+ * @retval @ref NRF_ERROR_INVALID_STATE If advertising instance was not initialized.
+ * @retval @ref NRF_SUCCESS or any error from @ref ble_advdata_encode or
+ *         @ref sd_ble_gap_adv_set_configure().
+ */
+ret_code_t ble_advertising_advdata_update(ble_advertising_t   * const p_advertising,
+                                          ble_advdata_t const * const p_advdata,
+                                          ble_advdata_t const * const p_srdata);
+
+/** @} */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_ADVERTISING_H__
+
+/** @} */

components/ble/ble_db_discovery/ble_db_discovery.h

@@ -0,0 +1,271 @@
+/**
+ * Copyright (c) 2013 - 2021, 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_db_discovery Database Discovery
+ * @{
+ * @ingroup ble_sdk_lib
+ *
+ * @brief Database discovery module.
+ *
+ * @details This module contains the APIs and types exposed by the DB Discovery module. These APIs
+ *          and types can be used by the application to perform discovery of a service and its
+ *          characteristics at the peer server. This module can also be used to discover the
+ *          desired services in multiple remote devices.
+ *
+ * @warning The maximum number of characteristics per service that can be discovered by this module
+ *          is determined by the number of characteristics in the service structure defined in
+ *          db_disc_config.h. If the peer has more than the supported number of characteristics, then
+ *          the first found will be discovered and any further characteristics will be ignored. Only the
+ *          following descriptors will be searched for at the peer: Client Characteristic Configuration,
+ *          Characteristic Extended Properties, Characteristic User Description, and Report Reference.
+ *
+ * @note Presently only one instance of a Primary Service can be discovered by this module. If
+ *       there are multiple instances of the service at the peer, only the first instance
+ *       of it at the peer is fetched and returned to the application.
+ *
+ * @note The application must propagate BLE stack events to this module by calling
+ *       ble_db_discovery_on_ble_evt().
+ *
+ */
+
+#ifndef BLE_DB_DISCOVERY_H__
+#define BLE_DB_DISCOVERY_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "nrf_error.h"
+#include "ble.h"
+#include "ble_gattc.h"
+#include "ble_gatt_db.h"
+#include "nrf_ble_gq.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if !(defined(__LINT__))
+/**@brief Macro for defining a ble_db_discovery instance.
+ *
+ * @param _name Name of the instance.
+ * @hideinitializer
+ */
+#define BLE_DB_DISCOVERY_DEF(_name)                                                                 \
+static ble_db_discovery_t _name = {.discovery_in_progress = 0,                                      \
+                                   .conn_handle           = BLE_CONN_HANDLE_INVALID};               \
+NRF_SDH_BLE_OBSERVER(_name ## _obs,                                                                 \
+                     BLE_DB_DISC_BLE_OBSERVER_PRIO,                                                 \
+                     ble_db_discovery_on_ble_evt, &_name)
+
+
+/** @brief Macro for defining multiple ble_db_discovery instances.
+ *
+ * @param _name Name of the array of instances.
+ * @param _cnt  Number of instances to define.
+ */
+#define BLE_DB_DISCOVERY_ARRAY_DEF(_name, _cnt)                                                     \
+static ble_db_discovery_t _name[_cnt] = {MACRO_REPEAT(_cnt, DB_INIT)};                              \
+                                                                                                    \
+NRF_SDH_BLE_OBSERVERS(_name ## _obs,                                                                \
+                      BLE_DB_DISC_BLE_OBSERVER_PRIO,                                                \
+                      ble_db_discovery_on_ble_evt, &_name, _cnt)
+
+#if !(defined(DOXYGEN))
+#define DB_INIT()                                                                                   \
+{                                                                                                   \
+    .discovery_in_progress = 0,                                                                     \
+    .conn_handle           = BLE_CONN_HANDLE_INVALID,                                               \
+},
+#endif
+
+#else // __LINT__
+/* Swallow semicolons */
+/*lint -save -esym(528, *) -esym(529, *) : Symbol not referenced. */
+#define BLE_DB_DISCOVERY_ARRAY_DEF(A, B) static ble_db_discovery_t A[B]
+#define BLE_DB_DISCOVERY_DEF(A) static ble_db_discovery_t A
+/*lint -restore */
+#endif //!(defined(__LINT__))
+
+#define BLE_DB_DISCOVERY_MAX_SRV        6   /**< Maximum number of services supported by this module. This also indicates the maximum number of users allowed to be registered to this module (one user per service). */
+
+
+/**@brief DB Discovery event type. */
+typedef enum
+{
+    BLE_DB_DISCOVERY_COMPLETE,      /**< Event indicating that the discovery of one service is complete. */
+    BLE_DB_DISCOVERY_ERROR,         /**< Event indicating that an internal error has occurred in the DB Discovery module. This could typically be because of the SoftDevice API returning an error code during the DB discover.*/
+    BLE_DB_DISCOVERY_SRV_NOT_FOUND, /**< Event indicating that the service was not found at the peer.*/
+    BLE_DB_DISCOVERY_AVAILABLE      /**< Event indicating that the DB discovery instance is available.*/
+} ble_db_discovery_evt_type_t;
+
+/**@brief Structure containing the event from the DB discovery module to the application. */
+typedef struct
+{
+    ble_db_discovery_evt_type_t evt_type;    /**< Type of event. */
+    uint16_t                    conn_handle; /**< Handle of the connection for which this event has occurred. */
+    union
+    {
+        ble_gatt_db_srv_t   discovered_db;  /**< Structure containing the information about the GATT Database at the server. This will be filled when the event type is @ref BLE_DB_DISCOVERY_COMPLETE. The UUID field of this will be filled when the event type is @ref BLE_DB_DISCOVERY_SRV_NOT_FOUND. */
+        void const        * p_db_instance;  /**< Pointer to DB discovery instance @ref ble_db_discovery_t, indicating availability to the new discovery process. This will be filled when the event type is @ref BLE_DB_DISCOVERY_AVAILABLE. */
+        uint32_t            err_code;       /**< nRF Error code indicating the type of error which occurred in the DB Discovery module. This will be filled when the event type is @ref BLE_DB_DISCOVERY_ERROR. */
+    } params;
+} ble_db_discovery_evt_t;
+
+/**@brief DB Discovery event handler type. */
+typedef void (* ble_db_discovery_evt_handler_t)(ble_db_discovery_evt_t * p_evt);
+
+/**@brief Structure containing the pending event. */
+typedef struct 
+{
+    ble_db_discovery_evt_t         evt;          /**< Pending event. */
+    ble_db_discovery_evt_handler_t evt_handler;  /**< Event handler which should be called to raise this event. */
+} ble_db_discovery_user_evt_t;
+
+/**@brief Structure for holding the information related to the GATT database at the server.
+ *
+ * @details This module identifies a remote database. Use one instance of this structure per
+ *          connection.
+ *
+ * @warning This structure must be zero-initialized.
+ */
+typedef struct
+{
+    ble_gatt_db_srv_t           services[BLE_DB_DISCOVERY_MAX_SRV];         /**< Information related to the current service being discovered. This is intended for internal use during service discovery.*/
+    uint8_t                     srv_count;                                  /**< Number of services at the peer's GATT database.*/
+    uint8_t                     curr_char_ind;                              /**< Index of the current characteristic being discovered. This is intended for internal use during service discovery.*/
+    uint8_t                     curr_srv_ind;                               /**< Index of the current service being discovered. This is intended for internal use during service discovery.*/
+    uint8_t                     discoveries_count;                          /**< Number of service discoveries made, both successful and unsuccessful. */
+    bool                        discovery_in_progress;                      /**< Variable to indicate whether there is a service discovery in progress. */
+    uint16_t                    conn_handle;                                /**< Connection handle on which the discovery is started. */
+    uint32_t                    pending_usr_evt_index;                      /**< The index to the pending user event array, pointing to the last added pending user event. */
+    ble_db_discovery_user_evt_t pending_usr_evts[BLE_DB_DISCOVERY_MAX_SRV]; /**< Whenever a discovery related event is to be raised to a user module, it is stored in this array first. When all expected services have been discovered, all pending events are sent to the corresponding user modules. */
+} ble_db_discovery_t;
+
+/**@brief DB discovery module initialization struct. */
+typedef struct
+{
+    ble_db_discovery_evt_handler_t   evt_handler;  /**< Event handler to be called by the DB Discovery module. */
+    nrf_ble_gq_t                   * p_gatt_queue; /**< Pointer to BLE GATT Queue instance. */
+} ble_db_discovery_init_t;
+
+
+/**@brief Function for initializing the DB Discovery module.
+ *
+ * @param[in] p_db_init   Pointer to DB discovery initialization structure.
+ *
+ * @retval NRF_SUCCESS    On successful initialization.
+ * @retval NRF_ERROR_NULL If the initialization structure was NULL or
+ *                        the structure content is empty.
+ */
+uint32_t ble_db_discovery_init(ble_db_discovery_init_t * p_db_init);
+
+
+/**@brief Function for closing the DB Discovery module.
+ *
+ * @details This function will clear up any internal variables and states maintained by the
+ *          module. To re-use the module after calling this function, the function @ref
+ *          ble_db_discovery_init must be called again. When using more than one DB Discovery
+ *          instance, this function should be called for each instance.
+ *
+ * @param[out] p_db_discovery Pointer to the DB discovery structure.
+ *
+ * @retval NRF_SUCCESS Operation success.
+ */
+uint32_t ble_db_discovery_close(ble_db_discovery_t * const p_db_discovery);
+
+
+/**@brief Function for registering with the DB Discovery module.
+ *
+ * @details The application can use this function to inform which service it is interested in
+ *          discovering at the server.
+ *
+ * @param[in] p_uuid Pointer to the UUID of the service to be discovered at the server.
+ *
+ * @note The total number of services that can be discovered by this module is @ref
+ *       BLE_DB_DISCOVERY_MAX_SRV. This effectively means that the maximum number of
+ *       registrations possible is equal to the @ref BLE_DB_DISCOVERY_MAX_SRV.
+ *
+ * @retval NRF_SUCCESS             Operation success.
+ * @retval NRF_ERROR_NULL          When a NULL pointer is passed as input.
+ * @retval NRF_ERROR_INVALID_STATE If this function is called without calling the
+ *                                 @ref ble_db_discovery_init.
+ * @retval NRF_ERROR_NO_MEM        The maximum number of registrations allowed by this module
+ *                                 has been reached.
+ */
+uint32_t ble_db_discovery_evt_register(const ble_uuid_t * const p_uuid);
+
+
+/**@brief Function for starting the discovery of the GATT database at the server.
+ *
+ * @param[out] p_db_discovery Pointer to the DB Discovery structure.
+ * @param[in]  conn_handle    The handle of the connection for which the discovery should be
+ *                            started.
+ *
+ * @retval NRF_SUCCESS             Operation success.
+ * @retval NRF_ERROR_NULL          When a NULL pointer is passed as input.
+ * @retval NRF_ERROR_INVALID_STATE If this function is called without calling the
+ *                                 @ref ble_db_discovery_init, or without calling
+ *                                 @ref ble_db_discovery_evt_register.
+ * @retval NRF_ERROR_BUSY          If a discovery is already in progress using
+ *                                 @p p_db_discovery. Use a different @ref ble_db_discovery_t
+ *                                 structure, or wait for a DB Discovery event before retrying.
+ * @return                         This API propagates the error code returned by functions:
+ *                                 @ref nrf_ble_gq_conn_handle_register and @ref nrf_ble_gq_item_add.
+ */
+uint32_t ble_db_discovery_start(ble_db_discovery_t * p_db_discovery,
+                                uint16_t             conn_handle);
+
+
+/**@brief Function for handling the Application's BLE Stack events.
+ *
+ * @param[in]     p_ble_evt Pointer to the BLE event received.
+ * @param[in,out] p_context Pointer to the DB Discovery structure.
+ */
+void ble_db_discovery_on_ble_evt(ble_evt_t const * p_ble_evt,
+                                 void            * p_context);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_DB_DISCOVERY_H__
+
+/** @} */

components/ble/ble_link_ctx_manager/ble_link_ctx_manager.c

@@ -0,0 +1,79 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+#include "ble_link_ctx_manager.h"
+#include "sdk_common.h"
+
+
+ret_code_t blcm_link_ctx_get(blcm_link_ctx_storage_t const * const p_link_ctx_storage,
+                             uint16_t                        const conn_handle,
+                             void                         ** const pp_ctx_data)
+{
+    uint8_t conn_id;
+
+    if (pp_ctx_data == NULL)
+    {
+        return NRF_ERROR_NULL;
+    }
+    else
+    {
+        *pp_ctx_data = NULL;
+    }
+
+    VERIFY_PARAM_NOT_NULL(p_link_ctx_storage);
+    VERIFY_PARAM_NOT_NULL(p_link_ctx_storage->p_ctx_data_pool);
+    VERIFY_TRUE((p_link_ctx_storage->link_ctx_size % BYTES_PER_WORD) == 0, NRF_ERROR_INVALID_PARAM);
+
+    conn_id = ble_conn_state_conn_idx(conn_handle);
+
+    if (conn_id == BLE_CONN_STATE_MAX_CONNECTIONS)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    if (conn_id >= p_link_ctx_storage->max_links_cnt)
+    {
+        return NRF_ERROR_NO_MEM;
+    }
+
+    *pp_ctx_data = (void *) ((uint8_t *) p_link_ctx_storage->p_ctx_data_pool + 
+                             conn_id * p_link_ctx_storage->link_ctx_size);
+    return NRF_SUCCESS;
+}
+

components/ble/ble_link_ctx_manager/ble_link_ctx_manager.h

@@ -0,0 +1,127 @@
+/**
+ * Copyright (c) 2017 - 2021, 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_link_ctx_manager BLE Link Context Manager
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief  Storage for link-related data.
+ *
+ * @details BLE Link Context Manager can be used as a simple storage for link-related data.
+ *          Each link context data is uniquely identified within the storage by its connection
+ *          handle and can be retrieved from it by using this handle.
+ *
+ */
+#ifndef BLE_LINK_CTX_MANAGER_H__
+#define BLE_LINK_CTX_MANAGER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include "ble_conn_state.h"
+#include "sdk_errors.h"
+
+
+/**@brief Macro for defining a blcm_link_ctx_storage instance.
+ *
+ * @param[in]   _name            Name of the instance.
+ * @param[in]   _max_clients     Maximum number of clients connected at a time.
+ * @param[in]   _link_ctx_size   Context size in bytes for a single link.
+ */
+#define BLE_LINK_CTX_MANAGER_DEF(_name, _max_clients, _link_ctx_size)                               \
+    STATIC_ASSERT((_max_clients) <= BLE_CONN_STATE_MAX_CONNECTIONS);                                \
+    static uint32_t CONCAT_2(_name, _ctx_data_pool)[(_max_clients)*BYTES_TO_WORDS(_link_ctx_size)]; \
+    static blcm_link_ctx_storage_t _name =                                                          \
+    {                                                                                               \
+        .p_ctx_data_pool = CONCAT_2(_name, _ctx_data_pool),                                         \
+        .max_links_cnt   = (_max_clients),                                                          \
+        .link_ctx_size   = sizeof(CONCAT_2(_name, _ctx_data_pool))/(_max_clients)                   \
+    }
+
+
+/**
+ * @brief Type of description that is used for registry of all current connections.
+ */
+typedef struct
+{
+    void   * const p_ctx_data_pool; /**< Pointer to links context memory pool. */
+    uint8_t  const max_links_cnt;   /**< Maximum number of concurrent links. */
+    uint16_t const link_ctx_size;   /**< Context size in bytes for a single link (word-aligned). */
+} blcm_link_ctx_storage_t;
+
+
+/**
+ * @brief Function for getting the link context from the link registry.
+ *
+ * This function finds the link context in the registry. The link to find is identified by the
+ * connection handle within the registry.
+ *
+ * The context is preserved for the lifetime of the connection. When a new connection occurs, the
+ * value of its context is undefined, and should be initialized.
+ *
+ * @param[in]  p_link_ctx_storage  Pointer to the link storage descriptor.
+ * @param[in]  conn_handle         Connection whose context to find.
+ * @param[out] pp_ctx_data         Pointer to data with context for the connection.
+ *
+ * @retval NRF_ERROR_NULL          If \p p_link_ctx_storage is NULL or contains a NULL pointer, or if
+ *                                 \p pp_ctx_data is NULL.
+ * @retval NRF_ERROR_INVALID_PARAM If \p p_link_ctx_storage::link_ctx_size is not multiple of word
+ *                                 size.
+ * @retval NRF_ERROR_NOT_FOUND     If \p conn_handle does not refer to an active or recently
+ *                                 disconnected link.
+ * @retval NRF_ERROR_NO_MEM        If there is not enough memory to store context for the given
+ *                                 connection handle. This can happen if the number of links is
+ *                                 greater than \p p_link_ctx_storage::max_links_cnt.
+ * @retval NRF_SUCCESS             If the operation was successful.
+ */
+ret_code_t blcm_link_ctx_get(blcm_link_ctx_storage_t const * const p_link_ctx_storage,
+                             uint16_t                        const conn_handle,
+                             void                         ** const pp_ctx_data);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_LINK_CTX_MANAGER_H__
+
+/** @} */

components/ble/ble_radio_notification/ble_radio_notification.c

@@ -0,0 +1,88 @@
+/**
+ * Copyright (c) 2012 - 2021, 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.
+ *
+ */
+#include "ble_radio_notification.h"
+#include "nrf_nvic.h"
+#include <stdlib.h>
+
+
+static bool                                 m_radio_active = false;  /**< Current radio state. */
+static ble_radio_notification_evt_handler_t m_evt_handler  = NULL;   /**< Application event handler for handling Radio Notification events. */
+
+
+void SWI1_IRQHandler(void)
+{
+    m_radio_active = !m_radio_active;
+    if (m_evt_handler != NULL)
+    {
+        m_evt_handler(m_radio_active);
+    }
+}
+
+
+uint32_t ble_radio_notification_init(uint32_t                             irq_priority,
+                                     uint8_t                              distance,
+                                     ble_radio_notification_evt_handler_t evt_handler)
+{
+    uint32_t err_code;
+
+    m_evt_handler = evt_handler;
+
+    // Initialize Radio Notification software interrupt
+    err_code = sd_nvic_ClearPendingIRQ(SWI1_IRQn);
+    if (err_code != NRF_SUCCESS)
+    {
+        return err_code;
+    }
+
+    err_code = sd_nvic_SetPriority(SWI1_IRQn, irq_priority);
+    if (err_code != NRF_SUCCESS)
+    {
+        return err_code;
+    }
+
+    err_code = sd_nvic_EnableIRQ(SWI1_IRQn);
+    if (err_code != NRF_SUCCESS)
+    {
+        return err_code;
+    }
+
+    // Configure the event
+    return sd_radio_notification_cfg_set(NRF_RADIO_NOTIFICATION_TYPE_INT_ON_BOTH, distance);
+}

components/ble/ble_radio_notification/ble_radio_notification.h

@@ -0,0 +1,82 @@
+/**
+ * Copyright (c) 2012 - 2021, 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_radio_notification Radio Notification Event Handler
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Module for propagating Radio Notification events to the application.
+ */
+
+#ifndef BLE_RADIO_NOTIFICATION_H__
+#define BLE_RADIO_NOTIFICATION_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "nrf_soc.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief Application radio notification event handler type. */
+typedef void (*ble_radio_notification_evt_handler_t) (bool radio_active);
+
+/**@brief Function for initializing the Radio Notification module.
+ *
+ * @param[in]  irq_priority   Interrupt priority for the Radio Notification interrupt handler.
+ * @param[in]  distance       The time from an Active event until the radio is activated.
+ * @param[in]  evt_handler    Handler to be executed when a radio notification event has been
+ *                            received.
+ *
+ * @return     NRF_SUCCESS on successful initialization, otherwise an error code.
+ */
+uint32_t ble_radio_notification_init(uint32_t                             irq_priority,
+                                     uint8_t                              distance,
+                                     ble_radio_notification_evt_handler_t evt_handler);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_RADIO_NOTIFICATION_H__
+
+/** @} */

components/ble/ble_services/ble_dfu/ble_dfu.c

@@ -0,0 +1,323 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+/* Attention!
+ *  To maintain compliance with Nordic Semiconductor ASA's Bluetooth profile
+ *  qualification listings, this section of source code must not be modified.
+ */
+
+#include "ble_dfu.h"
+#include <string.h>
+#include "ble_hci.h"
+#include "sdk_macros.h"
+#include "ble_srv_common.h"
+#include "nrf_nvic.h"
+#include "nrf_sdm.h"
+#include "nrf_soc.h"
+#include "nrf_log.h"
+#include "nrf_dfu_ble_svci_bond_sharing.h"
+#include "nrf_bootloader_info.h"
+#include "nrf_svci_async_function.h"
+#include "nrf_pwr_mgmt.h"
+#include "peer_manager.h"
+#include "gatts_cache_manager.h"
+#include "peer_id.h"
+
+#define MAX_CTRL_POINT_RESP_PARAM_LEN   3                           /**< Max length of the responses. */
+
+#define BLE_DFU_SERVICE_UUID            0xFE59                      /**< The 16-bit UUID of the Secure DFU Service. */
+
+static ble_dfu_buttonless_t             m_dfu;                      /**< Structure holding information about the Buttonless Secure DFU Service. */
+
+NRF_SDH_BLE_OBSERVER(m_dfus_obs, BLE_DFU_BLE_OBSERVER_PRIO, ble_dfu_buttonless_on_ble_evt, &m_dfu);
+
+
+/**@brief Function that is called if no event handler is provided.
+ */
+static void dummy_evt_handler(ble_dfu_buttonless_evt_type_t evt)
+{
+    NRF_LOG_DEBUG("Dummy event handler received event 0x%x", evt);
+}
+
+
+/**@brief Function for handling write events to the Buttonless Secure DFU Service Service Control Point characteristic.
+ *
+ * @param[in]   p_evt_write   Write event received from the BLE stack.
+ */
+static void on_ctrlpt_write(ble_gatts_evt_write_t const * p_evt_write)
+{
+    uint32_t      err_code;
+
+    ble_gatts_rw_authorize_reply_params_t write_authorize_reply;
+    memset(&write_authorize_reply, 0, sizeof(write_authorize_reply));
+
+    write_authorize_reply.type   = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
+
+    uint8_t cccd_val[2];
+    ble_gatts_value_t value = {.p_value = cccd_val, .len = 2, .offset = 0};
+    err_code = sd_ble_gatts_value_get(m_dfu.conn_handle, m_dfu.control_point_char.cccd_handle, &value);
+    if (err_code == NRF_SUCCESS && ble_srv_is_indication_enabled(cccd_val))
+    {
+        write_authorize_reply.params.write.update      = 1;
+        write_authorize_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
+    }
+    else
+    {
+        write_authorize_reply.params.write.gatt_status = BLE_GATT_STATUS_ATTERR_CPS_CCCD_CONFIG_ERROR;
+    }
+
+    // Authorize the write request
+    do {
+        err_code = sd_ble_gatts_rw_authorize_reply(m_dfu.conn_handle, &write_authorize_reply);
+    } while (err_code == NRF_ERROR_BUSY);
+
+
+    if (write_authorize_reply.params.write.gatt_status != BLE_GATT_STATUS_SUCCESS)
+    {
+        return;
+    }
+
+    // Forward the write event to the Buttonless DFU module.
+    ble_dfu_buttonless_on_ctrl_pt_write(p_evt_write);
+}
+
+
+/**@brief Write authorization request event handler.
+ *
+ * @details The write authorization request event handler is called when writing to the control point.
+ *
+ * @param[in]   p_ble_evt Event received from the BLE stack.
+ */
+static void on_rw_authorize_req(ble_evt_t const * p_ble_evt)
+{
+    if (p_ble_evt->evt.gatts_evt.conn_handle != m_dfu.conn_handle)
+    {
+        return;
+    }
+
+    const ble_gatts_evt_rw_authorize_request_t * p_auth_req =
+        &p_ble_evt->evt.gatts_evt.params.authorize_request;
+
+    if (
+        (p_auth_req->type == BLE_GATTS_AUTHORIZE_TYPE_WRITE)                            &&
+        (p_auth_req->request.write.handle == m_dfu.control_point_char.value_handle)     &&
+        (p_auth_req->request.write.op != BLE_GATTS_OP_PREP_WRITE_REQ)                   &&
+        (p_auth_req->request.write.op != BLE_GATTS_OP_EXEC_WRITE_REQ_NOW)               &&
+        (p_auth_req->request.write.op != BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL)
+       )
+    {
+        on_ctrlpt_write(&p_auth_req->request.write);
+    }
+}
+
+
+/**@brief Connect event handler.
+ *
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+static void on_connect(ble_evt_t const * p_ble_evt)
+{
+    m_dfu.conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+}
+
+
+/**@brief Disconnect event handler.
+ *
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+static void on_disconnect(ble_evt_t const * p_ble_evt)
+{
+    if (m_dfu.conn_handle != p_ble_evt->evt.gap_evt.conn_handle)
+    {
+        return;
+    }
+
+    m_dfu.conn_handle = BLE_CONN_HANDLE_INVALID;
+}
+
+
+/**@brief Function for handling the HVC events.
+ *
+ * @details Handles HVC events from the BLE stack.
+ *
+ * @param[in] p_ble_evt  Event received from the BLE stack.
+ */
+static void on_hvc(ble_evt_t const * p_ble_evt)
+{
+    uint32_t err_code;
+    ble_gatts_evt_hvc_t const * p_hvc = &p_ble_evt->evt.gatts_evt.params.hvc;
+
+    if (p_hvc->handle == m_dfu.control_point_char.value_handle)
+    {
+        // Enter bootloader if we were waiting for reset after hvc indication confimation.
+        if (m_dfu.is_waiting_for_reset)
+        {
+            err_code = ble_dfu_buttonless_bootloader_start_prepare();
+            if (err_code != NRF_SUCCESS)
+            {
+                m_dfu.evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+            }
+        }
+    }
+}
+
+
+void ble_dfu_buttonless_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    VERIFY_PARAM_NOT_NULL_VOID(p_ble_evt);
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            on_connect(p_ble_evt);
+            break;
+
+        case BLE_GAP_EVT_DISCONNECTED:
+            on_disconnect(p_ble_evt);
+            break;
+
+        case BLE_GATTS_EVT_RW_AUTHORIZE_REQUEST:
+            on_rw_authorize_req(p_ble_evt);
+            break;
+
+        case BLE_GATTS_EVT_HVC:
+            on_hvc(p_ble_evt);
+            break;
+
+        default:
+            // no implementation
+            break;
+    }
+}
+
+
+uint32_t ble_dfu_buttonless_resp_send(ble_dfu_buttonless_op_code_t op_code, ble_dfu_buttonless_rsp_code_t rsp_code)
+{
+    // Send indication
+    uint32_t                err_code;
+    const uint16_t          len = MAX_CTRL_POINT_RESP_PARAM_LEN;
+    uint16_t                hvx_len;
+    uint8_t                 hvx_data[MAX_CTRL_POINT_RESP_PARAM_LEN];
+    ble_gatts_hvx_params_t  hvx_params;
+
+    memset(&hvx_params, 0, sizeof(hvx_params));
+
+    hvx_len     = len;
+    hvx_data[0] = DFU_OP_RESPONSE_CODE;
+    hvx_data[1] = (uint8_t)op_code;
+    hvx_data[2] = (uint8_t)rsp_code;
+
+    hvx_params.handle = m_dfu.control_point_char.value_handle;
+    hvx_params.type   = BLE_GATT_HVX_INDICATION;
+    hvx_params.offset = 0;
+    hvx_params.p_len  = &hvx_len;
+    hvx_params.p_data = hvx_data;
+
+    err_code = sd_ble_gatts_hvx(m_dfu.conn_handle, &hvx_params);
+    if ((err_code == NRF_SUCCESS) && (hvx_len != len))
+    {
+        err_code = NRF_ERROR_DATA_SIZE;
+    }
+
+    return err_code;
+}
+
+
+uint32_t ble_dfu_buttonless_bootloader_start_finalize(void)
+{
+    uint32_t err_code;
+
+    NRF_LOG_DEBUG("In ble_dfu_buttonless_bootloader_start_finalize\r\n");
+
+    err_code = sd_power_gpregret_clr(0, 0xffffffff);
+    VERIFY_SUCCESS(err_code);
+
+    err_code = sd_power_gpregret_set(0, BOOTLOADER_DFU_START);
+    VERIFY_SUCCESS(err_code);
+
+    // Indicate that the Secure DFU bootloader will be entered
+    m_dfu.evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER);
+
+    // Signal that DFU mode is to be enter to the power management module
+    nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_DFU);
+
+    return NRF_SUCCESS;
+}
+
+
+uint32_t ble_dfu_buttonless_init(const ble_dfu_buttonless_init_t * p_dfu_init)
+{
+    uint32_t        err_code;
+    ble_uuid_t      service_uuid;
+    ble_uuid128_t   nordic_base_uuid = BLE_NORDIC_VENDOR_BASE_UUID;
+
+    VERIFY_PARAM_NOT_NULL(p_dfu_init);
+
+    // Initialize the service structure.
+    m_dfu.conn_handle                  = BLE_CONN_HANDLE_INVALID;
+    m_dfu.evt_handler                  = p_dfu_init->evt_handler;
+    m_dfu.is_waiting_for_reset         = false;
+
+    if (m_dfu.evt_handler == NULL)
+    {
+        m_dfu.evt_handler = dummy_evt_handler;
+    }
+
+    err_code = ble_dfu_buttonless_backend_init(&m_dfu);
+    VERIFY_SUCCESS(err_code);
+
+    BLE_UUID_BLE_ASSIGN(service_uuid, BLE_DFU_SERVICE_UUID);
+
+    // Add the DFU service declaration.
+    err_code = sd_ble_gatts_service_add(BLE_GATTS_SRVC_TYPE_PRIMARY,
+                                        &service_uuid,
+                                        &(m_dfu.service_handle));
+
+    VERIFY_SUCCESS(err_code);
+
+    // Add vendor specific base UUID to use with the Buttonless DFU characteristic.
+    err_code = sd_ble_uuid_vs_add(&nordic_base_uuid, &m_dfu.uuid_type);
+    VERIFY_SUCCESS(err_code);
+
+    // Add the Buttonless DFU Characteristic (with bonds/without bonds).
+    err_code = ble_dfu_buttonless_char_add(&m_dfu);
+    VERIFY_SUCCESS(err_code);
+
+    return NRF_SUCCESS;
+}

components/ble/ble_services/ble_dfu/ble_dfu.h

@@ -0,0 +1,249 @@
+/**
+ * Copyright (c) 2012 - 2021, 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_dfu Buttonless DFU Service
+ * @{
+ * @ingroup ble_sdk_srv
+ * @brief Buttonless DFU Service module.
+ *
+ * @details This module implements a proprietary Buttonless Secure DFU Service. The service can
+ *          be configured to support bonds or not. The bond support configuration must correspond to the
+ *          requirement of Secure DFU bootloader.
+ *
+ * @note Attention!
+ *  To maintain compliance with Nordic Semiconductor ASA Bluetooth profile
+ *  qualification listings, this section of source code must not be modified.
+ */
+
+#ifndef BLE_DFU_H__
+#define BLE_DFU_H__
+
+#include <stdint.h>
+#include "ble_srv_common.h"
+#include "nrf_sdh_ble.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**@brief   SoC observer priority.
+ * @details Priority of this module's SoC event handler.
+ */
+#define BLE_DFU_SOC_OBSERVER_PRIO   1
+
+#define BLE_DFU_BUTTONLESS_CHAR_UUID        (0x0003)    /**< Value combined with vendor-specific base to create Unbonded Buttonless characteristic UUID. */
+#define BLE_DFU_BUTTONLESS_BONDED_CHAR_UUID (0x0004)    /**< Value combined with vendor-specific base to create Bonded Buttonless characteristic UUID. */
+
+
+/**@brief Nordic vendor-specific base UUID.
+ */
+#define BLE_NORDIC_VENDOR_BASE_UUID                 \
+{{                                                  \
+    0x50, 0xEA, 0xDA, 0x30, 0x88, 0x83, 0xB8, 0x9F, \
+    0x60, 0x4F, 0x15, 0xF3, 0x00, 0x00, 0xC9, 0x8E  \
+}}
+
+
+/**@brief Nordic Buttonless DFU Service event type .
+ */
+typedef enum
+{
+    BLE_DFU_EVT_BOOTLOADER_ENTER_PREPARE,   /**< Event indicating that the device is preparing to enter bootloader.*/
+    BLE_DFU_EVT_BOOTLOADER_ENTER,           /**< Event indicating that the bootloader will be entered after return of this event.*/
+    BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED,    /**< Failure to enter bootloader mode.*/
+    BLE_DFU_EVT_RESPONSE_SEND_ERROR,        /**< Failure to send response.*/
+} ble_dfu_buttonless_evt_type_t;
+
+
+/**@brief Nordic Buttonless DFU Service event handler type.
+ */
+typedef void (*ble_dfu_buttonless_evt_handler_t) (ble_dfu_buttonless_evt_type_t p_evt);
+/**@brief Enumeration of Bootloader DFU response codes.
+ */
+typedef enum
+{
+    DFU_RSP_INVALID               = 0x00,                                           /**< Invalid op code. */
+    DFU_RSP_SUCCESS               = 0x01,                                           /**< Success. */
+    DFU_RSP_OP_CODE_NOT_SUPPORTED = 0x02,                                           /**< Op code not supported. */
+    DFU_RSP_OPERATION_FAILED      = 0x04,                                           /**< Operation failed. */
+    DFU_RSP_ADV_NAME_INVALID      = 0x05,                                           /**< Requested advertisement name is too short or too long. */
+    DFU_RSP_BUSY                  = 0x06,                                           /**< Ongoing async operation. */
+    DFU_RSP_NOT_BONDED            = 0x07,                                           /**< Buttonless unavailable due to device not bonded. */
+} ble_dfu_buttonless_rsp_code_t;
+
+
+/**@brief Enumeration of Bootloader DFU Operation codes.
+ */
+typedef enum
+{
+    DFU_OP_RESERVED         = 0x00, /**< Reserved for future use. */
+    DFU_OP_ENTER_BOOTLOADER = 0x01, /**< Enter bootloader. */
+    DFU_OP_SET_ADV_NAME     = 0x02, /**< Set advertisement name to use in DFU mode. */
+    DFU_OP_RESPONSE_CODE    = 0x20  /**< Response code. */
+} ble_dfu_buttonless_op_code_t;
+
+
+/**@brief Type holding memory used by Secure DFU Buttonless Service.
+  */
+typedef struct
+{
+    uint8_t                             uuid_type;                      /**< UUID type for DFU UUID. */
+    uint16_t                            service_handle;                 /**< Service Handle of DFU (as provided by the SoftDevice). */
+    uint16_t                            conn_handle;                    /**< Connection handle for the current peer. */
+    ble_gatts_char_handles_t            control_point_char;             /**< Handles related to the DFU Control Point characteristic. */
+    uint32_t                            peers_count;                    /**< Counter to see how many persistently stored peers must be updated for Service Changed indication. This value will be counted down when comparing write requests. */
+    ble_dfu_buttonless_evt_handler_t    evt_handler;                    /**< Event handler that is called upon Buttonless DFU events. See @ref ble_dfu_buttonless_evt_type_t. */
+    bool                                is_waiting_for_reset;           /**< Flag indicating that the device will enter bootloader. */
+    bool                                is_waiting_for_svci;            /**< Flag indicating that the device is waiting for async SVCI operation */
+} ble_dfu_buttonless_t;
+
+
+/**@brief Type used to initialize the Secure DFU Buttonless Service.
+ */
+typedef struct
+{
+    ble_dfu_buttonless_evt_handler_t   evt_handler;                       /**< Bootloader event handler. */
+} ble_dfu_buttonless_init_t;
+
+
+/**@brief Function for initializing the Device Firmware Update module.
+ *
+ * @param[in]   p_dfu_init   Structure containing the values of characteristics needed by the
+ *                           service.
+ * @retval      NRF_SUCCESS on successful initialization of the service.
+ */
+uint32_t ble_dfu_buttonless_init(const ble_dfu_buttonless_init_t * p_dfu_init);
+
+
+/**@brief Function for initializing the async SVCI interface.
+ *
+ * @warning Ensure that no interrupts are triggered when calling this functions as
+ *          interrupts and exceptions are forwarded to the bootloader for the period
+ *          of the call and may be lost.
+ *
+ * @details This configures the async interface for calling to the
+ *          bootloader through SVCI interface.
+ *
+ * @retval NRF_SUCCESS on success, otherwise an error code.
+ */
+uint32_t ble_dfu_buttonless_async_svci_init(void);
+
+
+/**@brief Function to initialize the backend Secure DFU Buttonless service which is either
+ *        supports bonds or not.
+ *
+ * @note    Do not call this function directly. It is called internally by @ref ble_dfu_buttonless_init.
+ *
+ * @param[in] p_dfu     Nordic DFU Service structure.
+ *
+ * @return NRF_SUCCESS  On sucessfully initializing, otherwise an error code.
+ */
+uint32_t ble_dfu_buttonless_backend_init(ble_dfu_buttonless_t * p_dfu);
+
+
+
+/**@brief Function for adding the buttonless characteristic.
+ *
+ * @note This will be implemented differently on bonded/unbonded Buttonless DFU service.
+ *
+ * @param[in] p_dfu       Nordic DFU Service structure.
+ *
+ * @return NRF_SUCCESS on success, otherwise an error code.
+ */
+uint32_t ble_dfu_buttonless_char_add(ble_dfu_buttonless_t * p_dfu);
+
+
+/**@brief Function for sending a response back to the client.
+ *
+ * @param[in]   op_code     Operation code to send the response for.
+ * @param[in]   rsp_code    Response code for the operation.
+ *
+ * @retval NRF_SUCCESS on success, otherwise an error code.
+ */
+uint32_t ble_dfu_buttonless_resp_send(ble_dfu_buttonless_op_code_t op_code, ble_dfu_buttonless_rsp_code_t rsp_code);
+
+
+/**@brief Function for handling the application's BLE stack events.
+ *
+ * @details Handles all events from the BLE stack of interest to the DFU buttonless service.
+ *
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ * @param[in]   p_context   BLE context structure.
+ */
+void ble_dfu_buttonless_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context);
+
+
+/**@brief Function for handling control point write requests.
+ *
+ * @details Handles write requests to the control point in
+ *          DFU with bonds or without bonds.
+ *
+ * @param[in]   p_evt_write     GATTS write event.
+ */
+void ble_dfu_buttonless_on_ctrl_pt_write(ble_gatts_evt_write_t const * p_evt_write);
+
+
+/**@brief Function for preparing to enter the bootloader.
+ *
+ * @warning This function is called directly. (It is called internally).
+ *
+ * @retval Any error code from calling @ref sd_ble_gap_disconnect.
+ */
+uint32_t ble_dfu_buttonless_bootloader_start_prepare(void);
+
+
+/**@brief Function for finalizing entering the bootloader.
+ *
+ * @warning This function is not to be called. (It is called internally).
+ *
+ * @retval NRF_SUCCESS Finalize was started correctly.
+ */
+uint32_t ble_dfu_buttonless_bootloader_start_finalize(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_DIS_H__
+
+/** @} */

components/ble/ble_services/ble_dfu/ble_dfu_bonded.c

@@ -0,0 +1,367 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include "nrf_dfu_ble_svci_bond_sharing.h"
+#include "nordic_common.h"
+#include "nrf_error.h"
+#include "ble_dfu.h"
+#include "nrf_log.h"
+#include "peer_manager.h"
+#include "gatts_cache_manager.h"
+#include "peer_id.h"
+#include "nrf_sdh_soc.h"
+#include "nrf_strerror.h"
+
+#if (NRF_DFU_BLE_BUTTONLESS_SUPPORTS_BONDS)
+
+
+void ble_dfu_buttonless_on_sys_evt(uint32_t, void * );
+uint32_t nrf_dfu_svci_vector_table_set(void);
+uint32_t nrf_dfu_svci_vector_table_unset(void);
+
+/**@brief Define function for async interface to set peer data. */
+NRF_SVCI_ASYNC_FUNC_DEFINE(NRF_DFU_SVCI_SET_PEER_DATA, nrf_dfu_set_peer_data, nrf_dfu_peer_data_t);
+
+// Register SoC observer for the Buttonless Secure DFU service
+NRF_SDH_SOC_OBSERVER(m_dfu_buttonless_soc_obs, BLE_DFU_SOC_OBSERVER_PRIO, ble_dfu_buttonless_on_sys_evt, NULL);
+
+ble_dfu_buttonless_t       * mp_dfu;
+static nrf_dfu_peer_data_t   m_peer_data;
+
+
+/**@brief Function for handling Peer Manager events.
+ *
+ * @param[in] p_evt  Peer Manager event.
+ */
+static void pm_evt_handler(pm_evt_t const * p_evt)
+{
+    uint32_t ret;
+
+    if (mp_dfu == NULL)
+    {
+        return;
+    }
+
+    // Only handle this when we are waiting to reset into DFU mode
+    if (!mp_dfu->is_waiting_for_reset)
+    {
+        return;
+    }
+
+    switch(p_evt->evt_id)
+    {
+        case PM_EVT_PEER_DATA_UPDATE_SUCCEEDED:
+            if (p_evt->params.peer_data_update_succeeded.data_id == PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING)
+            {
+                mp_dfu->peers_count--;
+                NRF_LOG_DEBUG("Updating Service Changed indication for peers, %d left", mp_dfu->peers_count);
+                if (mp_dfu->peers_count == 0)
+                {
+                    NRF_LOG_DEBUG("Finished updating Service Changed indication for peers");
+                    // We have updated Service Changed Indication for all devices.
+                    ret = ble_dfu_buttonless_bootloader_start_finalize();
+                    if (ret != NRF_SUCCESS)
+                    {
+                        mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+                    }
+                }
+            }
+            break;
+
+        case PM_EVT_PEER_DATA_UPDATE_FAILED:
+            // Failure to update data. Service Changed cannot be sent but DFU mode is still possible
+            ret = ble_dfu_buttonless_bootloader_start_finalize();
+            if (ret != NRF_SUCCESS)
+            {
+                mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+            }
+            break;
+
+        default:
+            break;
+    }
+}
+
+
+static uint32_t retrieve_peer_data(void)
+{
+    ret_code_t              ret;
+    pm_peer_data_bonding_t  bonding_data = {0};
+    pm_peer_id_t            peer_id;
+
+    ret = pm_peer_id_get(mp_dfu->conn_handle, &peer_id);
+    VERIFY_SUCCESS(ret);
+
+    if (peer_id == PM_PEER_ID_INVALID)
+    {
+        return NRF_ERROR_FORBIDDEN;
+    }
+
+    ret = pm_peer_data_bonding_load(peer_id, &bonding_data);
+    VERIFY_SUCCESS(ret);
+
+    memcpy(&m_peer_data.ble_id,  &bonding_data.peer_ble_id, sizeof(ble_gap_id_key_t));
+    memcpy(&m_peer_data.enc_key, &bonding_data.own_ltk,     sizeof(ble_gap_enc_key_t));
+
+    uint16_t len = SYSTEM_SERVICE_ATT_SIZE;
+    ret = sd_ble_gatts_sys_attr_get(mp_dfu->conn_handle,
+                                    m_peer_data.sys_serv_attr,
+                                    &len,
+                                    BLE_GATTS_SYS_ATTR_FLAG_SYS_SRVCS);
+
+    NRF_LOG_DEBUG("system attribute table len: %d", len);
+
+    return ret;
+}
+
+
+/**@brief Function for entering the bootloader.
+ *
+ * @details This starts forwarding peer data to the Secure DFU bootloader.
+ */
+static uint32_t enter_bootloader(void)
+{
+    uint32_t ret;
+
+    NRF_LOG_INFO("Writing peer data to the bootloader...");
+
+    if (mp_dfu->is_waiting_for_svci)
+    {
+        return ble_dfu_buttonless_resp_send(DFU_OP_ENTER_BOOTLOADER, DFU_RSP_BUSY);
+    }
+
+    // If retrieve_peer_data returns NRF_ERROR_FORBIDDEN, then the device was not bonded.
+    ret = retrieve_peer_data();
+    VERIFY_SUCCESS(ret);
+
+    ret = nrf_dfu_set_peer_data(&m_peer_data);
+    if (ret == NRF_SUCCESS)
+    {
+        // The request was accepted. Waiting for sys events to progress.
+        mp_dfu->is_waiting_for_svci = true;
+    }
+    else if (ret == NRF_ERROR_FORBIDDEN)
+    {
+        NRF_LOG_ERROR("The bootloader has write protected its settings page. This prohibits setting the peer data. "\
+                      "The bootloader must be compiled with NRF_BL_SETTINGS_PAGE_PROTECT=0 to allow setting the peer data.");
+    }
+
+    return ret;
+}
+
+
+uint32_t ble_dfu_buttonless_backend_init(ble_dfu_buttonless_t * p_dfu)
+{
+    VERIFY_PARAM_NOT_NULL(p_dfu);
+
+    // Set the memory used by the backend.
+    mp_dfu = p_dfu;
+
+    // Initialize the Peer manager handler.
+    return pm_register(pm_evt_handler);
+}
+
+
+uint32_t ble_dfu_buttonless_async_svci_init(void)
+{
+    uint32_t ret;
+
+    // Set the vector table base address to the bootloader.
+    ret = nrf_dfu_svci_vector_table_set();
+    NRF_LOG_DEBUG("nrf_dfu_svci_vector_table_set() -> %s",
+                  (ret == NRF_SUCCESS) ? "success" : nrf_strerror_get(ret));
+    VERIFY_SUCCESS(ret);
+
+    // Initialize the asynchronous SuperVisor interface to set peer data in Secure DFU bootloader.
+    ret = nrf_dfu_set_peer_data_init();
+    NRF_LOG_DEBUG("nrf_dfu_set_peer_data_init() -> %s",
+                  (ret == NRF_SUCCESS) ? "success" : nrf_strerror_get(ret));
+    VERIFY_SUCCESS(ret);
+
+    // Set the vector table base address back to main application.
+    ret = nrf_dfu_svci_vector_table_unset();
+    NRF_LOG_DEBUG("nrf_dfu_svci_vector_table_unset() -> %s",
+                  (ret == NRF_SUCCESS) ? "success" : nrf_strerror_get(ret));
+
+    return ret;
+}
+
+
+void ble_dfu_buttonless_on_sys_evt(uint32_t sys_evt, void * p_context)
+{
+    uint32_t ret;
+
+    if (!nrf_dfu_set_peer_data_is_initialized())
+    {
+        return;
+    }
+
+    ret = nrf_dfu_set_peer_data_on_sys_evt(sys_evt);
+    if (ret == NRF_ERROR_INVALID_STATE)
+    {
+        // The system event is not from an operation started by buttonless DFU.
+        // No action is taken, and nothing is reported.
+    }
+    else if (ret == NRF_SUCCESS)
+    {
+        // Peer data was successfully forwarded to the Secure DFU bootloader.
+        // Set the flag indicating that we are waiting for indication response
+        // to activate the reset.
+        mp_dfu->is_waiting_for_reset = true;
+        mp_dfu->is_waiting_for_svci  = false;
+
+        // Report back the positive response
+        ret = ble_dfu_buttonless_resp_send(DFU_OP_ENTER_BOOTLOADER, DFU_RSP_SUCCESS);
+        if (ret != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+            mp_dfu->is_waiting_for_reset = false;
+        }
+    }
+    else
+    {
+        // Failed to set peer data. Report this.
+        mp_dfu->is_waiting_for_reset = false;
+        mp_dfu->is_waiting_for_svci  = false;
+        ret = ble_dfu_buttonless_resp_send(DFU_OP_ENTER_BOOTLOADER, DFU_RSP_BUSY);
+
+        // Report the failure to send the response to the client
+        if (ret != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+        }
+
+        // Report the failure to enter DFU mode
+        mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+    }
+}
+
+
+uint32_t ble_dfu_buttonless_char_add(ble_dfu_buttonless_t * p_dfu)
+{
+    ble_add_char_params_t add_char_params;
+
+    memset(&add_char_params, 0, sizeof(add_char_params));
+    add_char_params.uuid                = BLE_DFU_BUTTONLESS_BONDED_CHAR_UUID;
+    add_char_params.uuid_type           = p_dfu->uuid_type;
+    add_char_params.char_props.indicate = 1;
+    add_char_params.char_props.write    = 1;
+    add_char_params.is_defered_write    = true;
+    add_char_params.is_var_len          = true;
+    add_char_params.max_len             = BLE_GATT_ATT_MTU_DEFAULT;
+
+    add_char_params.cccd_write_access = SEC_JUST_WORKS;
+    add_char_params.write_access      = SEC_JUST_WORKS;
+    add_char_params.read_access       = SEC_OPEN;
+
+    return characteristic_add(p_dfu->service_handle, &add_char_params, &p_dfu->control_point_char);
+}
+
+
+void ble_dfu_buttonless_on_ctrl_pt_write(ble_gatts_evt_write_t const * p_evt_write)
+{
+    uint32_t ret;
+    ble_dfu_buttonless_rsp_code_t rsp_code = DFU_RSP_OPERATION_FAILED;
+
+    // Start executing the control point write action
+    switch (p_evt_write->data[0])
+    {
+        case DFU_OP_ENTER_BOOTLOADER:
+            ret = enter_bootloader();
+            if (ret == NRF_SUCCESS)
+            {
+                rsp_code = DFU_RSP_SUCCESS;
+            }
+            else if (ret == NRF_ERROR_BUSY)
+            {
+                rsp_code = DFU_RSP_BUSY;
+            }
+            else if (ret == NRF_ERROR_FORBIDDEN)
+            {
+                rsp_code = DFU_RSP_NOT_BONDED;
+            }
+            break;
+
+        default:
+            rsp_code = DFU_RSP_OP_CODE_NOT_SUPPORTED;
+            break;
+    }
+
+    // Report back in case of error
+    if (rsp_code != DFU_RSP_SUCCESS)
+    {
+        ret = ble_dfu_buttonless_resp_send((ble_dfu_buttonless_op_code_t)p_evt_write->data[0],
+                                            rsp_code);
+
+        if (ret != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+        }
+
+        // Report the error to the main application
+        mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+    }
+}
+
+
+uint32_t ble_dfu_buttonless_bootloader_start_prepare(void)
+{
+    NRF_LOG_DEBUG("In ble_dfu_buttonless_bootloader_start_prepare");
+
+    // Indicate to main app that DFU mode is starting.
+    // This event can be used to let the device take down any connection to
+    // bonded devices.
+    mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_PREPARE);
+
+    // Store the number of peers for which Peer Manager is expected to successfully write events.
+    mp_dfu->peers_count = peer_id_n_ids();
+
+    // Set local database changed to get Service Changed indication for all bonded peers
+    // on next bootup (either because of a successful or aborted DFU).
+    gscm_local_database_has_changed();
+
+    return NRF_SUCCESS;
+}
+
+#endif // NRF_DFU_BLE_BUTTONLESS_SUPPORTS_BONDS
+

components/ble/ble_services/ble_dfu/ble_dfu_unbonded.c

@@ -0,0 +1,299 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include "nrf_dfu_ble_svci_bond_sharing.h"
+#include "nordic_common.h"
+#include "nrf_error.h"
+#include "ble_dfu.h"
+#include "nrf_log.h"
+#include "nrf_sdh_soc.h"
+
+#if (!NRF_DFU_BLE_BUTTONLESS_SUPPORTS_BONDS)
+
+#define NRF_DFU_ADV_NAME_MAX_LENGTH     (20)
+
+
+void ble_dfu_buttonless_on_sys_evt(uint32_t, void * );
+uint32_t nrf_dfu_svci_vector_table_set(void);
+uint32_t nrf_dfu_svci_vector_table_unset(void);
+
+/**@brief Define functions for async interface to set new advertisement name for DFU mode.  */
+NRF_SVCI_ASYNC_FUNC_DEFINE(NRF_DFU_SVCI_SET_ADV_NAME, nrf_dfu_set_adv_name, nrf_dfu_adv_name_t);
+
+// Register SoC observer for the Buttonless Secure DFU service
+NRF_SDH_SOC_OBSERVER(m_dfu_buttonless_soc_obs, BLE_DFU_SOC_OBSERVER_PRIO, ble_dfu_buttonless_on_sys_evt, NULL);
+
+ble_dfu_buttonless_t      * mp_dfu = NULL;
+static nrf_dfu_adv_name_t   m_adv_name;
+
+
+/**@brief Function for setting an advertisement name.
+ *
+ * @param[in]   adv_name    The new advertisement name.
+ *
+ * @retval NRF_SUCCESS      Advertisement name was successfully set.
+ * @retval DFU_RSP_BUSY     Advertisement name was not set because of an ongoing operation.
+ * @retval Any other errors from the SVCI interface call.
+ */
+static uint32_t set_adv_name(nrf_dfu_adv_name_t * p_adv_name)
+{
+    uint32_t err_code;
+
+    if (mp_dfu->is_waiting_for_svci)
+    {
+        return DFU_RSP_BUSY;
+    }
+
+    err_code = nrf_dfu_set_adv_name(p_adv_name);
+    if (err_code == NRF_SUCCESS)
+    {
+        // The request was accepted.
+        mp_dfu->is_waiting_for_svci = true;
+    }
+    else if (err_code == NRF_ERROR_FORBIDDEN)
+    {
+        NRF_LOG_ERROR("The bootloader has write protected its settings page. This prohibits setting the advertising name. "\
+                      "The bootloader must be compiled with NRF_BL_SETTINGS_PAGE_PROTECT=0 to allow setting the advertising name.");
+    }
+
+    return err_code;
+}
+
+
+/**@brief Function for entering the bootloader.
+ */
+static uint32_t enter_bootloader()
+{
+    uint32_t err_code;
+
+    if (mp_dfu->is_waiting_for_svci)
+    {
+        // We have an ongoing async operation. Entering bootloader mode is not possible at this time.
+        err_code = ble_dfu_buttonless_resp_send(DFU_OP_ENTER_BOOTLOADER, DFU_RSP_BUSY);
+        if (err_code != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+        }
+        return NRF_SUCCESS;
+    }
+
+    // Set the flag indicating that we expect DFU mode.
+    // This will be handled on acknowledgement of the characteristic indication.
+    mp_dfu->is_waiting_for_reset = true;
+
+    err_code = ble_dfu_buttonless_resp_send(DFU_OP_ENTER_BOOTLOADER, DFU_RSP_SUCCESS);
+    if (err_code != NRF_SUCCESS)
+    {
+        mp_dfu->is_waiting_for_reset = false;
+    }
+
+    return err_code;
+}
+
+
+uint32_t ble_dfu_buttonless_backend_init(ble_dfu_buttonless_t * p_dfu)
+{
+    VERIFY_PARAM_NOT_NULL(p_dfu);
+
+    mp_dfu = p_dfu;
+
+    return NRF_SUCCESS;
+}
+
+
+uint32_t ble_dfu_buttonless_async_svci_init(void)
+{
+    uint32_t ret_val;
+
+    ret_val = nrf_dfu_svci_vector_table_set();
+    VERIFY_SUCCESS(ret_val);
+
+    ret_val = nrf_dfu_set_adv_name_init();
+    VERIFY_SUCCESS(ret_val);
+
+    ret_val = nrf_dfu_svci_vector_table_unset();
+
+    return ret_val;
+}
+
+
+void ble_dfu_buttonless_on_sys_evt(uint32_t sys_evt, void * p_context)
+{
+    uint32_t err_code;
+
+    if (!nrf_dfu_set_adv_name_is_initialized())
+    {
+        return;
+    }
+
+    err_code = nrf_dfu_set_adv_name_on_sys_evt(sys_evt);
+    if (err_code == NRF_ERROR_INVALID_STATE)
+    {
+        // The system event is not from an operation started by buttonless DFU.
+        // No action is taken, and nothing is reported.
+    }
+    else if (err_code == NRF_SUCCESS)
+    {
+        // The async operation is finished.
+        // Set the flag indicating that we are waiting for indication response
+        // to activate the reset.
+        mp_dfu->is_waiting_for_svci = false;
+
+        // Report back the positive response
+        err_code = ble_dfu_buttonless_resp_send(DFU_OP_SET_ADV_NAME, DFU_RSP_SUCCESS);
+        if (err_code != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+        }
+    }
+    else
+    {
+        // Invalid error code reported back.
+        mp_dfu->is_waiting_for_svci = false;
+
+        err_code = ble_dfu_buttonless_resp_send(DFU_OP_SET_ADV_NAME, DFU_RSP_BUSY);
+        if (err_code != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+        }
+
+        // Report the failure to enter DFU mode
+        mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+    }
+}
+
+
+uint32_t ble_dfu_buttonless_char_add(ble_dfu_buttonless_t * p_dfu)
+{
+    ble_add_char_params_t add_char_params;
+
+    memset(&add_char_params, 0, sizeof(add_char_params));
+    add_char_params.uuid                = BLE_DFU_BUTTONLESS_CHAR_UUID;
+    add_char_params.uuid_type           = p_dfu->uuid_type;
+    add_char_params.char_props.indicate = 1;
+    add_char_params.char_props.write    = 1;
+    add_char_params.is_defered_write    = true;
+    add_char_params.is_var_len          = true;
+    add_char_params.max_len             = BLE_GATT_ATT_MTU_DEFAULT;
+
+    add_char_params.cccd_write_access = SEC_OPEN;
+    add_char_params.write_access      = SEC_OPEN;
+    add_char_params.read_access       = SEC_OPEN;
+
+    return characteristic_add(p_dfu->service_handle, &add_char_params, &p_dfu->control_point_char);
+}
+
+
+void ble_dfu_buttonless_on_ctrl_pt_write(ble_gatts_evt_write_t const * p_evt_write)
+{
+    uint32_t err_code;
+    ble_dfu_buttonless_rsp_code_t rsp_code = DFU_RSP_OPERATION_FAILED;
+
+    // Start executing the control point write operation
+    /*lint -e415 -e416 -save "Out of bounds access"*/
+    switch (p_evt_write->data[0])
+    {
+        case DFU_OP_ENTER_BOOTLOADER:
+            err_code = enter_bootloader();
+            if (err_code == NRF_SUCCESS)
+            {
+                rsp_code = DFU_RSP_SUCCESS;
+            }
+            else if (err_code == NRF_ERROR_BUSY)
+            {
+                rsp_code = DFU_RSP_BUSY;
+            }
+            break;
+
+        case DFU_OP_SET_ADV_NAME:
+            if(    (p_evt_write->data[1] > NRF_DFU_ADV_NAME_MAX_LENGTH)
+                || (p_evt_write->data[1] == 0))
+            {
+                // New advertisement name too short or too long.
+                rsp_code = DFU_RSP_ADV_NAME_INVALID;
+            }
+            else
+            {
+                memcpy(m_adv_name.name, &p_evt_write->data[2], p_evt_write->data[1]);
+                m_adv_name.len = p_evt_write->data[1];
+                err_code = set_adv_name(&m_adv_name);
+                if (err_code == NRF_SUCCESS)
+                {
+                    rsp_code = DFU_RSP_SUCCESS;
+                }
+            }
+            break;
+
+        default:
+            rsp_code = DFU_RSP_OP_CODE_NOT_SUPPORTED;
+            break;
+    }
+    /*lint -restore*/
+
+
+    // Report back in case of error
+    if (rsp_code != DFU_RSP_SUCCESS)
+    {
+        err_code = ble_dfu_buttonless_resp_send((ble_dfu_buttonless_op_code_t)p_evt_write->data[0], rsp_code);
+        if (err_code != NRF_SUCCESS)
+        {
+            mp_dfu->evt_handler(BLE_DFU_EVT_RESPONSE_SEND_ERROR);
+
+        }
+        // Report the error to the main application
+        mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED);
+    }
+}
+
+uint32_t ble_dfu_buttonless_bootloader_start_prepare(void)
+{
+    uint32_t err_code;
+
+    // Indicate to main app that DFU mode is starting.
+    mp_dfu->evt_handler(BLE_DFU_EVT_BOOTLOADER_ENTER_PREPARE);
+
+    err_code = ble_dfu_buttonless_bootloader_start_finalize();
+    return err_code;
+}
+
+#endif // NRF_DFU_BOTTONLESS_SUPPORT_BOND

components/ble/ble_services/ble_nus/ble_nus.c

@@ -0,0 +1,354 @@
+/**
+ * Copyright (c) 2012 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(BLE_NUS)
+#include "ble.h"
+#include "ble_nus.h"
+#include "ble_srv_common.h"
+
+#define NRF_LOG_MODULE_NAME ble_nus
+#if BLE_NUS_CONFIG_LOG_ENABLED
+#define NRF_LOG_LEVEL       BLE_NUS_CONFIG_LOG_LEVEL
+#define NRF_LOG_INFO_COLOR  BLE_NUS_CONFIG_INFO_COLOR
+#define NRF_LOG_DEBUG_COLOR BLE_NUS_CONFIG_DEBUG_COLOR
+#else // BLE_NUS_CONFIG_LOG_ENABLED
+#define NRF_LOG_LEVEL       0
+#endif // BLE_NUS_CONFIG_LOG_ENABLED
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+
+#define BLE_UUID_NUS_TX_CHARACTERISTIC 0x0003               /**< The UUID of the TX Characteristic. */
+#define BLE_UUID_NUS_RX_CHARACTERISTIC 0x0002               /**< The UUID of the RX Characteristic. */
+
+#define BLE_NUS_MAX_RX_CHAR_LEN        BLE_NUS_MAX_DATA_LEN /**< Maximum length of the RX Characteristic (in bytes). */
+#define BLE_NUS_MAX_TX_CHAR_LEN        BLE_NUS_MAX_DATA_LEN /**< Maximum length of the TX Characteristic (in bytes). */
+
+#define NUS_BASE_UUID                  {{0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5, 0x00, 0x00, 0x40, 0x6E}} /**< Used vendor specific UUID. */
+
+
+/**@brief Function for handling the @ref BLE_GAP_EVT_CONNECTED event from the SoftDevice.
+ *
+ * @param[in] p_nus     Nordic UART Service structure.
+ * @param[in] p_ble_evt Pointer to the event received from BLE stack.
+ */
+static void on_connect(ble_nus_t * p_nus, ble_evt_t const * p_ble_evt)
+{
+    ret_code_t                 err_code;
+    ble_nus_evt_t              evt;
+    ble_gatts_value_t          gatts_val;
+    uint8_t                    cccd_value[2];
+    ble_nus_client_context_t * p_client = NULL;
+
+    err_code = blcm_link_ctx_get(p_nus->p_link_ctx_storage,
+                                 p_ble_evt->evt.gap_evt.conn_handle,
+                                 (void *) &p_client);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Link context for 0x%02X connection handle could not be fetched.",
+                      p_ble_evt->evt.gap_evt.conn_handle);
+    }
+
+    /* Check the hosts CCCD value to inform of readiness to send data using the RX characteristic */
+    memset(&gatts_val, 0, sizeof(ble_gatts_value_t));
+    gatts_val.p_value = cccd_value;
+    gatts_val.len     = sizeof(cccd_value);
+    gatts_val.offset  = 0;
+
+    err_code = sd_ble_gatts_value_get(p_ble_evt->evt.gap_evt.conn_handle,
+                                      p_nus->tx_handles.cccd_handle,
+                                      &gatts_val);
+
+    if ((err_code == NRF_SUCCESS)     &&
+        (p_nus->data_handler != NULL) &&
+        ble_srv_is_notification_enabled(gatts_val.p_value))
+    {
+        if (p_client != NULL)
+        {
+            p_client->is_notification_enabled = true;
+        }
+
+        memset(&evt, 0, sizeof(ble_nus_evt_t));
+        evt.type        = BLE_NUS_EVT_COMM_STARTED;
+        evt.p_nus       = p_nus;
+        evt.conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+        evt.p_link_ctx  = p_client;
+
+        p_nus->data_handler(&evt);
+    }
+}
+
+
+/**@brief Function for handling the @ref BLE_GATTS_EVT_WRITE event from the SoftDevice.
+ *
+ * @param[in] p_nus     Nordic UART Service structure.
+ * @param[in] p_ble_evt Pointer to the event received from BLE stack.
+ */
+static void on_write(ble_nus_t * p_nus, ble_evt_t const * p_ble_evt)
+{
+    ret_code_t                    err_code;
+    ble_nus_evt_t                 evt;
+    ble_nus_client_context_t    * p_client;
+    ble_gatts_evt_write_t const * p_evt_write = &p_ble_evt->evt.gatts_evt.params.write;
+
+    err_code = blcm_link_ctx_get(p_nus->p_link_ctx_storage,
+                                 p_ble_evt->evt.gatts_evt.conn_handle,
+                                 (void *) &p_client);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Link context for 0x%02X connection handle could not be fetched.",
+                      p_ble_evt->evt.gatts_evt.conn_handle);
+    }
+
+    memset(&evt, 0, sizeof(ble_nus_evt_t));
+    evt.p_nus       = p_nus;
+    evt.conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
+    evt.p_link_ctx  = p_client;
+
+    if ((p_evt_write->handle == p_nus->tx_handles.cccd_handle) &&
+        (p_evt_write->len == 2))
+    {
+        if (p_client != NULL)
+        {
+            if (ble_srv_is_notification_enabled(p_evt_write->data))
+            {
+                p_client->is_notification_enabled = true;
+                evt.type                          = BLE_NUS_EVT_COMM_STARTED;
+            }
+            else
+            {
+                p_client->is_notification_enabled = false;
+                evt.type                          = BLE_NUS_EVT_COMM_STOPPED;
+            }
+
+            if (p_nus->data_handler != NULL)
+            {
+                p_nus->data_handler(&evt);
+            }
+
+        }
+    }
+    else if ((p_evt_write->handle == p_nus->rx_handles.value_handle) &&
+             (p_nus->data_handler != NULL))
+    {
+        evt.type                  = BLE_NUS_EVT_RX_DATA;
+        evt.params.rx_data.p_data = p_evt_write->data;
+        evt.params.rx_data.length = p_evt_write->len;
+
+        p_nus->data_handler(&evt);
+    }
+    else
+    {
+        // Do Nothing. This event is not relevant for this service.
+    }
+}
+
+
+/**@brief Function for handling the @ref BLE_GATTS_EVT_HVN_TX_COMPLETE event from the SoftDevice.
+ *
+ * @param[in] p_nus     Nordic UART Service structure.
+ * @param[in] p_ble_evt Pointer to the event received from BLE stack.
+ */
+static void on_hvx_tx_complete(ble_nus_t * p_nus, ble_evt_t const * p_ble_evt)
+{
+    ret_code_t                 err_code;
+    ble_nus_evt_t              evt;
+    ble_nus_client_context_t * p_client;
+
+    err_code = blcm_link_ctx_get(p_nus->p_link_ctx_storage,
+                                 p_ble_evt->evt.gatts_evt.conn_handle,
+                                 (void *) &p_client);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Link context for 0x%02X connection handle could not be fetched.",
+                      p_ble_evt->evt.gatts_evt.conn_handle);
+        return;
+    }
+
+    if ((p_client->is_notification_enabled) && (p_nus->data_handler != NULL))
+    {
+        memset(&evt, 0, sizeof(ble_nus_evt_t));
+        evt.type        = BLE_NUS_EVT_TX_RDY;
+        evt.p_nus       = p_nus;
+        evt.conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
+        evt.p_link_ctx  = p_client;
+
+        p_nus->data_handler(&evt);
+    }
+}
+
+
+void ble_nus_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    if ((p_context == NULL) || (p_ble_evt == NULL))
+    {
+        return;
+    }
+
+    ble_nus_t * p_nus = (ble_nus_t *)p_context;
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            on_connect(p_nus, p_ble_evt);
+            break;
+
+        case BLE_GATTS_EVT_WRITE:
+            on_write(p_nus, p_ble_evt);
+            break;
+
+        case BLE_GATTS_EVT_HVN_TX_COMPLETE:
+            on_hvx_tx_complete(p_nus, p_ble_evt);
+            break;
+
+        default:
+            // No implementation needed.
+            break;
+    }
+}
+
+
+uint32_t ble_nus_init(ble_nus_t * p_nus, ble_nus_init_t const * p_nus_init)
+{
+    ret_code_t            err_code;
+    ble_uuid_t            ble_uuid;
+    ble_uuid128_t         nus_base_uuid = NUS_BASE_UUID;
+    ble_add_char_params_t add_char_params;
+
+    VERIFY_PARAM_NOT_NULL(p_nus);
+    VERIFY_PARAM_NOT_NULL(p_nus_init);
+
+    // Initialize the service structure.
+    p_nus->data_handler = p_nus_init->data_handler;
+
+    /**@snippet [Adding proprietary Service to the SoftDevice] */
+    // Add a custom base UUID.
+    err_code = sd_ble_uuid_vs_add(&nus_base_uuid, &p_nus->uuid_type);
+    VERIFY_SUCCESS(err_code);
+
+    ble_uuid.type = p_nus->uuid_type;
+    ble_uuid.uuid = BLE_UUID_NUS_SERVICE;
+
+    // Add the service.
+    err_code = sd_ble_gatts_service_add(BLE_GATTS_SRVC_TYPE_PRIMARY,
+                                        &ble_uuid,
+                                        &p_nus->service_handle);
+    /**@snippet [Adding proprietary Service to the SoftDevice] */
+    VERIFY_SUCCESS(err_code);
+
+    // Add the RX Characteristic.
+    memset(&add_char_params, 0, sizeof(add_char_params));
+    add_char_params.uuid                     = BLE_UUID_NUS_RX_CHARACTERISTIC;
+    add_char_params.uuid_type                = p_nus->uuid_type;
+    add_char_params.max_len                  = BLE_NUS_MAX_RX_CHAR_LEN;
+    add_char_params.init_len                 = sizeof(uint8_t);
+    add_char_params.is_var_len               = true;
+    add_char_params.char_props.write         = 1;
+    add_char_params.char_props.write_wo_resp = 1;
+
+    add_char_params.read_access  = SEC_OPEN;
+    add_char_params.write_access = SEC_OPEN;
+
+    err_code = characteristic_add(p_nus->service_handle, &add_char_params, &p_nus->rx_handles);
+    if (err_code != NRF_SUCCESS)
+    {
+        return err_code;
+    }
+
+    // Add the TX Characteristic.
+    /**@snippet [Adding proprietary characteristic to the SoftDevice] */
+    memset(&add_char_params, 0, sizeof(add_char_params));
+    add_char_params.uuid              = BLE_UUID_NUS_TX_CHARACTERISTIC;
+    add_char_params.uuid_type         = p_nus->uuid_type;
+    add_char_params.max_len           = BLE_NUS_MAX_TX_CHAR_LEN;
+    add_char_params.init_len          = sizeof(uint8_t);
+    add_char_params.is_var_len        = true;
+    add_char_params.char_props.notify = 1;
+
+    add_char_params.read_access       = SEC_OPEN;
+    add_char_params.write_access      = SEC_OPEN;
+    add_char_params.cccd_write_access = SEC_OPEN;
+
+    return characteristic_add(p_nus->service_handle, &add_char_params, &p_nus->tx_handles);
+    /**@snippet [Adding proprietary characteristic to the SoftDevice] */
+}
+
+
+uint32_t ble_nus_data_send(ble_nus_t * p_nus,
+                           uint8_t   * p_data,
+                           uint16_t  * p_length,
+                           uint16_t    conn_handle)
+{
+    ret_code_t                 err_code;
+    ble_gatts_hvx_params_t     hvx_params;
+    ble_nus_client_context_t * p_client;
+
+    VERIFY_PARAM_NOT_NULL(p_nus);
+
+    err_code = blcm_link_ctx_get(p_nus->p_link_ctx_storage, conn_handle, (void *) &p_client);
+    VERIFY_SUCCESS(err_code);
+
+    if ((conn_handle == BLE_CONN_HANDLE_INVALID) || (p_client == NULL))
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    if (!p_client->is_notification_enabled)
+    {
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    if (*p_length > BLE_NUS_MAX_DATA_LEN)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    memset(&hvx_params, 0, sizeof(hvx_params));
+
+    hvx_params.handle = p_nus->tx_handles.value_handle;
+    hvx_params.p_data = p_data;
+    hvx_params.p_len  = p_length;
+    hvx_params.type   = BLE_GATT_HVX_NOTIFICATION;
+
+    return sd_ble_gatts_hvx(conn_handle, &hvx_params);
+}
+
+
+#endif // NRF_MODULE_ENABLED(BLE_NUS)

components/ble/ble_services/ble_nus/ble_nus.h

@@ -0,0 +1,244 @@
+ /**
+ * Copyright (c) 2012 - 2021, 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_nus Nordic UART Service
+ * @{
+ * @ingroup  ble_sdk_srv
+ * @brief    Nordic UART Service implementation.
+ *
+ * @details The Nordic UART Service is a simple GATT-based service with TX and RX characteristics.
+ *          Data received from the peer is passed to the application, and the data received
+ *          from the application of this service is sent to the peer as Handle Value
+ *          Notifications. This module demonstrates how to implement a custom GATT-based
+ *          service and characteristics using the SoftDevice. The service
+ *          is used by the application to send and receive ASCII text strings to and from the
+ *          peer.
+ *
+ * @note    The application must register this module as BLE event observer using the
+ *          NRF_SDH_BLE_OBSERVER macro. Example:
+ *          @code
+ *              ble_nus_t instance;
+ *              NRF_SDH_BLE_OBSERVER(anything, BLE_NUS_BLE_OBSERVER_PRIO,
+ *                                   ble_nus_on_ble_evt, &instance);
+ *          @endcode
+ */
+#ifndef BLE_NUS_H__
+#define BLE_NUS_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "sdk_config.h"
+#include "ble.h"
+#include "ble_srv_common.h"
+#include "nrf_sdh_ble.h"
+#include "ble_link_ctx_manager.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief   Macro for defining a ble_nus instance.
+ *
+ * @param     _name            Name of the instance.
+ * @param[in] _nus_max_clients Maximum number of NUS clients connected at a time.
+ * @hideinitializer
+ */
+#define BLE_NUS_DEF(_name, _nus_max_clients)                      \
+    BLE_LINK_CTX_MANAGER_DEF(CONCAT_2(_name, _link_ctx_storage),  \
+                             (_nus_max_clients),                  \
+                             sizeof(ble_nus_client_context_t));   \
+    static ble_nus_t _name =                                      \
+    {                                                             \
+        .p_link_ctx_storage = &CONCAT_2(_name, _link_ctx_storage) \
+    };                                                            \
+    NRF_SDH_BLE_OBSERVER(_name ## _obs,                           \
+                         BLE_NUS_BLE_OBSERVER_PRIO,               \
+                         ble_nus_on_ble_evt,                      \
+                         &_name)
+
+#define BLE_UUID_NUS_SERVICE 0x0001 /**< The UUID of the Nordic UART Service. */
+
+#define OPCODE_LENGTH        1
+#define HANDLE_LENGTH        2
+
+/**@brief   Maximum length of data (in bytes) that can be transmitted to the peer by the Nordic UART service module. */
+#if defined(NRF_SDH_BLE_GATT_MAX_MTU_SIZE) && (NRF_SDH_BLE_GATT_MAX_MTU_SIZE != 0)
+    #define BLE_NUS_MAX_DATA_LEN (NRF_SDH_BLE_GATT_MAX_MTU_SIZE - OPCODE_LENGTH - HANDLE_LENGTH)
+#else
+    #define BLE_NUS_MAX_DATA_LEN (BLE_GATT_MTU_SIZE_DEFAULT - OPCODE_LENGTH - HANDLE_LENGTH)
+    #warning NRF_SDH_BLE_GATT_MAX_MTU_SIZE is not defined.
+#endif
+
+
+/**@brief   Nordic UART Service event types. */
+typedef enum
+{
+    BLE_NUS_EVT_RX_DATA,      /**< Data received. */
+    BLE_NUS_EVT_TX_RDY,       /**< Service is ready to accept new data to be transmitted. */
+    BLE_NUS_EVT_COMM_STARTED, /**< Notification has been enabled. */
+    BLE_NUS_EVT_COMM_STOPPED, /**< Notification has been disabled. */
+} ble_nus_evt_type_t;
+
+
+/* Forward declaration of the ble_nus_t type. */
+typedef struct ble_nus_s ble_nus_t;
+
+
+/**@brief   Nordic UART Service @ref BLE_NUS_EVT_RX_DATA event data.
+ *
+ * @details This structure is passed to an event when @ref BLE_NUS_EVT_RX_DATA occurs.
+ */
+typedef struct
+{
+    uint8_t const * p_data; /**< A pointer to the buffer with received data. */
+    uint16_t        length; /**< Length of received data. */
+} ble_nus_evt_rx_data_t;
+
+
+/**@brief Nordic UART Service client context structure.
+ *
+ * @details This structure contains state context related to hosts.
+ */
+typedef struct
+{
+    bool is_notification_enabled; /**< Variable to indicate if the peer has enabled notification of the RX characteristic.*/
+} ble_nus_client_context_t;
+
+
+/**@brief   Nordic UART Service event structure.
+ *
+ * @details This structure is passed to an event coming from service.
+ */
+typedef struct
+{
+    ble_nus_evt_type_t         type;        /**< Event type. */
+    ble_nus_t                * p_nus;       /**< A pointer to the instance. */
+    uint16_t                   conn_handle; /**< Connection handle. */
+    ble_nus_client_context_t * p_link_ctx;  /**< A pointer to the link context. */
+    union
+    {
+        ble_nus_evt_rx_data_t rx_data; /**< @ref BLE_NUS_EVT_RX_DATA event data. */
+    } params;
+} ble_nus_evt_t;
+
+
+/**@brief Nordic UART Service event handler type. */
+typedef void (* ble_nus_data_handler_t) (ble_nus_evt_t * p_evt);
+
+
+/**@brief   Nordic UART Service initialization structure.
+ *
+ * @details This structure contains the initialization information for the service. The application
+ * must fill this structure and pass it to the service using the @ref ble_nus_init
+ *          function.
+ */
+typedef struct
+{
+    ble_nus_data_handler_t data_handler; /**< Event handler to be called for handling received data. */
+} ble_nus_init_t;
+
+
+/**@brief   Nordic UART Service structure.
+ *
+ * @details This structure contains status information related to the service.
+ */
+struct ble_nus_s
+{
+    uint8_t                         uuid_type;          /**< UUID type for Nordic UART Service Base UUID. */
+    uint16_t                        service_handle;     /**< Handle of Nordic UART Service (as provided by the SoftDevice). */
+    ble_gatts_char_handles_t        tx_handles;         /**< Handles related to the TX characteristic (as provided by the SoftDevice). */
+    ble_gatts_char_handles_t        rx_handles;         /**< Handles related to the RX characteristic (as provided by the SoftDevice). */
+    blcm_link_ctx_storage_t * const p_link_ctx_storage; /**< Pointer to link context storage with handles of all current connections and its context. */
+    ble_nus_data_handler_t          data_handler;       /**< Event handler to be called for handling received data. */
+};
+
+
+/**@brief   Function for initializing the Nordic UART Service.
+ *
+ * @param[out] p_nus      Nordic UART Service structure. This structure must be supplied
+ *                        by the application. It is initialized by this function and will
+ *                        later be used to identify this particular service instance.
+ * @param[in] p_nus_init  Information needed to initialize the service.
+ *
+ * @retval NRF_SUCCESS If the service was successfully initialized. Otherwise, an error code is returned.
+ * @retval NRF_ERROR_NULL If either of the pointers p_nus or p_nus_init is NULL.
+ */
+uint32_t ble_nus_init(ble_nus_t * p_nus, ble_nus_init_t const * p_nus_init);
+
+
+/**@brief   Function for handling the Nordic UART Service's BLE events.
+ *
+ * @details The Nordic UART Service expects the application to call this function each time an
+ * event is received from the SoftDevice. This function processes the event if it
+ * is relevant and calls the Nordic UART Service event handler of the
+ * application if necessary.
+ *
+ * @param[in] p_ble_evt     Event received from the SoftDevice.
+ * @param[in] p_context     Nordic UART Service structure.
+ */
+void ble_nus_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context);
+
+
+/**@brief   Function for sending a data to the peer.
+ *
+ * @details This function sends the input string as an RX characteristic notification to the
+ *          peer.
+ *
+ * @param[in]     p_nus       Pointer to the Nordic UART Service structure.
+ * @param[in]     p_data      String to be sent.
+ * @param[in,out] p_length    Pointer Length of the string. Amount of sent bytes.
+ * @param[in]     conn_handle Connection Handle of the destination client.
+ *
+ * @retval NRF_SUCCESS If the string was sent successfully. Otherwise, an error code is returned.
+ */
+uint32_t ble_nus_data_send(ble_nus_t * p_nus,
+                           uint8_t   * p_data,
+                           uint16_t  * p_length,
+                           uint16_t    conn_handle);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_NUS_H__
+
+/** @} */

components/ble/common/ble_advdata.c

@@ -0,0 +1,838 @@
+/**
+ * Copyright (c) 2012 - 2021, 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.
+ *
+ */
+#include "ble_advdata.h"
+#include "ble_gap.h"
+#include "ble_srv_common.h"
+#include "sdk_common.h"
+
+// NOTE: For now, Security Manager Out of Band Flags (OOB) are omitted from the advertising data.
+
+
+// Types of LE Bluetooth Device Address AD type
+#define AD_TYPE_BLE_DEVICE_ADDR_TYPE_PUBLIC 0UL
+#define AD_TYPE_BLE_DEVICE_ADDR_TYPE_RANDOM 1UL
+
+#define UUID16_SIZE     2   /**< Size of 16 bit UUID. */
+#define UUID32_SIZE     4   /**< Size of 32 bit UUID. */
+#define UUID128_SIZE    16  /**< Size of 128 bit UUID. */
+
+#define N_AD_TYPES      2   /**< The number of Advertising data types to search for at a time. */
+
+
+static ret_code_t ble_device_addr_encode(uint8_t  * p_encoded_data,
+                                         uint16_t * p_offset,
+                                         uint16_t   max_size)
+{
+    ret_code_t     err_code;
+    ble_gap_addr_t device_addr;
+
+    // Check for buffer overflow.
+    if (((*p_offset) + AD_TYPE_BLE_DEVICE_ADDR_SIZE) > max_size)
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Get BLE address.
+    err_code = sd_ble_gap_addr_get(&device_addr);
+    VERIFY_SUCCESS(err_code);
+
+    // Encode LE Bluetooth Device Address.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE +
+                                               AD_TYPE_BLE_DEVICE_ADDR_DATA_SIZE);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_LE_BLUETOOTH_DEVICE_ADDRESS;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+    memcpy(&p_encoded_data[*p_offset], &device_addr.addr[0], BLE_GAP_ADDR_LEN);
+    *p_offset                 += BLE_GAP_ADDR_LEN;
+    if (BLE_GAP_ADDR_TYPE_PUBLIC == device_addr.addr_type)
+    {
+        p_encoded_data[*p_offset] = AD_TYPE_BLE_DEVICE_ADDR_TYPE_PUBLIC;
+    }
+    else
+    {
+        p_encoded_data[*p_offset] = AD_TYPE_BLE_DEVICE_ADDR_TYPE_RANDOM;
+    }
+    *p_offset += AD_TYPE_BLE_DEVICE_ADDR_TYPE_SIZE;
+
+    return NRF_SUCCESS;
+}
+
+static ret_code_t name_encode(const ble_advdata_t * p_advdata,
+                              uint8_t             * p_encoded_data,
+                              uint16_t            * p_offset,
+                              uint16_t              max_size)
+{
+    ret_code_t err_code;
+    uint16_t   rem_adv_data_len;
+    uint16_t   actual_length;
+    uint8_t    adv_data_format;
+
+
+    // Validate parameters
+    if ((BLE_ADVDATA_SHORT_NAME == p_advdata->name_type) && (0 == p_advdata->short_name_len))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    // Check for buffer overflow.
+    if ( (((*p_offset) + AD_DATA_OFFSET) > max_size) ||
+         ( (BLE_ADVDATA_SHORT_NAME == p_advdata->name_type) &&
+           (((*p_offset) + AD_DATA_OFFSET + p_advdata->short_name_len) > max_size)))
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    rem_adv_data_len = max_size - (*p_offset) - AD_DATA_OFFSET;
+    actual_length    = rem_adv_data_len;
+
+    // Get GAP device name and length
+    err_code = sd_ble_gap_device_name_get(&p_encoded_data[(*p_offset) + AD_DATA_OFFSET],
+                                          &actual_length);
+    VERIFY_SUCCESS(err_code);
+
+    // Check if device intend to use short name and it can fit available data size.
+    // If the name is shorter than the preferred short name length then it is no longer 
+    // a short name and is in fact the complete name of the device.
+    if (((p_advdata->name_type == BLE_ADVDATA_FULL_NAME) || 
+        (actual_length <= p_advdata->short_name_len)) && 
+        (actual_length <= rem_adv_data_len))
+    {
+        // Complete device name can fit, setting Complete Name in Adv Data.
+        adv_data_format = BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME;
+    }
+    else
+    {
+        // Else short name needs to be used. Or application has requested use of short name.
+        adv_data_format = BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME;
+
+        // If application has set a preference on the short name size, it needs to be considered,
+        // else fit what can be fit.
+        if ((BLE_ADVDATA_SHORT_NAME == p_advdata->name_type) &&
+                (p_advdata->short_name_len <= rem_adv_data_len))
+        {
+            // Short name fits available size.
+            actual_length = p_advdata->short_name_len;
+        }
+        // Else whatever can fit the data buffer will be packed.
+        else
+        {
+            actual_length = rem_adv_data_len;
+        }
+    }
+
+    // There is only 1 byte intended to encode length which is (actual_length + AD_TYPE_FIELD_SIZE)
+    if (actual_length > (0x00FF - AD_TYPE_FIELD_SIZE))
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Complete name field in encoded data.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE + actual_length);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = adv_data_format;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+    *p_offset                 += actual_length;
+
+    return NRF_SUCCESS;
+}
+
+
+static ret_code_t appearance_encode(uint8_t  * p_encoded_data,
+                                    uint16_t * p_offset,
+                                    uint16_t   max_size)
+{
+    ret_code_t err_code;
+    uint16_t   appearance;
+
+    // Check for buffer overflow.
+    if (((*p_offset) + AD_TYPE_APPEARANCE_SIZE) > max_size)
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Get GAP appearance field.
+    err_code = sd_ble_gap_appearance_get(&appearance);
+    VERIFY_SUCCESS(err_code);
+
+    // Encode Length, AD Type and Appearance.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE + AD_TYPE_APPEARANCE_DATA_SIZE);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_APPEARANCE;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+    *p_offset                 += uint16_encode(appearance, &p_encoded_data[*p_offset]);
+
+    return NRF_SUCCESS;
+}
+
+static ret_code_t flags_encode(int8_t     flags,
+                               uint8_t  * p_encoded_data,
+                               uint16_t * p_offset,
+                               uint16_t   max_size)
+{
+    // Check for buffer overflow.
+    if (((*p_offset) + AD_TYPE_FLAGS_SIZE) > max_size)
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Encode flags.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE + AD_TYPE_FLAGS_DATA_SIZE);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_FLAGS;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = flags;
+    *p_offset                 += AD_TYPE_FLAGS_DATA_SIZE;
+
+    return NRF_SUCCESS;
+}
+
+static ret_code_t tx_power_level_encode(int8_t     tx_power_level,
+                                        uint8_t  * p_encoded_data,
+                                        uint16_t * p_offset,
+                                        uint16_t   max_size)
+{
+    // Check for buffer overflow.
+    if (((*p_offset) + AD_TYPE_TX_POWER_LEVEL_SIZE) > max_size)
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Encode TX Power Level.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE +
+                                                  AD_TYPE_TX_POWER_LEVEL_DATA_SIZE);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_TX_POWER_LEVEL;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = tx_power_level;
+    *p_offset                 += AD_TYPE_TX_POWER_LEVEL_DATA_SIZE;
+
+    return NRF_SUCCESS;
+}
+
+
+static ret_code_t uuid_list_sized_encode(const ble_advdata_uuid_list_t * p_uuid_list,
+                                         uint8_t                         adv_type,
+                                         uint8_t                         uuid_size,
+                                         uint8_t                       * p_encoded_data,
+                                         uint16_t                      * p_offset,
+                                         uint16_t                        max_size)
+{
+    int      i;
+    bool     is_heading_written = false;
+    uint16_t start_pos          = *p_offset;
+    uint16_t length;
+
+    for (i = 0; i < p_uuid_list->uuid_cnt; i++)
+    {
+        ret_code_t err_code;
+        uint8_t    encoded_size;
+        ble_uuid_t uuid = p_uuid_list->p_uuids[i];
+
+        // Find encoded uuid size.
+        err_code = sd_ble_uuid_encode(&uuid, &encoded_size, NULL);
+        VERIFY_SUCCESS(err_code);
+
+        // Check size.
+        if (encoded_size == uuid_size)
+        {
+            uint8_t heading_bytes = (is_heading_written) ? 0 : AD_DATA_OFFSET;
+
+            // Check for buffer overflow
+            if (((*p_offset) + encoded_size + heading_bytes) > max_size)
+            {
+                return NRF_ERROR_DATA_SIZE;
+            }
+
+            if (!is_heading_written)
+            {
+                // Write AD structure heading.
+                *p_offset                 += AD_LENGTH_FIELD_SIZE;
+                p_encoded_data[*p_offset]  = adv_type;
+                *p_offset                 += AD_TYPE_FIELD_SIZE;
+                is_heading_written         = true;
+            }
+
+            // Write UUID.
+            err_code = sd_ble_uuid_encode(&uuid, &encoded_size, &p_encoded_data[*p_offset]);
+            VERIFY_SUCCESS(err_code);
+            *p_offset += encoded_size;
+        }
+    }
+
+    if (is_heading_written)
+    {
+        // Write length.
+        length = (*p_offset) - (start_pos + AD_LENGTH_FIELD_SIZE);
+        // There is only 1 byte intended to encode length
+        if (length > 0x00FF)
+        {
+            return NRF_ERROR_DATA_SIZE;
+        }
+        p_encoded_data[start_pos] = (uint8_t)length;
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+static ret_code_t uuid_list_encode(const ble_advdata_uuid_list_t * p_uuid_list,
+                                   uint8_t                         adv_type_16,
+                                   uint8_t                         adv_type_128,
+                                   uint8_t                       * p_encoded_data,
+                                   uint16_t                      * p_offset,
+                                   uint16_t                        max_size)
+{
+    ret_code_t err_code;
+
+    // Encode 16 bit UUIDs.
+    err_code = uuid_list_sized_encode(p_uuid_list,
+                                      adv_type_16,
+                                      sizeof(uint16_le_t),
+                                      p_encoded_data,
+                                      p_offset,
+                                      max_size);
+    VERIFY_SUCCESS(err_code);
+
+    // Encode 128 bit UUIDs.
+    err_code = uuid_list_sized_encode(p_uuid_list,
+                                      adv_type_128,
+                                      sizeof(ble_uuid128_t),
+                                      p_encoded_data,
+                                      p_offset,
+                                      max_size);
+    VERIFY_SUCCESS(err_code);
+
+    return NRF_SUCCESS;
+}
+
+
+static ret_code_t conn_int_check(const ble_advdata_conn_int_t *p_conn_int)
+{
+    // Check Minimum Connection Interval.
+    if ((p_conn_int->min_conn_interval < 0x0006) ||
+        (
+            (p_conn_int->min_conn_interval > 0x0c80) &&
+            (p_conn_int->min_conn_interval != 0xffff)
+        )
+       )
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    // Check Maximum Connection Interval.
+    if ((p_conn_int->max_conn_interval < 0x0006) ||
+        (
+            (p_conn_int->max_conn_interval > 0x0c80) &&
+            (p_conn_int->max_conn_interval != 0xffff)
+        )
+       )
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    // Make sure Minimum Connection Interval is not bigger than Maximum Connection Interval.
+    if ((p_conn_int->min_conn_interval != 0xffff) &&
+        (p_conn_int->max_conn_interval != 0xffff) &&
+        (p_conn_int->min_conn_interval > p_conn_int->max_conn_interval)
+        )
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+static ret_code_t conn_int_encode(const ble_advdata_conn_int_t * p_conn_int,
+                                  uint8_t                      * p_encoded_data,
+                                  uint16_t                     * p_offset,
+                                  uint16_t                       max_size)
+{
+    ret_code_t err_code;
+
+    // Check for buffer overflow.
+    if (((*p_offset) + AD_TYPE_CONN_INT_SIZE) > max_size)
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Check parameters.
+    err_code = conn_int_check(p_conn_int);
+    VERIFY_SUCCESS(err_code);
+
+    // Encode Length and AD Type.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE + AD_TYPE_CONN_INT_DATA_SIZE);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_SLAVE_CONNECTION_INTERVAL_RANGE;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+
+    // Encode Minimum and Maximum Connection Intervals.
+    *p_offset += uint16_encode(p_conn_int->min_conn_interval, &p_encoded_data[*p_offset]);
+    *p_offset += uint16_encode(p_conn_int->max_conn_interval, &p_encoded_data[*p_offset]);
+
+    return NRF_SUCCESS;
+}
+
+
+static ret_code_t manuf_specific_data_encode(const ble_advdata_manuf_data_t * p_manuf_sp_data,
+                                             uint8_t                        * p_encoded_data,
+                                             uint16_t                       * p_offset,
+                                             uint16_t                         max_size)
+{
+    uint32_t data_size = AD_TYPE_MANUF_SPEC_DATA_ID_SIZE + p_manuf_sp_data->data.size;
+
+    // Check for buffer overflow.
+    if (((*p_offset) + AD_DATA_OFFSET + data_size) > max_size)
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // There is only 1 byte intended to encode length which is (data_size + AD_TYPE_FIELD_SIZE)
+    if (data_size > (0x00FF - AD_TYPE_FIELD_SIZE))
+    {
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Encode Length and AD Type.
+    p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE + data_size);
+    *p_offset                 += AD_LENGTH_FIELD_SIZE;
+    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
+    *p_offset                 += AD_TYPE_FIELD_SIZE;
+
+    // Encode Company Identifier.
+    *p_offset += uint16_encode(p_manuf_sp_data->company_identifier, &p_encoded_data[*p_offset]);
+
+    // Encode additional manufacturer specific data.
+    if (p_manuf_sp_data->data.size > 0)
+    {
+        if (p_manuf_sp_data->data.p_data == NULL)
+        {
+            return NRF_ERROR_INVALID_PARAM;
+        }
+        memcpy(&p_encoded_data[*p_offset], p_manuf_sp_data->data.p_data, p_manuf_sp_data->data.size);
+        *p_offset += p_manuf_sp_data->data.size;
+    }
+
+    return NRF_SUCCESS;
+}
+
+// Implemented only for 16-bit UUIDs
+static ret_code_t service_data_encode(const ble_advdata_t * p_advdata,
+                                      uint8_t             * p_encoded_data,
+                                      uint16_t            * p_offset,
+                                      uint16_t              max_size)
+{
+    uint8_t i;
+
+    // Check parameter consistency.
+    if (p_advdata->p_service_data_array == NULL)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    for (i = 0; i < p_advdata->service_data_count; i++)
+    {
+        ble_advdata_service_data_t * p_service_data;
+        uint32_t                     data_size;
+
+        p_service_data = &p_advdata->p_service_data_array[i];
+        // For now implemented only for 16-bit UUIDs
+        data_size      = AD_TYPE_SERV_DATA_16BIT_UUID_SIZE + p_service_data->data.size;
+
+        // There is only 1 byte intended to encode length which is (data_size + AD_TYPE_FIELD_SIZE)
+        if (data_size > (0x00FF - AD_TYPE_FIELD_SIZE))
+        {
+            return NRF_ERROR_DATA_SIZE;
+        }
+
+        // Encode Length and AD Type.
+        p_encoded_data[*p_offset]  = (uint8_t)(AD_TYPE_FIELD_SIZE + data_size);
+        *p_offset                 += AD_LENGTH_FIELD_SIZE;
+        p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_SERVICE_DATA;
+        *p_offset                 += AD_TYPE_FIELD_SIZE;
+
+        // Encode service 16-bit UUID.
+        *p_offset += uint16_encode(p_service_data->service_uuid, &p_encoded_data[*p_offset]);
+
+        // Encode additional service data.
+        if (p_service_data->data.size > 0)
+        {
+            if (p_service_data->data.p_data == NULL)
+            {
+                return NRF_ERROR_INVALID_PARAM;
+            }
+            memcpy(&p_encoded_data[*p_offset], p_service_data->data.p_data, p_service_data->data.size);
+            *p_offset += p_service_data->data.size;
+        }
+    }
+
+    return NRF_SUCCESS;
+}
+
+ret_code_t ble_advdata_encode(ble_advdata_t const * const p_advdata,
+                              uint8_t             * const p_encoded_data,
+                              uint16_t            * const p_len)
+{
+    ret_code_t err_code = NRF_SUCCESS;
+    uint16_t   max_size = *p_len;
+    *p_len = 0;
+
+    // Encode LE Bluetooth Device Address
+    if (p_advdata->include_ble_device_addr)
+    {
+        err_code = ble_device_addr_encode(p_encoded_data, p_len, max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode appearance.
+    if (p_advdata->include_appearance)
+    {
+        err_code = appearance_encode(p_encoded_data, p_len, max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    //Encode Flags
+    if (p_advdata->flags != 0 )
+    {
+        err_code = flags_encode(p_advdata->flags, p_encoded_data, p_len, max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode TX power level.
+    if (p_advdata->p_tx_power_level != NULL)
+    {
+        err_code = tx_power_level_encode(*p_advdata->p_tx_power_level,
+                                         p_encoded_data,
+                                         p_len,
+                                         max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode 'more available' uuid list.
+    if (p_advdata->uuids_more_available.uuid_cnt > 0)
+    {
+        err_code = uuid_list_encode(&p_advdata->uuids_more_available,
+                                    BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_MORE_AVAILABLE,
+                                    BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_MORE_AVAILABLE,
+                                    p_encoded_data,
+                                    p_len,
+                                    max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode 'complete' uuid list.
+    if (p_advdata->uuids_complete.uuid_cnt > 0)
+    {
+        err_code = uuid_list_encode(&p_advdata->uuids_complete,
+                                    BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE,
+                                    BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE,
+                                    p_encoded_data,
+                                    p_len,
+                                    max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode 'solicited service' uuid list.
+    if (p_advdata->uuids_solicited.uuid_cnt > 0)
+    {
+        err_code = uuid_list_encode(&p_advdata->uuids_solicited,
+                                    BLE_GAP_AD_TYPE_SOLICITED_SERVICE_UUIDS_16BIT,
+                                    BLE_GAP_AD_TYPE_SOLICITED_SERVICE_UUIDS_128BIT,
+                                    p_encoded_data,
+                                    p_len,
+                                    max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode Slave Connection Interval Range.
+    if (p_advdata->p_slave_conn_int != NULL)
+    {
+        err_code = conn_int_encode(p_advdata->p_slave_conn_int, p_encoded_data, p_len, max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode Manufacturer Specific Data.
+    if (p_advdata->p_manuf_specific_data != NULL)
+    {
+        err_code = manuf_specific_data_encode(p_advdata->p_manuf_specific_data,
+                                              p_encoded_data,
+                                              p_len,
+                                              max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode Service Data.
+    if (p_advdata->service_data_count > 0)
+    {
+        err_code = service_data_encode(p_advdata, p_encoded_data, p_len, max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    // Encode name. WARNING: it is encoded last on purpose since too long device name is truncated.
+    if (p_advdata->name_type != BLE_ADVDATA_NO_NAME)
+    {
+        err_code = name_encode(p_advdata, p_encoded_data, p_len, max_size);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    return err_code;
+}
+
+
+uint16_t ble_advdata_search(uint8_t const * p_encoded_data,
+                            uint16_t        data_len,
+                            uint16_t      * p_offset,
+                            uint8_t         ad_type)
+{
+    if ((p_encoded_data == NULL) || (p_offset == NULL))
+    {
+        return 0;
+    }
+
+    uint16_t i = 0;
+
+    while ((i + 1 < data_len) && ((i < *p_offset) || (p_encoded_data[i + 1] != ad_type)))
+    {
+        // Jump to next data.
+        i += (p_encoded_data[i] + 1);
+    }
+
+    if (i >= data_len)
+    {
+        return 0;
+    }
+    else
+    {
+        uint16_t offset = i + 2;
+        uint16_t len    = p_encoded_data[i] ? (p_encoded_data[i] - 1) : 0;
+        if (!len || ((offset + len) > data_len))
+        {
+            // Malformed. Zero length or extends beyond provided data.
+            return 0;
+        }
+        *p_offset = offset;
+        return len;
+    }
+}
+
+
+uint8_t * ble_advdata_parse(uint8_t  * p_encoded_data,
+                            uint16_t   data_len,
+                            uint8_t    ad_type)
+{
+    uint16_t offset = 0;
+    uint16_t len    = ble_advdata_search(p_encoded_data, data_len, &offset, ad_type);
+
+    if (len == 0)
+    {
+        return NULL;
+    }
+    else
+    {
+        return &p_encoded_data[offset];
+    }
+}
+
+
+bool ble_advdata_name_find(uint8_t const * p_encoded_data,
+                           uint16_t        data_len,
+                           char    const * p_target_name)
+{
+    uint16_t        parsed_name_len;
+    uint8_t const * p_parsed_name;
+    uint16_t        data_offset          = 0;
+
+    if (p_target_name == NULL)
+    {
+        return false;
+    }
+
+
+    parsed_name_len = ble_advdata_search(p_encoded_data,
+                                         data_len,
+                                         &data_offset,
+                                         BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME);
+
+    p_parsed_name = &p_encoded_data[data_offset];
+
+    if (   (data_offset != 0)
+        && (parsed_name_len != 0)
+        && (strlen(p_target_name) == parsed_name_len)
+        && (memcmp(p_target_name, p_parsed_name, parsed_name_len) == 0))
+    {
+        return true;
+    }
+
+    return false;
+}
+
+
+bool ble_advdata_short_name_find(uint8_t const * p_encoded_data,
+                                 uint16_t        data_len,
+                                 char    const * p_target_name,
+                                 uint8_t const   short_name_min_len)
+{
+    uint16_t        parsed_name_len;
+    uint8_t const * p_parsed_name;
+    uint16_t        data_offset          = 0;
+
+    if (p_target_name == NULL)
+    {
+        return false;
+    }
+
+    parsed_name_len = ble_advdata_search(p_encoded_data,
+                                         data_len,
+                                         &data_offset,
+                                         BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME);
+
+    p_parsed_name = &p_encoded_data[data_offset];
+
+    if (   (data_offset != 0)
+        && (parsed_name_len != 0)
+        && (parsed_name_len >= short_name_min_len)
+        && (parsed_name_len < strlen(p_target_name))
+        && (memcmp(p_target_name, p_parsed_name, parsed_name_len) == 0))
+    {
+        return true;
+    }
+
+    return false;
+}
+
+
+bool ble_advdata_uuid_find(uint8_t    const * p_encoded_data,
+                           uint16_t           data_len,
+                           ble_uuid_t const * p_target_uuid)
+{
+
+    ret_code_t      err_code;
+    uint16_t        data_offset = 0;
+    uint8_t         raw_uuid_len    = UUID128_SIZE;
+    uint8_t const * p_parsed_uuid;
+    uint16_t        parsed_uuid_len = data_len;
+    uint8_t         raw_uuid[UUID128_SIZE];
+    uint8_t         ad_types[N_AD_TYPES];
+
+    err_code = sd_ble_uuid_encode(p_target_uuid, &raw_uuid_len, raw_uuid);
+
+    if ((p_encoded_data == NULL) || (err_code != NRF_SUCCESS))
+    {
+        // Invalid p_encoded_data or p_target_uuid.
+        return false;
+    }
+
+    switch (raw_uuid_len)
+    {
+        case UUID16_SIZE:
+            ad_types[0] = BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE;
+            ad_types[1] = BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_MORE_AVAILABLE;
+            break;
+
+        case UUID32_SIZE:
+            // Not currently supported by sd_ble_uuid_encode().
+            ad_types[0] = BLE_GAP_AD_TYPE_32BIT_SERVICE_UUID_COMPLETE;
+            ad_types[1] = BLE_GAP_AD_TYPE_32BIT_SERVICE_UUID_MORE_AVAILABLE;
+            break;
+
+        case UUID128_SIZE:
+            ad_types[0] = BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE;
+            ad_types[1] = BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_MORE_AVAILABLE;
+            break;
+
+        default:
+            return false;
+    }
+
+    for (uint8_t i = 0; (i < N_AD_TYPES) && (data_offset == 0); i++)
+    {
+        parsed_uuid_len = ble_advdata_search(p_encoded_data, data_len, &data_offset, ad_types[i]);
+    }
+
+    if (data_offset == 0)
+    {
+        // Could not find any relevant UUIDs in the encoded data.
+        return false;
+    }
+
+    p_parsed_uuid = &p_encoded_data[data_offset];
+
+    // Verify if any UUID matches the given UUID.
+    for (uint16_t list_offset = 0; list_offset < parsed_uuid_len; list_offset += raw_uuid_len)
+    {
+        if (memcmp(&p_parsed_uuid[list_offset], raw_uuid, raw_uuid_len) == 0)
+        {
+            return true;
+        }
+    }
+
+    // Could not find the UUID among the encoded data.
+    return false;
+}
+
+
+bool ble_advdata_appearance_find(uint8_t  const * p_encoded_data,
+                                 uint16_t         data_len,
+                                 uint16_t const * p_target_appearance)
+{
+    uint16_t        data_offset = 0;
+    uint8_t         appearance_len;
+    uint16_t        decoded_appearance;
+
+    appearance_len = ble_advdata_search(p_encoded_data, data_len, &data_offset, BLE_GAP_AD_TYPE_APPEARANCE);
+
+    if (   (data_offset == 0)
+        || (p_target_appearance == NULL)
+        || (appearance_len == 0))
+    {
+        // Could not find any Appearance in the encoded data, or invalid p_target_appearance.
+        return false;
+    }
+
+    decoded_appearance = uint16_decode(&p_encoded_data[data_offset]);
+
+    if (decoded_appearance == *p_target_appearance)
+    {
+        return true;
+    }
+
+    // Could not find the appearance among the encoded data.
+    return false;
+}

components/ble/common/ble_advdata.h

@@ -0,0 +1,326 @@
+/**
+ * Copyright (c) 2012 - 2021, 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_lib_advdata Advertising and Scan Response Data Encoder
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Functions for encoding data in the Advertising and Scan Response Data format,
+ *        and for passing the data to the stack.
+ */
+
+#ifndef BLE_ADVDATA_H__
+#define BLE_ADVDATA_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "ble.h"
+#include "sdk_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define AD_LENGTH_FIELD_SIZE               1UL                                         /**< Advertising Data and Scan Response format contains 1 octet for the length. */
+#define AD_TYPE_FIELD_SIZE                 1UL                                         /**< Advertising Data and Scan Response format contains 1 octet for the AD type. */
+#define AD_DATA_OFFSET                     (AD_LENGTH_FIELD_SIZE + AD_TYPE_FIELD_SIZE) /**< Offset for the AD data field of the Advertising Data and Scan Response format. */
+
+#define AD_TYPE_BLE_DEVICE_ADDR_TYPE_SIZE  1UL                                 /**< Data size (in octets) of the Address type of the LE Bluetooth Device Address AD type. */
+#define AD_TYPE_BLE_DEVICE_ADDR_DATA_SIZE  (BLE_GAP_ADDR_LEN + \
+                                            AD_TYPE_BLE_DEVICE_ADDR_TYPE_SIZE) /**< Data size (in octets) of the LE Bluetooth Device Address AD type. */
+#define AD_TYPE_BLE_DEVICE_ADDR_SIZE       (AD_DATA_OFFSET + \
+                                            AD_TYPE_BLE_DEVICE_ADDR_DATA_SIZE) /**< Size (in octets) of the LE Bluetooth Device Address AD type. */
+#define AD_TYPE_APPEARANCE_DATA_SIZE       2UL                                 /**< Data size (in octets) of the Appearance AD type. */
+#define AD_TYPE_APPEARANCE_SIZE            (AD_DATA_OFFSET + \
+                                            AD_TYPE_APPEARANCE_DATA_SIZE)      /**< Size (in octets) of the Appearance AD type. */
+#define AD_TYPE_FLAGS_DATA_SIZE            1UL                                 /**< Data size (in octets) of the Flags AD type. */
+#define AD_TYPE_FLAGS_SIZE                 (AD_DATA_OFFSET + \
+                                            AD_TYPE_FLAGS_DATA_SIZE)           /**< Size (in octets) of the Flags AD type. */
+#define AD_TYPE_TX_POWER_LEVEL_DATA_SIZE   1UL                                 /**< Data size (in octets) of the TX Power Level AD type. */
+#define AD_TYPE_TX_POWER_LEVEL_SIZE        (AD_DATA_OFFSET + \
+                                            AD_TYPE_TX_POWER_LEVEL_DATA_SIZE)  /**< Size (in octets) of the TX Power Level AD type. */
+#define AD_TYPE_CONN_INT_DATA_SIZE         4UL                                 /**< Data size (in octets) of the Slave Connection Interval Range AD type. */
+#define AD_TYPE_CONN_INT_SIZE              (AD_DATA_OFFSET + \
+                                            AD_TYPE_CONN_INT_DATA_SIZE)        /**< Data size (in octets) of the Slave Connection Interval Range AD type. */
+#define AD_TYPE_MANUF_SPEC_DATA_ID_SIZE    2UL                                 /**< Size (in octets) of the Company Identifier Code, which is a part of the Manufacturer Specific Data AD type. */
+#define AD_TYPE_SERV_DATA_16BIT_UUID_SIZE  2UL                                 /**< Size (in octets) of the 16-bit UUID, which is a part of the Service Data AD type. */
+
+#define BLE_ADV_DATA_MATCH_FULL_NAME       0xff
+
+
+/**@brief Security Manager TK value. */
+typedef struct
+{
+  uint8_t tk[BLE_GAP_SEC_KEY_LEN];      /**< Array containing TK value in little-endian format. */
+} ble_advdata_tk_value_t;
+
+/**@brief Advertising data LE Role types. This enumeration contains the options available for the LE role inside
+ *        the advertising data. */
+typedef enum
+{
+    BLE_ADVDATA_ROLE_NOT_PRESENT = 0,                                 /**< LE Role AD structure not present. */
+    BLE_ADVDATA_ROLE_ONLY_PERIPH,                                     /**< Only Peripheral Role supported. */
+    BLE_ADVDATA_ROLE_ONLY_CENTRAL,                                    /**< Only Central Role supported. */
+    BLE_ADVDATA_ROLE_BOTH_PERIPH_PREFERRED,                           /**< Peripheral and Central Role supported. Peripheral Role preferred for connection establishment. */
+    BLE_ADVDATA_ROLE_BOTH_CENTRAL_PREFERRED                           /**< Peripheral and Central Role supported. Central Role preferred for connection establishment */
+} ble_advdata_le_role_t;
+
+/**@brief Advertising data name type. This enumeration contains the options available for the device name inside
+ *        the advertising data. */
+typedef enum
+{
+    BLE_ADVDATA_NO_NAME,                                              /**< Include no device name in advertising data. */
+    BLE_ADVDATA_SHORT_NAME,                                           /**< Include short device name in advertising data. */
+    BLE_ADVDATA_FULL_NAME                                             /**< Include full device name in advertising data. */
+} ble_advdata_name_type_t;
+
+/**@brief UUID list type. */
+typedef struct
+{
+    uint16_t                     uuid_cnt;                            /**< Number of UUID entries. */
+    ble_uuid_t *                 p_uuids;                             /**< Pointer to UUID array entries. */
+} ble_advdata_uuid_list_t;
+
+/**@brief Connection interval range structure. */
+typedef struct
+{
+    uint16_t                     min_conn_interval;                   /**< Minimum connection interval, in units of 1.25 ms, range 6 to 3200 (7.5 ms to 4 s). */
+    uint16_t                     max_conn_interval;                   /**< Maximum connection interval, in units of 1.25 ms, range 6 to 3200 (7.5 ms to 4 s). The value 0xFFFF indicates no specific maximum. */
+} ble_advdata_conn_int_t;
+
+/**@brief Manufacturer specific data structure. */
+typedef struct
+{
+    uint16_t                     company_identifier;                  /**< Company identifier code. */
+    uint8_array_t                data;                                /**< Additional manufacturer specific data. */
+} ble_advdata_manuf_data_t;
+
+/**@brief Service data structure. */
+typedef struct
+{
+    uint16_t                     service_uuid;                        /**< Service UUID. */
+    uint8_array_t                data;                                /**< Additional service data. */
+} ble_advdata_service_data_t;
+
+/**@brief Advertising data structure. This structure contains all options and data needed for encoding and
+ *        setting the advertising data. */
+typedef struct
+{
+    ble_advdata_name_type_t      name_type;                           /**< Type of device name. */
+    uint8_t                      short_name_len;                      /**< Length of short device name (if short type is specified). */
+    bool                         include_appearance;                  /**< Determines if Appearance shall be included. */
+    uint8_t                      flags;                               /**< Advertising data Flags field. */
+    int8_t *                     p_tx_power_level;                    /**< TX Power Level field. */
+    ble_advdata_uuid_list_t      uuids_more_available;                /**< List of UUIDs in the 'More Available' list. */
+    ble_advdata_uuid_list_t      uuids_complete;                      /**< List of UUIDs in the 'Complete' list. */
+    ble_advdata_uuid_list_t      uuids_solicited;                     /**< List of solicited UUIDs. */
+    ble_advdata_conn_int_t *     p_slave_conn_int;                    /**< Slave Connection Interval Range. */
+    ble_advdata_manuf_data_t *   p_manuf_specific_data;               /**< Manufacturer specific data. */
+    ble_advdata_service_data_t * p_service_data_array;                /**< Array of Service data structures. */
+    uint8_t                      service_data_count;                  /**< Number of Service data structures. */
+    bool                         include_ble_device_addr;             /**< Determines if LE Bluetooth Device Address shall be included. */
+    ble_advdata_le_role_t        le_role;                             /**< LE Role field. Included when different from @ref BLE_ADVDATA_ROLE_NOT_PRESENT. @warning This field can be used only for NFC. For BLE advertising, set it to NULL. */
+    ble_advdata_tk_value_t *     p_tk_value;                          /**< Security Manager TK value field. Included when different from NULL. @warning This field can be used only for NFC. For BLE advertising, set it to NULL.*/
+    uint8_t *                    p_sec_mgr_oob_flags;                 /**< Security Manager Out Of Band Flags field. Included when different from NULL. @warning This field can be used only for NFC. For BLE advertising, set it to NULL.*/
+    ble_gap_lesc_oob_data_t *    p_lesc_data;                         /**< LE Secure Connections OOB data. Included when different from NULL. @warning This field can be used only for NFC. For BLE advertising, set it to NULL.*/
+} ble_advdata_t;
+
+/**@brief Function for encoding data in the Advertising and Scan Response data format (AD structures).
+ *
+ * @details This function encodes data into the Advertising and Scan Response data format
+ *          (AD structures) based on the fields in the supplied structures. This function can be
+ *          used to create a payload of Advertising packet or Scan Response packet, or a payload of
+ *          NFC message intended for initiating the Out-of-Band pairing.
+ *
+ * @param[in]      p_advdata       Pointer to the structure for specifying the content of encoded data.
+ * @param[out]     p_encoded_data  Pointer to the buffer where encoded data will be returned.
+ * @param[in,out]  p_len           \c in: Size of \p p_encoded_data buffer.
+ *                                 \c out: Length of encoded data.
+ *
+ * @retval NRF_SUCCESS             If the operation was successful.
+ * @retval NRF_ERROR_INVALID_PARAM If the operation failed because a wrong parameter was provided in
+ *                                 \p p_advdata.
+ * @retval NRF_ERROR_DATA_SIZE     If the operation failed because not all the requested data could
+ *                                 fit into the provided buffer or some encoded AD structure is too
+ *                                 long and its length cannot be encoded with one octet.
+ *
+ * @warning This API may override the application's request to use the long name and use a short name
+ * instead. This truncation will occur in case the long name does not fit the provided buffer size.
+ * The application can specify a preferred short name length if truncation is required.
+ * For example, if the complete device name is ABCD_HRMonitor, the application can specify the short name
+ * length to be 8, so that the short device name appears as ABCD_HRM instead of ABCD_HRMo or ABCD_HRMoni
+ * if the available size for the short name is 9 or 12 respectively, to have a more appropriate short name.
+ * However, it should be noted that this is just a preference that the application can specify, and
+ * if the preference is too large to fit in the provided buffer, the name can be truncated further.
+ */
+ret_code_t ble_advdata_encode(ble_advdata_t const * const p_advdata,
+                              uint8_t             * const p_encoded_data,
+                              uint16_t            * const p_len);
+
+
+/**@brief Function for searching encoded Advertising or Scan Response data for specific data types.
+ *
+ * @details This function searches through encoded data e.g. the data produced by
+ *          @ref ble_advdata_encode, or the data found in Advertising reports
+ *          (@ref BLE_GAP_EVT_ADV_REPORT), and gives the offset of the data within the data buffer.
+ *          The data with type \p ad_type can be found at p_encoded_data[*p_offset] after calling
+ *          the function. This function can iterate through multiple instances of data of one
+ *          type by calling it again with the offset provided by the previous call.
+ *
+ *          Example code for finding multiple instances of one type of data:
+ *              offset = 0;
+ *              ble_advdata_search(&data, len, &offset, AD_TYPE);
+ *              first_instance_of_data = data[offset];
+ *              ble_advdata_search(&data, len, &offset, AD_TYPE);
+ *              second_instance_of_data = data[offset];
+ *
+ * @param[in]    p_encoded_data  The data buffer containing the encoded Advertising data.
+ * @param[in]    data_len        The length of the data buffer \p p_encoded_data.
+ * @param[inout] p_offset        \c in: The offset to start searching from.
+ *                               \c out: The offset the data type can be found at.
+ *                                       This value is not changed if the call returns 0.
+ * @param[in]    ad_type         The type of data to search for.
+ *
+ * @return The length of the found data, or 0 if no data was found with the the type \p ad_type,
+ *         or if \p p_encoded_data or \p p_offset were NULL.
+ */
+uint16_t ble_advdata_search(uint8_t const * p_encoded_data,
+                            uint16_t        data_len,
+                            uint16_t      * p_offset,
+                            uint8_t         ad_type);
+
+/**@brief Function for getting specific data from encoded Advertising or Scan Response data.
+ *
+ * @details This function searches through encoded data e.g. the data produced by
+ *          @ref ble_advdata_encode, or the data found in Advertising reports
+ *          (@ref BLE_GAP_EVT_ADV_REPORT), and returns a pointer directly to the data within the
+ *          data buffer.
+ *
+ *          Example code:
+ *              ad_type_data = ble_advdata_parse(&data, len, AD_TYPE);
+ *
+ * @param[in]    p_encoded_data  Data buffer containing the encoded Advertising data.
+ * @param[in]    data_len        Length of the data buffer \p p_encoded_data.
+ * @param[in]    ad_type         Type of data to search for.
+ *
+ * @return Pointer to the found data, or NULL if no data was found with the type \p ad_type,
+ *         or if \p p_encoded_data or \p p_data_len were NULL.
+ */
+uint8_t * ble_advdata_parse(uint8_t  * p_encoded_data,
+                            uint16_t   data_len,
+                            uint8_t    ad_type);
+
+
+/**@brief Function for searching through encoded Advertising data for a complete local name.
+ *
+ * @param[in]    p_encoded_data Data buffer containing the encoded Advertising data.
+ * @param[in]    data_len       Length of the data buffer \p p_encoded_data.
+ * @param[in]    p_target_name  Name to search for.
+ *
+ * @retval true   If \p p_target_name was found among \p p_encoded_data, as a complete local name.
+ * @retval false  If \p p_target_name was not found among \p p_encoded_data, or if \p p_encoded_data
+ *                or \p p_target_name was NULL.
+ */
+bool ble_advdata_name_find(uint8_t const * p_encoded_data,
+                           uint16_t        data_len,
+                           char    const * p_target_name);
+
+
+/**@brief Function for searching through encoded Advertising data for a device shortened name.
+ *
+ * @param[in]    p_encoded_data     Data buffer containing the encoded Advertising data.
+ * @param[in]    data_len           Length of the data buffer \p p_encoded_data.
+ * @param[in]    p_target_name      Name to search for.
+ * @param[in]    short_name_min_len Minimum length of the shortened name.
+ *               For example, if the advertising data has a shortened name 'No' and this parameter is
+ *               set to 4 with a target_name set to Nordic_XXX it will return false, but if
+ *               the shortened name in the advertising data is 'Nord', it will return true.
+ * @note: If the shortened name in the Advertising data has the same length as the target name,
+ *        this function will return false, since this means that the complete name is actually
+ *        longer, thus different than the target name.
+ *
+ * @retval true   If \p p_target_name was found among \p p_encoded_data, as short local name.
+ * @retval false  If \p p_target_name was not found among \p p_encoded_data, or if \p p_encoded_data
+ *                or \p p_target_name was NULL.
+ */
+bool ble_advdata_short_name_find(uint8_t const * p_encoded_data,
+                                 uint16_t        data_len,
+                                 char    const * p_target_name,
+                                 uint8_t const   short_name_min_len);
+
+/**@brief Function for searching through encoded Advertising data for a UUID (16-bit or 128-bit).
+ *
+ * @param[in]    p_encoded_data  Data buffer containing the encoded Advertising data.
+ * @param[in]    data_len        Length of the data buffer \p p_encoded_data.
+ * @param[in]    p_target_uuid   UUID to search for.
+ *
+ * @retval true   If \p p_target_uuid was found among \p p_encoded_data.
+ * @retval false  If \p p_target_uuid was not found among \p p_encoded_data, or if \p p_encoded_data
+ *                or \p p_target_uuid was NULL.
+ */
+bool ble_advdata_uuid_find(uint8_t    const * p_encoded_data,
+                           uint16_t           data_len,
+                           ble_uuid_t const * p_target_uuid);
+
+
+/**@brief Function for searching through encoded Advertising data for an appearance.
+ *
+ * @param[in]    p_encoded_data        Data buffer containing the encoded Advertising data.
+ * @param[in]    data_len              Length of the data buffer \p p_encoded_data.
+ * @param[in]    p_target_appearance   Appearance to search for.
+ *
+ * @retval true   If \p p_target_appearance was found among \p p_encoded_data.
+ * @retval false  If \p p_target_appearance was not found among \p p_encoded_data, or if \p p_encoded_data
+ *                or \p p_target_appearance was NULL.
+ */
+bool ble_advdata_appearance_find(uint8_t   const * p_encoded_data,
+                                 uint16_t          data_len,
+                                 uint16_t  const * p_target_appearance);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_ADVDATA_H__
+
+/** @} */

components/ble/common/ble_conn_params.c

@@ -0,0 +1,575 @@
+/**
+ * Copyright (c) 2012 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(NRF_BLE_CONN_PARAMS)
+#include <stdlib.h>
+#include "nrf.h"
+#include "sdk_errors.h"
+#include "ble_hci.h"
+#include "ble_err.h"
+#include "ble_conn_params.h"
+#include "ble_srv_common.h"
+#include "ble_conn_state.h"
+#include "nrf_sdh_ble.h"
+#include "app_timer.h"
+#include "app_util.h"
+
+
+#define NRF_BLE_CONN_PARAMS_INSTANCE_COUNT  NRF_SDH_BLE_PERIPHERAL_LINK_COUNT   //!< The number of @ref ble_conn_params_instance_t instances kept by the conn_params module.
+
+#if (NRF_BLE_CONN_PARAMS_INSTANCE_COUNT < 1)
+#error Invalid NRF_SDH_BLE_PERIPHERAL_LINK_COUNT value. Set it in SDK config (nrf_sdh_ble).
+#endif
+
+/** @brief Each peripheral link has such an instance associated with it.
+ */
+typedef struct
+{
+    uint16_t              conn_handle;           //!< The connection handle of this link. If this is @ref BLE_CONN_HANDLE_INVALID, the instance is free.
+    app_timer_id_t        timer_id;              //!< The ID of the timer associated with this link.
+    uint8_t               update_count;          //!< The number of times the connection parameters have been attempted negotiated on this link.
+    uint8_t               params_ok;             //!< Whether the current connection parameters on this link are acceptable according to the @p preferred_conn_params, and configured maximum deviations.
+    ble_gap_conn_params_t preferred_conn_params; //!< The desired connection parameters for this link.
+} ble_conn_params_instance_t;
+
+static app_timer_t                m_timer_data[NRF_BLE_CONN_PARAMS_INSTANCE_COUNT] = {{{0}}};          //!< Data needed for timers.
+static ble_conn_params_instance_t m_conn_params_instances[NRF_BLE_CONN_PARAMS_INSTANCE_COUNT] = {{0}}; //!< Configuration data for each connection.
+static ble_conn_params_init_t     m_conn_params_config;                                                //!< Configuration as provided by the application during intialization.
+static ble_gap_conn_params_t      m_preferred_conn_params;                                             //!< The preferred connection parameters as specified during initialization.
+//lint -esym(551, m_preferred_conn_params) "Not accessed"
+
+
+/**@brief Function for retrieving the conn_params instance belonging to a conn_handle
+ *
+ * @params[in]  conn_handle  The connection handle to retrieve the instance of.
+ *
+ * @return  A pointer to the instance, or NULL if no instance was found with that conn_handle.
+ */
+static ble_conn_params_instance_t * instance_get(uint16_t conn_handle)
+{
+    //lint -save -e681 "Loop not entered" when NRF_BLE_CONN_PARAMS_INSTANCE_COUNT is 0
+    for (uint32_t i = 0; i < NRF_BLE_CONN_PARAMS_INSTANCE_COUNT; i++)
+    {
+        if (m_conn_params_instances[i].conn_handle == conn_handle)
+        {
+            return &m_conn_params_instances[i];
+        }
+    }
+    //lint -restore
+    return NULL;
+}
+
+
+/**@brief Function for initializing an instance, and associating it with a conn_handle.
+ *
+ * @params[in]  p_instance   The instance to initialize and associate.
+ * @params[in]  conn_handle  The connection handle to associate with.
+ */
+static __INLINE void instance_claim(ble_conn_params_instance_t * p_instance, uint16_t conn_handle)
+{
+    p_instance->conn_handle           = conn_handle;
+    p_instance->update_count          = 0;
+    p_instance->preferred_conn_params = m_preferred_conn_params;
+}
+
+
+/**@brief Function for freeing an instance.
+ *
+ * @params[in]  p_instance   The instance to free.
+ */
+static __INLINE void instance_free(ble_conn_params_instance_t * p_instance)
+{
+    p_instance->conn_handle = BLE_CONN_HANDLE_INVALID;
+}
+
+
+/**@brief Function for validating a set of connection parameters against the preferred parameters.
+ *
+ * @param[in]  p_preferred_conn_params  The desired parameters.
+ * @param[in]  p_actual_conn_params     The parameters to validate.
+ * @param[in]  max_slave_latency_err    The amount of discrepancy in slave latency, in number of
+ *                                      connection intervals, that will be accepted.
+ * @param[in]  max_sup_timeout_err      The amount of discrepancy in supervision timeout, in tens of
+ *                                      milliseconds, that will be accepted.
+ *
+ * @return  Whether the params in @p p_actual_conn_params are acceptable given the other parameters.
+ */
+static bool is_conn_params_ok(ble_gap_conn_params_t const * p_preferred_conn_params,
+                              ble_gap_conn_params_t const * p_actual_conn_params,
+                              uint16_t                      max_slave_latency_err,
+                              uint16_t                      max_sup_timeout_err)
+{
+    uint32_t max_allowed_sl = p_preferred_conn_params->slave_latency + max_slave_latency_err;
+    uint32_t min_allowed_sl = p_preferred_conn_params->slave_latency
+                              - MIN(max_slave_latency_err, p_preferred_conn_params->slave_latency);
+    uint32_t max_allowed_to = p_preferred_conn_params->conn_sup_timeout + max_sup_timeout_err;
+    uint32_t min_allowed_to = p_preferred_conn_params->conn_sup_timeout
+                              - MIN(max_sup_timeout_err, p_preferred_conn_params->conn_sup_timeout);
+
+    // Check if interval is within the acceptable range.
+    // NOTE: Using max_conn_interval in the received event data because this contains
+    //       the client's connection interval.
+    if   ((p_actual_conn_params->max_conn_interval < p_preferred_conn_params->min_conn_interval)
+       || (p_actual_conn_params->max_conn_interval > p_preferred_conn_params->max_conn_interval))
+    {
+        return false;
+    }
+
+    // Check if slave latency is within the acceptable deviation.
+    if   ((p_actual_conn_params->slave_latency < min_allowed_sl)
+       || (p_actual_conn_params->slave_latency > max_allowed_sl))
+    {
+        return false;
+    }
+
+    // Check if supervision timeout is within the acceptable deviation.
+    if   ((p_actual_conn_params->conn_sup_timeout < min_allowed_to)
+       || (p_actual_conn_params->conn_sup_timeout > max_allowed_to))
+    {
+        return false;
+    }
+
+    return true;
+}
+
+
+static void send_error_evt(ret_code_t err_code)
+{
+    if (m_conn_params_config.error_handler != NULL)
+    {
+        m_conn_params_config.error_handler(err_code);
+    }
+}
+
+
+/**@brief Function for sending a conn_param_update request on-air, and handling errors.
+ *
+ * @param[in]  conn_handle        Connection to send request on.
+ * @param[in]  p_new_conn_params  Connection parameters to request.
+ *
+ * @return  Whether the request was successfully sent.
+ */
+static bool send_update_request(uint16_t conn_handle, ble_gap_conn_params_t * p_new_conn_params)
+{
+    ret_code_t err_code;
+
+    err_code = sd_ble_gap_conn_param_update(conn_handle, p_new_conn_params);
+    if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_BUSY)) // NRF_ERROR_BUSY means another conn_param_update request is pending.
+    {
+        send_error_evt(err_code);
+    }
+
+    return (err_code == NRF_SUCCESS);
+}
+
+
+/**@brief Function called after conn_params_update_delay has happened. This is triggered by app_timer.
+ *
+ * @param[in]  p_context  Context identifying which connection this is for.
+ */
+static void update_timeout_handler(void * p_context)
+{
+    uint32_t                     conn_handle = (uint32_t)p_context;
+    ble_conn_params_instance_t * p_instance  = instance_get(conn_handle);
+
+    if (p_instance != NULL)
+    {
+        // Check if we have reached the maximum number of attempts
+        if (p_instance->update_count < m_conn_params_config.max_conn_params_update_count)
+        {
+            bool update_sent = send_update_request(conn_handle, &p_instance->preferred_conn_params);
+            if (update_sent)
+            {
+                p_instance->update_count++;
+            }
+        }
+        else
+        {
+            p_instance->update_count = 0;
+
+            // Negotiation failed, disconnect automatically if this has been configured
+            if (m_conn_params_config.disconnect_on_fail)
+            {
+                ret_code_t err_code;
+
+                err_code = sd_ble_gap_disconnect(conn_handle, BLE_HCI_CONN_INTERVAL_UNACCEPTABLE);
+                if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_INVALID_STATE)) // NRF_ERROR_INVALID_STATE means disconnect is already in progress.
+                {
+                    send_error_evt(err_code);
+                }
+            }
+
+            // Notify the application that the procedure has failed
+            if (m_conn_params_config.evt_handler != NULL)
+            {
+                ble_conn_params_evt_t evt;
+
+                evt.evt_type = BLE_CONN_PARAMS_EVT_FAILED;
+                evt.conn_handle = conn_handle;
+                m_conn_params_config.evt_handler(&evt);
+            }
+        }
+    }
+}
+
+
+ret_code_t ble_conn_params_init(const ble_conn_params_init_t * p_init)
+{
+    ret_code_t err_code;
+
+    VERIFY_PARAM_NOT_NULL(p_init);
+
+    m_conn_params_config = *p_init;
+    m_conn_params_config.p_conn_params = &m_preferred_conn_params;
+
+    if (p_init->p_conn_params != NULL)
+    {
+        // Set the connection params in stack.
+        err_code = sd_ble_gap_ppcp_set(p_init->p_conn_params);
+        if (err_code != NRF_SUCCESS)
+        {
+            return err_code;
+        }
+        m_preferred_conn_params = *p_init->p_conn_params;
+    }
+    else
+    {
+        // Get the (default) connection params from stack.
+        err_code = sd_ble_gap_ppcp_get(&m_preferred_conn_params);
+        if (err_code != NRF_SUCCESS)
+        {
+            return err_code;
+        }
+    }
+
+    //lint -save -e681 "Loop not entered" when NRF_BLE_CONN_PARAMS_INSTANCE_COUNT is 0
+    for (uint32_t i = 0; i < NRF_BLE_CONN_PARAMS_INSTANCE_COUNT; i++)
+    {
+        ble_conn_params_instance_t * p_instance = &m_conn_params_instances[i];
+
+        instance_free(p_instance);
+        p_instance->timer_id = &m_timer_data[i];
+
+        err_code = app_timer_create(&p_instance->timer_id,
+                            APP_TIMER_MODE_SINGLE_SHOT,
+                            update_timeout_handler);
+        if (err_code != NRF_SUCCESS)
+        {
+            return NRF_ERROR_INTERNAL;
+        }
+    }
+    //lint -restore
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t ble_conn_params_stop(void)
+{
+    ret_code_t err_code;
+
+    //lint -save -e681 "Loop not entered" when NRF_BLE_CONN_PARAMS_INSTANCE_COUNT is 0
+    for (uint32_t i = 0; i < NRF_BLE_CONN_PARAMS_INSTANCE_COUNT; i++)
+        {
+        err_code = app_timer_stop(m_conn_params_instances[i].timer_id);
+        switch (err_code)
+            {
+            case NRF_SUCCESS:
+                /* do nothing */
+                break;
+
+            case NRF_ERROR_INVALID_STATE:
+                /* do nothing */
+                break;
+
+            case NRF_ERROR_NO_MEM:
+                return NRF_ERROR_BUSY;
+
+            case NRF_ERROR_INVALID_PARAM:
+                /* fallthrough */
+            default:
+                return NRF_ERROR_INTERNAL;
+            }
+        }
+    //lint -restore
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function for taking appropriate action based on the current state of connection parameters.
+ *
+ * @param[in]  conn_handle  Connection to handle.
+ * @param[in]  p_instance   Configuration for the connection.
+ */
+static void conn_params_negotiation(uint16_t conn_handle, ble_conn_params_instance_t * p_instance)
+        {
+    // Start negotiation if the received connection parameters are not acceptable
+    if (!p_instance->params_ok)
+            {
+        ret_code_t err_code;
+        uint32_t   timeout_ticks;
+
+        if (p_instance->update_count == 0)
+        {
+                // First connection parameter update
+                timeout_ticks = m_conn_params_config.first_conn_params_update_delay;
+            }
+            else
+            {
+                timeout_ticks = m_conn_params_config.next_conn_params_update_delay;
+            }
+
+        err_code = app_timer_start(p_instance->timer_id, timeout_ticks, (void *)(uint32_t)conn_handle);
+        if (err_code != NRF_SUCCESS)
+            {
+            send_error_evt(err_code);
+            }
+        }
+    else
+    {
+        p_instance->update_count = 0;
+
+        // Notify the application that the procedure has succeeded
+        if (m_conn_params_config.evt_handler != NULL)
+        {
+            ble_conn_params_evt_t evt;
+
+            evt.evt_type = BLE_CONN_PARAMS_EVT_SUCCEEDED;
+            evt.conn_handle = conn_handle;
+            m_conn_params_config.evt_handler(&evt);
+        }
+    }
+}
+
+
+/**@brief Function for handling a connection event from the SoftDevice.
+ *
+ * @param[in]  p_ble_evt  Event from the SoftDevice.
+ */
+static void on_connect(ble_evt_t const * p_ble_evt)
+{
+    uint8_t  role        = p_ble_evt->evt.gap_evt.params.connected.role;
+    uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+
+    if (role != BLE_GAP_ROLE_PERIPH)
+    {
+        return;
+    }
+
+    ble_conn_params_instance_t * p_instance  = instance_get(BLE_CONN_HANDLE_INVALID);
+
+    if (p_instance == NULL)
+    {
+        send_error_evt(NRF_ERROR_NO_MEM);
+        return;
+    }
+
+    instance_claim(p_instance, conn_handle);
+    p_instance->params_ok = is_conn_params_ok(&p_instance->preferred_conn_params,
+                                              &p_ble_evt->evt.gap_evt.params.connected.conn_params,
+                                              NRF_BLE_CONN_PARAMS_MAX_SLAVE_LATENCY_DEVIATION,
+                                              NRF_BLE_CONN_PARAMS_MAX_SUPERVISION_TIMEOUT_DEVIATION);
+
+    // Check if we shall handle negotiation on connect
+    if (m_conn_params_config.start_on_notify_cccd_handle == BLE_GATT_HANDLE_INVALID)
+    {
+        conn_params_negotiation(conn_handle, p_instance);
+    }
+}
+
+
+/**@brief Function for handling a disconnection event from the SoftDevice.
+ *
+ * @param[in]  p_ble_evt  Event from the SoftDevice.
+ */
+static void on_disconnect(ble_evt_t const * p_ble_evt)
+{
+    ret_code_t                   err_code;
+    uint16_t                     conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+    ble_conn_params_instance_t * p_instance  = instance_get(conn_handle);
+
+    if (p_instance != NULL)
+    {
+    // Stop timer if running
+        err_code = app_timer_stop(p_instance->timer_id);
+        if (err_code != NRF_SUCCESS)
+        {
+            send_error_evt(err_code);
+        }
+
+        instance_free(p_instance);
+    }
+		
+		
+		
+}
+
+
+/**@brief Function for handling a GATT write event from the SoftDevice.
+ *
+ * @details To provide the start_on_notify_cccd_handle functionality.
+ *
+ * @param[in]  p_ble_evt  Event from the SoftDevice.
+ */
+static void on_write(ble_evt_t const * p_ble_evt)
+{
+    ble_gatts_evt_write_t const * p_evt_write = &p_ble_evt->evt.gatts_evt.params.write;
+
+    // Check if this is the correct CCCD
+    if (
+        (p_evt_write->handle == m_conn_params_config.start_on_notify_cccd_handle)
+        &&
+        (p_evt_write->len == 2)
+       )
+    {
+        uint16_t                     conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+        ble_conn_params_instance_t * p_instance  = instance_get(conn_handle);
+
+        if (p_instance != NULL)
+        {
+            // Check if this is a 'start notification'
+            if (ble_srv_is_notification_enabled(p_evt_write->data))
+            {
+                // Do connection parameter negotiation if necessary
+                    conn_params_negotiation(conn_handle, p_instance);
+            }
+            else
+            {
+                ret_code_t err_code;
+
+                // Stop timer if running
+                err_code = app_timer_stop(p_instance->timer_id);
+                if (err_code != NRF_SUCCESS)
+                {
+                    send_error_evt(err_code);
+                }
+            }
+        }
+    }
+}
+
+
+/**@brief Function for handling a connection parameter update event from the SoftDevice.
+ *
+ * @details This event means the peer central has changed the connection parameters or declined our
+ *          request.
+ *
+ * @param[in]  p_ble_evt  Event from the SoftDevice.
+ */
+static void on_conn_params_update(ble_evt_t const * p_ble_evt)
+{
+    uint16_t                     conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+    ble_conn_params_instance_t * p_instance  = instance_get(conn_handle);
+
+    if (p_instance != NULL)
+    {
+        p_instance->params_ok = is_conn_params_ok(
+                                     &p_instance->preferred_conn_params,
+                                     &p_ble_evt->evt.gap_evt.params.conn_param_update.conn_params,
+                                     NRF_BLE_CONN_PARAMS_MAX_SLAVE_LATENCY_DEVIATION,
+                                     NRF_BLE_CONN_PARAMS_MAX_SUPERVISION_TIMEOUT_DEVIATION);
+
+        conn_params_negotiation(conn_handle, p_instance);
+    }
+}
+
+
+/**
+ * @brief Function for handling BLE events.
+ *
+ * @param[in]   p_ble_evt       Event received from the BLE stack.
+ * @param[in]   p_context       Context.
+ */
+static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            on_connect(p_ble_evt);
+            break;
+
+        case BLE_GAP_EVT_DISCONNECTED:
+            on_disconnect(p_ble_evt);
+            break;
+
+        case BLE_GATTS_EVT_WRITE:
+            on_write(p_ble_evt);
+            break;
+
+        case BLE_GAP_EVT_CONN_PARAM_UPDATE:
+            on_conn_params_update(p_ble_evt);
+            break;
+
+        default:
+            // No implementation needed.
+            break;
+    }
+}
+
+
+ret_code_t ble_conn_params_change_conn_params(uint16_t                conn_handle,
+                                              ble_gap_conn_params_t * p_new_params)
+{
+    ret_code_t                   err_code   = BLE_ERROR_INVALID_CONN_HANDLE;
+    ble_conn_params_instance_t * p_instance = instance_get(conn_handle);
+
+    if (p_new_params == NULL)
+    {
+        p_new_params = &m_preferred_conn_params;
+    }
+
+    if (p_instance != NULL)
+    {
+        // Send request to central.
+        err_code = sd_ble_gap_conn_param_update(conn_handle, p_new_params);
+        if (err_code == NRF_SUCCESS)
+        {
+            p_instance->params_ok             = false;
+            p_instance->update_count          = 1;
+            p_instance->preferred_conn_params = *p_new_params;
+        }
+    }
+
+    return err_code;
+}
+
+NRF_SDH_BLE_OBSERVER(m_ble_observer, BLE_CONN_PARAMS_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
+
+#endif //ENABLED

components/ble/common/ble_conn_params.h

@@ -0,0 +1,156 @@
+/**
+ * Copyright (c) 2012 - 2021, 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_conn_params Connection Parameters Negotiation
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Module for initiating and executing a connection parameters negotiation procedure.
+ */
+
+#ifndef BLE_CONN_PARAMS_H__
+#define BLE_CONN_PARAMS_H__
+
+#include <stdint.h>
+#include "ble.h"
+#include "ble_srv_common.h"
+#include "sdk_errors.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief Connection Parameters Module event type. */
+typedef enum
+{
+    BLE_CONN_PARAMS_EVT_FAILED,                                     //!< Negotiation procedure failed.
+    BLE_CONN_PARAMS_EVT_SUCCEEDED                                   //!< Negotiation procedure succeeded.
+} ble_conn_params_evt_type_t;
+
+/**@brief Connection Parameters Module event. */
+typedef struct
+{
+    ble_conn_params_evt_type_t evt_type;                            //!< Type of event.
+    uint16_t                   conn_handle;                         //!< Connection the event refers to.
+} ble_conn_params_evt_t;
+
+/**@brief Connection Parameters Module event handler type. */
+typedef void (*ble_conn_params_evt_handler_t) (ble_conn_params_evt_t * p_evt);
+
+/**@brief Connection Parameters Module init structure. This contains all options and data needed for
+ *        initialization of the connection parameters negotiation module. */
+typedef struct
+{
+    ble_gap_conn_params_t *       p_conn_params;                    //!< Pointer to the connection parameters desired by the application. When calling ble_conn_params_init, if this parameter is set to NULL, the connection parameters will be fetched from host.
+    uint32_t                      first_conn_params_update_delay;   //!< Time from initiating event (connect or start of notification) to first time sd_ble_gap_conn_param_update is called (in number of timer ticks).
+    uint32_t                      next_conn_params_update_delay;    //!< Time between each call to sd_ble_gap_conn_param_update after the first (in number of timer ticks). Recommended value 30 seconds as per BLUETOOTH SPECIFICATION Version 4.0.
+    uint8_t                       max_conn_params_update_count;     //!< Number of attempts before giving up the negotiation.
+    uint16_t                      start_on_notify_cccd_handle;      //!< If procedure is to be started when notification is started, set this to the handle of the corresponding CCCD. Set to BLE_GATT_HANDLE_INVALID if procedure is to be started on connect event.
+    bool                          disconnect_on_fail;               //!< Set to TRUE if a failed connection parameters update shall cause an automatic disconnection, set to FALSE otherwise.
+    ble_conn_params_evt_handler_t evt_handler;                      //!< Event handler to be called for handling events in the Connection Parameters.
+    ble_srv_error_handler_t       error_handler;                    //!< Function to be called in case of an error.
+} ble_conn_params_init_t;
+
+
+/**@brief Function for initializing the Connection Parameters module.
+ *
+ * @note If the negotiation procedure should be triggered when notification/indication of
+ *       any characteristic is enabled by the peer, then this function must be called after
+ *       having initialized the services.
+ *
+ * @param[in]   p_init  This contains information needed to initialize this module.
+ *
+ * @retval      NRF_SUCCESS              Successful initialization.
+ * @retval      NRF_ERROR_INVALID_ADDR   The provided Connection Parameters pointer is invalid.
+ * @retval      NRF_ERROR_INVALID_PARAM  The provided Connection Parameters are not valid.
+ * @retval      NRF_ERROR_NULL           @p p_init was NULL.
+ * @retval      NRF_ERROR_INTERNAL       An unexpected error occurred.
+ */
+ret_code_t ble_conn_params_init(const ble_conn_params_init_t * p_init);
+
+/**@brief Function for stopping the Connection Parameters module.
+ *
+ * @details This function is intended to be used by the application to clean up the connection
+ *          parameters update module. This will stop the connection parameters update timer if
+ *          running, thereby preventing any impending connection parameters update procedure. This
+ *          function must be called by the application when it needs to clean itself up (for
+ *          example, before disabling the bluetooth SoftDevice) so that an unwanted timer expiry
+ *          event can be avoided.
+ *
+ * @retval      NRF_SUCCESS         Successfully stopped module.
+ * @retval      NRF_ERROR_BUSY      Could not complete operation at this time. Try again later.
+                                    Note that some timers may have been disabled.
+ * @retval      NRF_ERROR_INTERNAL  An unexpected error occurred.
+ */
+ret_code_t ble_conn_params_stop(void);
+
+/**@brief Function for changing the current connection parameters to a new set.
+ *
+ *  @details Use this function to change the connection parameters to a new set of parameter
+ *       (ie different from the ones given at init of the module).
+ *       This function is useful for scenario where most of the time the application
+ *       needs a relatively big connection interval, and just sometimes, for a temporary
+ *       period requires shorter connection interval, for example to transfer a higher
+ *       amount of data.
+ *       If the given parameters does not match the current connection's parameters
+ *       this function initiates a new negotiation.
+ *
+ * @param[in]  conn_handle   The connection to change connection parameters on.
+ * @param[in]  p_new_params  This contains the new connections parameters to setup.
+ *
+ * @retval NRF_SUCCESS                    Successfully started Connection Parameter update procedure.
+ * @retval NRF_ERROR_INVALID_ADDR         The provided Connection Parameters pointer is invalid.
+ * @retval NRF_ERROR_INVALID_PARAM        The provided Connection Parameters are not valid.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  The provided connection handle is invalid.
+ * @retval NRF_ERROR_INVALID_STATE        The connection is not in a state where this operation can
+ *                                        performed.
+ * @retval NRF_ERROR_BUSY                 Could not start operation at this time. Try again later.
+ * @retval NRF_ERROR_NO_MEM               The SoftDevice lacks the memory to perform the action.
+ */
+ret_code_t ble_conn_params_change_conn_params(uint16_t                conn_handle,
+                                              ble_gap_conn_params_t * p_new_params);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_CONN_PARAMS_H__
+
+/** @} */

components/ble/common/ble_conn_state.c

@@ -0,0 +1,497 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "ble_conn_state.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include "ble.h"
+#include "nrf_atflags.h"
+#include "app_error.h"
+#include "nrf_sdh_ble.h"
+#include "app_util_platform.h"
+
+
+
+#define DEFAULT_FLAG_COLLECTION_COUNT 6                                /**< The number of flags kept for each connection, excluding user flags. */
+#define TOTAL_FLAG_COLLECTION_COUNT (DEFAULT_FLAG_COLLECTION_COUNT \
+                                   + BLE_CONN_STATE_USER_FLAG_COUNT)   /**< The number of flags kept for each connection, including user flags. */
+
+/**@brief Structure containing all the flag collections maintained by the Connection State module.
+ */
+typedef struct
+{
+    nrf_atflags_t valid_flags;                                 /**< Flags indicating which connection handles are valid. */
+    nrf_atflags_t connected_flags;                             /**< Flags indicating which connections are connected, since disconnected connection handles will not immediately be invalidated. */
+    nrf_atflags_t central_flags;                               /**< Flags indicating in which connections the local device is the central. */
+    nrf_atflags_t encrypted_flags;                             /**< Flags indicating which connections are encrypted. */
+    nrf_atflags_t mitm_protected_flags;                        /**< Flags indicating which connections have encryption with protection from man-in-the-middle attacks. */
+    nrf_atflags_t lesc_flags;                                  /**< Flags indicating which connections have bonded using LE Secure Connections (LESC). */
+    nrf_atflags_t user_flags[BLE_CONN_STATE_USER_FLAG_COUNT];  /**< Flags that can be reserved by the user. The flags will be cleared when a connection is invalidated, otherwise, the user is wholly responsible for the flag states. */
+} ble_conn_state_flag_collections_t;
+
+
+ANON_UNIONS_ENABLE;
+
+/**@brief Structure containing the internal state of the Connection State module.
+ */
+typedef struct
+{
+    nrf_atflags_t acquired_flags; /**< Bitmap for keeping track of which user flags have been acquired. */
+    union
+    {
+        ble_conn_state_flag_collections_t flags;                                   /**< Flag collections kept by the Connection State module. */
+        nrf_atflags_t                     flag_array[TOTAL_FLAG_COLLECTION_COUNT]; /**< Flag collections as array to allow iterating over all flag collections. */
+    };
+} ble_conn_state_t;
+
+ANON_UNIONS_DISABLE;
+
+
+static ble_conn_state_t m_bcs = {0}; /**< Instantiation of the internal state. */
+
+
+/**@brief Function for resetting all internal memory to the values it had at initialization.
+ */
+void bcs_internal_state_reset(void)
+{
+    memset( &m_bcs, 0, sizeof(ble_conn_state_t) );
+}
+
+
+ble_conn_state_conn_handle_list_t conn_handle_list_get(nrf_atflags_t flags)
+{
+    ble_conn_state_conn_handle_list_t conn_handle_list;
+    conn_handle_list.len = 0;
+
+    if (flags != 0)
+    {
+        for (uint32_t i = 0; i < BLE_CONN_STATE_MAX_CONNECTIONS; i++)
+        {
+            if (nrf_atflags_get(&flags, i))
+            {
+                conn_handle_list.conn_handles[conn_handle_list.len++] = i;
+            }
+        }
+    }
+
+    return conn_handle_list;
+}
+
+
+uint32_t active_flag_count(nrf_atflags_t flags)
+{
+    uint32_t set_flag_count = 0;
+
+    for (uint32_t i = 0; i < BLE_CONN_STATE_MAX_CONNECTIONS; i++)
+    {
+        if (nrf_atflags_get(&flags, i))
+        {
+            set_flag_count += 1;
+        }
+    }
+    return set_flag_count;
+}
+
+
+/**@brief Function for activating a connection record.
+ *
+ * @param p_record     The record to activate.
+ * @param conn_handle  The connection handle to copy into the record.
+ * @param role         The role of the connection.
+ *
+ * @return whether the record was activated successfully.
+ */
+static bool record_activate(uint16_t conn_handle)
+{
+    if (conn_handle >= BLE_CONN_STATE_MAX_CONNECTIONS)
+    {
+        return false;
+    }
+    nrf_atflags_set(&m_bcs.flags.connected_flags, conn_handle);
+    nrf_atflags_set(&m_bcs.flags.valid_flags, conn_handle);
+    return true;
+}
+
+
+/**@brief Function for marking a connection record as invalid and resetting the values.
+ *
+ * @param p_record  The record to invalidate.
+ */
+static void record_invalidate(uint16_t conn_handle)
+{
+    for (uint32_t i = 0; i < TOTAL_FLAG_COLLECTION_COUNT; i++)
+    {
+        nrf_atflags_clear(&m_bcs.flag_array[i], conn_handle);
+    }
+}
+
+
+/**@brief Function for marking a connection as disconnected. See @ref BLE_CONN_STATUS_DISCONNECTED.
+ *
+ * @param p_record   The record of the connection to set as disconnected.
+ */
+static void record_set_disconnected(uint16_t conn_handle)
+{
+    nrf_atflags_clear(&m_bcs.flags.connected_flags, conn_handle);
+}
+
+
+/**@brief Function for invalidating records with a @ref BLE_CONN_STATUS_DISCONNECTED
+ *        connection status
+ */
+static void record_purge_disconnected()
+{
+    nrf_atflags_t                     disconnected_flags = ~m_bcs.flags.connected_flags;
+    ble_conn_state_conn_handle_list_t disconnected_list;
+
+    UNUSED_RETURN_VALUE(nrf_atomic_u32_and(&disconnected_flags, m_bcs.flags.valid_flags));
+    disconnected_list  =  conn_handle_list_get(disconnected_flags);
+
+    for (uint32_t i = 0; i < disconnected_list.len; i++)
+    {
+        record_invalidate(disconnected_list.conn_handles[i]);
+    }
+}
+
+
+/**@brief Function for checking if a user flag has been acquired.
+ *
+ * @param[in]  flag_id  Which flag to check.
+ *
+ * @return  Whether the flag has been acquired.
+ */
+static bool user_flag_is_acquired(ble_conn_state_user_flag_id_t flag_id)
+{
+    return nrf_atflags_get(&m_bcs.acquired_flags, flag_id);
+}
+
+
+void ble_conn_state_init(void)
+{
+    bcs_internal_state_reset();
+}
+
+static void flag_toggle(nrf_atflags_t * p_flags, uint16_t conn_handle, bool value)
+{
+    if (value)
+    {
+        nrf_atflags_set(p_flags, conn_handle);
+    }
+    else
+    {
+        nrf_atflags_clear(p_flags, conn_handle);
+    }
+}
+
+/**
+ * @brief Function for handling BLE events.
+ *
+ * @param[in]   p_ble_evt       Event received from the BLE stack.
+ * @param[in]   p_context       Context.
+ */
+static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            record_purge_disconnected();
+
+            if ( !record_activate(conn_handle) )
+            {
+                // No more records available. Should not happen.
+                APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
+            }
+#ifdef BLE_GAP_ROLE_CENTRAL
+            else if ((p_ble_evt->evt.gap_evt.params.connected.role == BLE_GAP_ROLE_CENTRAL))
+            {
+                // Central
+                nrf_atflags_set(&m_bcs.flags.central_flags, conn_handle);
+            }
+#endif // BLE_GAP_ROLE_CENTRAL
+            else
+            {
+                // No implementation required.
+            }
+
+            break;
+
+        case BLE_GAP_EVT_DISCONNECTED:
+            record_set_disconnected(conn_handle);
+            break;
+
+        case BLE_GAP_EVT_CONN_SEC_UPDATE:
+        {
+            uint8_t sec_lv = p_ble_evt->evt.gap_evt.params.conn_sec_update.conn_sec.sec_mode.lv;
+            // Set/unset flags based on security level.
+            flag_toggle(&m_bcs.flags.lesc_flags, conn_handle, sec_lv >= 4);
+            flag_toggle(&m_bcs.flags.mitm_protected_flags, conn_handle, sec_lv >= 3);
+            flag_toggle(&m_bcs.flags.encrypted_flags, conn_handle, sec_lv >= 2);
+            break;
+        }
+
+        case BLE_GAP_EVT_AUTH_STATUS:
+            if (p_ble_evt->evt.gap_evt.params.auth_status.auth_status == BLE_GAP_SEC_STATUS_SUCCESS)
+            {
+                bool lesc = p_ble_evt->evt.gap_evt.params.auth_status.lesc;
+                flag_toggle(&m_bcs.flags.lesc_flags, conn_handle, lesc);
+            }
+            break;
+    }
+}
+
+NRF_SDH_BLE_OBSERVER(m_ble_evt_observer, BLE_CONN_STATE_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
+
+
+bool ble_conn_state_valid(uint16_t conn_handle)
+{
+    if (conn_handle >= BLE_CONN_STATE_MAX_CONNECTIONS)
+    {
+        return false;
+    }
+    return nrf_atflags_get(&m_bcs.flags.valid_flags, conn_handle);
+}
+
+
+uint8_t ble_conn_state_role(uint16_t conn_handle)
+{
+    uint8_t role = BLE_GAP_ROLE_INVALID;
+
+    if (ble_conn_state_valid(conn_handle))
+    {
+#if defined (BLE_GAP_ROLE_PERIPH) && defined (BLE_GAP_ROLE_CENTRAL)
+        bool central = nrf_atflags_get(&m_bcs.flags.central_flags, conn_handle);
+        role = central ? BLE_GAP_ROLE_CENTRAL : BLE_GAP_ROLE_PERIPH;
+#elif defined (BLE_GAP_ROLE_CENTRAL)
+        role = BLE_GAP_ROLE_CENTRAL;
+#else
+        role = BLE_GAP_ROLE_PERIPH;
+#endif // defined (BLE_GAP_ROLE_PERIPH) && defined (BLE_GAP_ROLE_CENTRAL)
+    }
+
+    return role;
+}
+
+
+ble_conn_state_status_t ble_conn_state_status(uint16_t conn_handle)
+{
+    ble_conn_state_status_t conn_status = BLE_CONN_STATUS_INVALID;
+
+    if (ble_conn_state_valid(conn_handle))
+    {
+        bool connected = nrf_atflags_get(&m_bcs.flags.connected_flags, conn_handle);
+        conn_status = connected ? BLE_CONN_STATUS_CONNECTED : BLE_CONN_STATUS_DISCONNECTED;
+    }
+
+    return conn_status;
+}
+
+
+bool ble_conn_state_encrypted(uint16_t conn_handle)
+{
+    if (ble_conn_state_valid(conn_handle))
+    {
+        return nrf_atflags_get(&m_bcs.flags.encrypted_flags, conn_handle);
+    }
+    return false;
+}
+
+
+bool ble_conn_state_mitm_protected(uint16_t conn_handle)
+{
+    if (ble_conn_state_valid(conn_handle))
+    {
+        return nrf_atflags_get(&m_bcs.flags.mitm_protected_flags, conn_handle);
+    }
+    return false;
+}
+
+bool ble_conn_state_lesc(uint16_t conn_handle)
+{
+    if (ble_conn_state_valid(conn_handle))
+    {
+        return nrf_atflags_get(&m_bcs.flags.lesc_flags, conn_handle);
+    }
+    return false;
+}
+
+
+uint32_t ble_conn_state_conn_count(void)
+{
+    return active_flag_count(m_bcs.flags.connected_flags);
+}
+
+
+uint32_t ble_conn_state_central_conn_count(void)
+{
+    nrf_atflags_t central_conn_flags = m_bcs.flags.central_flags;
+    UNUSED_RETURN_VALUE(nrf_atomic_u32_and(&central_conn_flags, m_bcs.flags.connected_flags));
+
+    return active_flag_count(central_conn_flags);
+}
+
+
+uint32_t ble_conn_state_peripheral_conn_count(void)
+{
+    nrf_atflags_t peripheral_conn_flags = ~m_bcs.flags.central_flags;
+    UNUSED_RETURN_VALUE(nrf_atomic_u32_and(&peripheral_conn_flags, m_bcs.flags.connected_flags));
+
+    return active_flag_count(peripheral_conn_flags);
+}
+
+
+ble_conn_state_conn_handle_list_t ble_conn_state_conn_handles(void)
+{
+    return conn_handle_list_get(m_bcs.flags.valid_flags);
+}
+
+
+ble_conn_state_conn_handle_list_t ble_conn_state_central_handles(void)
+{
+    nrf_atflags_t central_conn_flags = m_bcs.flags.central_flags;
+    UNUSED_RETURN_VALUE(nrf_atomic_u32_and(&central_conn_flags, m_bcs.flags.connected_flags));
+
+    return conn_handle_list_get(central_conn_flags);
+}
+
+
+ble_conn_state_conn_handle_list_t ble_conn_state_periph_handles(void)
+{
+    nrf_atflags_t peripheral_conn_flags = ~m_bcs.flags.central_flags;
+    UNUSED_RETURN_VALUE(nrf_atomic_u32_and(&peripheral_conn_flags, m_bcs.flags.connected_flags));
+
+    return conn_handle_list_get(peripheral_conn_flags);
+}
+
+
+uint16_t ble_conn_state_conn_idx(uint16_t conn_handle)
+{
+    if (ble_conn_state_valid(conn_handle))
+    {
+        return conn_handle;
+    }
+    else
+    {
+        return BLE_CONN_STATE_MAX_CONNECTIONS;
+    }
+}
+
+
+ble_conn_state_user_flag_id_t ble_conn_state_user_flag_acquire(void)
+{
+    uint32_t acquired_flag = nrf_atflags_find_and_set_flag(&m_bcs.acquired_flags,
+                                                            BLE_CONN_STATE_USER_FLAG_COUNT);
+
+    if (acquired_flag == BLE_CONN_STATE_USER_FLAG_COUNT)
+    {
+        return BLE_CONN_STATE_USER_FLAG_INVALID;
+    }
+    return (ble_conn_state_user_flag_id_t)acquired_flag;
+}
+
+
+bool ble_conn_state_user_flag_get(uint16_t conn_handle, ble_conn_state_user_flag_id_t flag_id)
+{
+    if (user_flag_is_acquired(flag_id) && ble_conn_state_valid(conn_handle))
+    {
+        return nrf_atflags_get(&m_bcs.flags.user_flags[flag_id], conn_handle);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+
+void ble_conn_state_user_flag_set(uint16_t                      conn_handle,
+                                  ble_conn_state_user_flag_id_t flag_id,
+                                  bool                          value)
+{
+    if (user_flag_is_acquired(flag_id) && ble_conn_state_valid(conn_handle))
+    {
+        flag_toggle(&m_bcs.flags.user_flags[flag_id], conn_handle, value);
+    }
+}
+
+
+static uint32_t for_each_set_flag(nrf_atflags_t                  flags,
+                                  ble_conn_state_user_function_t user_function,
+                                  void                         * p_context)
+{
+    if (user_function == NULL)
+    {
+        return 0;
+    }
+
+    uint32_t call_count = 0;
+
+    if (flags != 0)
+    {
+        for (uint32_t i = 0; i < BLE_CONN_STATE_MAX_CONNECTIONS; i++)
+        {
+            if (nrf_atflags_get(&flags, i))
+            {
+                user_function(i, p_context);
+                call_count += 1;
+            }
+        }
+    }
+    return call_count;
+}
+
+
+uint32_t ble_conn_state_for_each_connected(ble_conn_state_user_function_t user_function,
+                                           void                         * p_context)
+{
+    return for_each_set_flag(m_bcs.flags.connected_flags, user_function, p_context);
+}
+
+
+uint32_t ble_conn_state_for_each_set_user_flag(ble_conn_state_user_flag_id_t  flag_id,
+                                               ble_conn_state_user_function_t user_function,
+                                               void                         * p_context)
+{
+    if (!user_flag_is_acquired(flag_id))
+    {
+        return 0;
+    }
+
+    return for_each_set_flag(m_bcs.flags.user_flags[flag_id], user_function, p_context);
+}

components/ble/common/ble_conn_state.h

@@ -0,0 +1,361 @@
+/**
+ * Copyright (c) 2015 - 2021, 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_conn_state Connection state
+ * @ingroup ble_sdk_lib
+ * @{
+ * @brief Module for storing data on BLE connections.
+ *
+ * @details This module stores certain states for each connection, which can be queried by
+ *          connection handle. The module uses BLE events to keep the states updated.
+ *
+ *          In addition to the preprogrammed states, this module can also keep track of a number of
+ *          binary user states, or <i>user flags</i>. These are reset to 0 for new connections, but
+ *          otherwise not touched by this module.
+ *
+ *          This module uses the @ref nrf_atomic module to make the flag operations thread-safe.
+ *
+ * @note A connection handle is not immediately invalidated when it is disconnected. Certain states,
+ *       such as the role, can still be queried until the next time a new connection is established
+ *       to any device.
+ *
+ */
+
+#ifndef BLE_CONN_STATE_H__
+#define BLE_CONN_STATE_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "ble.h"
+#include "ble_gap.h"
+#include "nrf_atomic.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief Connection handle statuses.
+ */
+typedef enum
+{
+    BLE_CONN_STATUS_INVALID,       /**< The connection handle is invalid. */
+    BLE_CONN_STATUS_DISCONNECTED,  /**< The connection handle refers to a connection that has been disconnected, but not yet invalidated. */
+    BLE_CONN_STATUS_CONNECTED,     /**< The connection handle refers to an active connection. */
+} ble_conn_state_status_t;
+
+#define BLE_CONN_STATE_MAX_CONNECTIONS  BLE_GAP_ROLE_COUNT_COMBINED_MAX  /**< The maximum number of connections supported. */
+#define BLE_CONN_STATE_USER_FLAG_COUNT  24                               /**< The number of available user flags. */
+
+
+/**@brief Type used to present a list of conn_handles.
+ */
+typedef struct
+{
+    uint32_t len;                                           /**< The length of the list. */
+    uint16_t conn_handles[BLE_CONN_STATE_MAX_CONNECTIONS];  /**< The list of handles. */
+} ble_conn_state_conn_handle_list_t;
+
+/**@brief One ID for each user flag collection.
+ *
+ * @details These IDs are used to identify user flag collections in the API calls.
+ */
+typedef enum
+{
+    BLE_CONN_STATE_USER_FLAG0 = 0,
+    BLE_CONN_STATE_USER_FLAG1,
+    BLE_CONN_STATE_USER_FLAG2,
+    BLE_CONN_STATE_USER_FLAG3,
+    BLE_CONN_STATE_USER_FLAG4,
+    BLE_CONN_STATE_USER_FLAG5,
+    BLE_CONN_STATE_USER_FLAG6,
+    BLE_CONN_STATE_USER_FLAG7,
+    BLE_CONN_STATE_USER_FLAG8,
+    BLE_CONN_STATE_USER_FLAG9,
+    BLE_CONN_STATE_USER_FLAG10,
+    BLE_CONN_STATE_USER_FLAG11,
+    BLE_CONN_STATE_USER_FLAG12,
+    BLE_CONN_STATE_USER_FLAG13,
+    BLE_CONN_STATE_USER_FLAG14,
+    BLE_CONN_STATE_USER_FLAG15,
+    BLE_CONN_STATE_USER_FLAG16,
+    BLE_CONN_STATE_USER_FLAG17,
+    BLE_CONN_STATE_USER_FLAG18,
+    BLE_CONN_STATE_USER_FLAG19,
+    BLE_CONN_STATE_USER_FLAG20,
+    BLE_CONN_STATE_USER_FLAG21,
+    BLE_CONN_STATE_USER_FLAG22,
+    BLE_CONN_STATE_USER_FLAG23,
+    BLE_CONN_STATE_USER_FLAG_INVALID,
+} ble_conn_state_user_flag_id_t;
+
+
+/**@brief Function to be called when a flag ID is set. See @ref ble_conn_state_for_each_set_user_flag.
+ *
+ * @param[in]  conn_handle  The connection the flag is set for.
+ * @param[in]  p_context    Arbitrary pointer provided by the caller of
+ *                          @ref ble_conn_state_for_each_set_user_flag.
+ */
+typedef void (*ble_conn_state_user_function_t)(uint16_t conn_handle, void * p_context);
+
+
+/**
+ * @defgroup ble_conn_state_functions BLE connection state functions
+ * @{
+ */
+
+
+/**@brief Function for initializing or resetting the module.
+ *
+ * @details This function sets all states to their default, removing all records of connection handles.
+ */
+void ble_conn_state_init(void);
+
+
+/**@brief Function for querying whether a connection handle represents a valid connection.
+ *
+ * @details A connection might be valid and have a BLE_CONN_STATUS_DISCONNECTED status.
+ *          Those connections are invalidated after a new connection occurs.
+ *
+ * @param[in]  conn_handle  Handle of the connection.
+ *
+ * @retval true   If conn_handle represents a valid connection, thus a connection for which
+                  we have a record.
+ * @retval false  If conn_handle is @ref BLE_GAP_ROLE_INVALID, or if it has never been recorded.
+ */
+bool ble_conn_state_valid(uint16_t conn_handle);
+
+
+/**@brief Function for querying the role of the local device in a connection.
+ *
+ * @param[in]  conn_handle  Handle of the connection to get the role for.
+ *
+ * @return  The role of the local device in the connection (see @ref BLE_GAP_ROLES).
+ *          If conn_handle is not valid, the function returns BLE_GAP_ROLE_INVALID.
+ */
+uint8_t ble_conn_state_role(uint16_t conn_handle);
+
+
+/**@brief Function for querying the status of a connection.
+ *
+ * @param[in]  conn_handle  Handle of the connection.
+ *
+ * @return  The status of the connection.
+ *          If conn_handle is not valid, the function returns BLE_CONN_STATE_INVALID.
+ */
+ble_conn_state_status_t ble_conn_state_status(uint16_t conn_handle);
+
+
+/**@brief Function for querying whether a connection is encrypted.
+ *
+ * @param[in]  conn_handle  Handle of connection to get the encryption state for.
+ *
+ * @retval true   If the connection is encrypted.
+ * @retval false  If the connection is not encrypted or conn_handle is invalid.
+ */
+bool ble_conn_state_encrypted(uint16_t conn_handle);
+
+
+/**@brief Function for querying whether a connection encryption is protected from Man in the Middle
+ *        attacks.
+ *
+ * @param[in]  conn_handle  Handle of connection to get the MITM state for.
+ *
+ * @retval true   If the connection is encrypted with MITM protection.
+ * @retval false  If the connection is not encrypted, or encryption is not MITM protected, or
+ *                conn_handle is invalid.
+ */
+bool ble_conn_state_mitm_protected(uint16_t conn_handle);
+
+
+/**@brief Function for querying whether a connection was bonded using LE Secure Connections (LESC).
+ *
+ * The connection must currently be encrypted.
+ *
+ * @note This function will report false if bonded, and the LESC bonding was unauthenticated
+ *       ("Just Works") and happened in a previous connection. To detect such cases as well, check
+ *       the stored bonding key, e.g. in Peer Manager, which has a LESC flag associated with it.
+ *
+ * @param[in]  conn_handle  Handle of connection to get the LESC state for.
+ *
+ * @retval true   If the connection was bonded using LESC.
+ * @retval false  If the connection has not been bonded using LESC, or conn_handle is invalid.
+ */
+bool ble_conn_state_lesc(uint16_t conn_handle);
+
+
+/**@brief Function for querying the total number of connections.
+ *
+ * @return  The total number of valid connections for which the module has a record.
+ */
+uint32_t ble_conn_state_conn_count(void);
+
+
+/**@brief Function for querying the total number of connections in which the role of the local
+ *        device is @ref BLE_GAP_ROLE_CENTRAL.
+ *
+ * @return  The number of connections in which the role of the local device is
+ *          @ref BLE_GAP_ROLE_CENTRAL.
+ */
+uint32_t ble_conn_state_central_conn_count(void);
+
+
+/**@brief Function for querying the total number of connections in which the role of the local
+ *        device is @ref BLE_GAP_ROLE_PERIPH.
+ *
+ * @return  The number of connections in which the role of the local device is
+ *          @ref BLE_GAP_ROLE_PERIPH.
+ */
+uint32_t ble_conn_state_peripheral_conn_count(void);
+
+
+/**@brief Function for obtaining a list of all connection handles for which the module has a record.
+ *
+ * @details This function takes into account connections whose state is BLE_CONN_STATUS_DISCONNECTED.
+ *
+ * @return  A list of all valid connection handles for which the module has a record.
+ */
+ble_conn_state_conn_handle_list_t ble_conn_state_conn_handles(void);
+
+
+/**@brief Function for obtaining a list of connection handles in which the role of the local
+ *        device is @ref BLE_GAP_ROLE_CENTRAL.
+ *
+ * @details This function takes into account connections whose state is BLE_CONN_STATUS_DISCONNECTED.
+ *
+ * @return  A list of all valid connection handles for which the module has a record and in which
+ *          the role of local device is @ref BLE_GAP_ROLE_CENTRAL.
+ */
+ble_conn_state_conn_handle_list_t ble_conn_state_central_handles(void);
+
+
+/**@brief Function for obtaining the handle for the connection in which the role of the local device
+ *        is @ref BLE_GAP_ROLE_PERIPH.
+ *
+ * @details This function takes into account connections whose state is BLE_CONN_STATUS_DISCONNECTED.
+ *
+ * @return  A list of all valid connection handles for which the module has a record and in which
+ *          the role of local device is @ref BLE_GAP_ROLE_PERIPH.
+ */
+ble_conn_state_conn_handle_list_t ble_conn_state_periph_handles(void);
+
+
+/**@brief Function for translating a connection handle to a value that can be used as an array index.
+ *
+ * @details Function for mapping connection handles onto the range <0 - MAX_CONNECTIONS>.
+ *
+ * @note The index will be the same as long as a connection is invalid. A subsequent connection with
+ *       the same connection handle might have a different index.
+ *
+ * @param[in]  conn_handle  The connection for which to retrieve an index.
+ *
+ * @return  An index unique to this connection. Or @ref BLE_CONN_STATE_MAX_CONNECTIONS if
+ *          @p conn_handle refers to an invalid connection.
+ */
+uint16_t ble_conn_state_conn_idx(uint16_t conn_handle);
+
+
+/**@brief Function for obtaining exclusive access to one of the user flag collections.
+ *
+ * @details The acquired collection contains one flag for each connection. These flags can be set
+ *          and read individually for each connection.
+ *
+ *          The state of user flags will not be modified by the connection state module, except to
+ *          set it to 0 for a connection when that connection is invalidated.
+ *
+ * @return  The ID of the acquired flag, or BLE_CONN_STATE_USER_FLAG_INVALID if none are available.
+ */
+ble_conn_state_user_flag_id_t ble_conn_state_user_flag_acquire(void);
+
+
+/**@brief Function for reading the value of a user flag.
+ *
+ * @param[in]  conn_handle  Handle of connection to get the flag state for.
+ * @param[in]  flag_id      Which flag to get the state for.
+ *
+ * @return  The state of the flag. If conn_handle is invalid, the function returns false.
+ */
+bool ble_conn_state_user_flag_get(uint16_t conn_handle, ble_conn_state_user_flag_id_t flag_id);
+
+
+/**@brief Function for setting the value of a user flag.
+ *
+ * @param[in]  conn_handle  Handle of connection to set the flag state for.
+ * @param[in]  flag_id      Which flag to set the state for.
+ * @param[in]  value        Value to set the flag state to.
+ */
+void ble_conn_state_user_flag_set(uint16_t                      conn_handle,
+                                  ble_conn_state_user_flag_id_t flag_id,
+                                  bool                          value);
+
+
+/**@brief Function for running a function for each active connection.
+ *
+ * @param[in]  user_function  The function to run for each connection.
+ * @param[in]  p_context      Arbitrary context to be passed to \p user_function.
+ *
+ * @return  The number of times \p user_function was run.
+ */
+uint32_t ble_conn_state_for_each_connected(ble_conn_state_user_function_t user_function,
+                                           void                         * p_context);
+
+
+/**@brief Function for running a function for each flag that is set in a user flag collection.
+ *
+ * @param[in]  flag_id        Which flags to check.
+ * @param[in]  user_function  The function to run when a flag is set.
+ * @param[in]  p_context      Arbitrary context to be passed to \p user_function.
+ *
+ * @return  The number of times \p user_function was run.
+ */
+uint32_t ble_conn_state_for_each_set_user_flag(ble_conn_state_user_flag_id_t  flag_id,
+                                               ble_conn_state_user_function_t user_function,
+                                               void                         * p_context);
+
+/** @} */
+/** @} */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLE_CONN_STATE_H__ */

components/ble/common/ble_date_time.h

@@ -0,0 +1,113 @@
+/**
+ * Copyright (c) 2011 - 2021, 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.
+ *
+ */
+/* Attention!
+*  To maintain compliance with Nordic Semiconductor ASA’s Bluetooth profile
+*  qualification listings, this section of source code must not be modified.
+*/
+
+/** @file
+ * @brief Contains definition of ble_date_time structure.
+ */
+
+/** @file
+ *
+ * @defgroup ble_sdk_srv_date_time BLE Date Time characteristic type
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Definition of ble_date_time_t type.
+ */
+
+#ifndef BLE_DATE_TIME_H__
+#define BLE_DATE_TIME_H__
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief Date and Time structure. */
+typedef struct
+{
+    uint16_t year;
+    uint8_t  month;
+    uint8_t  day;
+    uint8_t  hours;
+    uint8_t  minutes;
+    uint8_t  seconds;
+} ble_date_time_t;
+
+static __INLINE uint8_t ble_date_time_encode(const ble_date_time_t * p_date_time,
+                                             uint8_t *               p_encoded_data)
+{
+    uint8_t len = uint16_encode(p_date_time->year, p_encoded_data);
+
+    p_encoded_data[len++] = p_date_time->month;
+    p_encoded_data[len++] = p_date_time->day;
+    p_encoded_data[len++] = p_date_time->hours;
+    p_encoded_data[len++] = p_date_time->minutes;
+    p_encoded_data[len++] = p_date_time->seconds;
+
+    return len;
+}
+
+static __INLINE uint8_t ble_date_time_decode(ble_date_time_t * p_date_time,
+                                             const uint8_t *   p_encoded_data)
+{
+    uint8_t len = sizeof(uint16_t);
+
+    p_date_time->year    = uint16_decode(p_encoded_data);
+    p_date_time->month   = p_encoded_data[len++];
+    p_date_time->day     = p_encoded_data[len++];
+    p_date_time->hours   = p_encoded_data[len++];
+    p_date_time->minutes = p_encoded_data[len++];
+    p_date_time->seconds = p_encoded_data[len++];
+
+    return len;
+}
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_DATE_TIME_H__
+
+/** @} */

components/ble/common/ble_gatt_db.h

@@ -0,0 +1,92 @@
+/**
+ * Copyright (c) 2016 - 2021, 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_lib_gatt_db GATT Database Service Structure
+ * @{
+ * @ingroup  ble_sdk_lib
+ */
+
+#ifndef BLE_GATT_DB_H__
+#define BLE_GATT_DB_H__
+
+#include <stdint.h>
+#include "ble.h"
+#include "ble_gattc.h"
+#include "sdk_config.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef BLE_GATT_DB_MAX_CHARS
+#define BLE_GATT_DB_MAX_CHARS 6        /**< The maximum number of characteristics present in a service record. */
+#endif // BLE_GATT_DB_MAX_CHARS
+
+/**@brief Structure for holding the characteristic and the handle of its CCCD present on a server.
+ */
+typedef struct
+{
+    ble_gattc_char_t characteristic;    /**< Structure containing information about the characteristic. */
+    uint16_t         cccd_handle;       /**< CCCD Handle value for this characteristic. This will be set to BLE_GATT_HANDLE_INVALID if a CCCD is not present at the server. */
+    uint16_t         ext_prop_handle;   /**< Extended Properties Handle value for this characteristic. This will be set to BLE_GATT_HANDLE_INVALID if an Extended Properties descriptor is not present at the server. */
+    uint16_t         user_desc_handle;  /**< User Description Handle value for this characteristic. This will be set to BLE_GATT_HANDLE_INVALID if a User Description descriptor is not present at the server. */
+    uint16_t         report_ref_handle; /**< Report Reference Handle value for this characteristic. This will be set to BLE_GATT_HANDLE_INVALID if a Report Reference descriptor is not present at the server. */
+} ble_gatt_db_char_t;
+
+/**@brief Structure for holding information about the service and the characteristics present on a
+ *        server.
+ */
+typedef struct
+{
+    ble_uuid_t               srv_uuid;                                  /**< UUID of the service. */
+    uint8_t                  char_count;                                /**< Number of characteristics present in the service. */
+    ble_gattc_handle_range_t handle_range;                              /**< Service Handle Range. */
+    ble_gatt_db_char_t       charateristics[BLE_GATT_DB_MAX_CHARS];     /**< Array of information related to the characteristics present in the service. This list can extend further than one. */
+} ble_gatt_db_srv_t;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLE_GATT_DB_H__ */
+
+/** @} */

components/ble/common/ble_sensor_location.h

@@ -0,0 +1,76 @@
+/**
+ * Copyright (c) 2012 - 2021, 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.
+ *
+ */
+/* Attention!
+*  To maintain compliance with Nordic Semiconductor ASA’s Bluetooth profile
+*  qualification listings, this section of source code must not be modified.
+*/
+
+#ifndef BLE_SENSOR_LOCATION_H__
+#define BLE_SENSOR_LOCATION_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+    BLE_SENSOR_LOCATION_OTHER        = 0 ,  /**<-- Other        */
+    BLE_SENSOR_LOCATION_TOP_OF_SHOE  = 1 ,  /**<-- Top of shoe  */
+    BLE_SENSOR_LOCATION_IN_SHOE      = 2 ,  /**<-- In shoe      */
+    BLE_SENSOR_LOCATION_HIP          = 3 ,  /**<-- Hip          */
+    BLE_SENSOR_LOCATION_FRONT_WHEEL  = 4 ,  /**<-- Front Wheel  */
+    BLE_SENSOR_LOCATION_LEFT_CRANK   = 5 ,  /**<-- Left Crank   */
+    BLE_SENSOR_LOCATION_RIGHT_CRANK  = 6 ,  /**<-- Right Crank  */
+    BLE_SENSOR_LOCATION_LEFT_PEDAL   = 7 ,  /**<-- Left Pedal   */
+    BLE_SENSOR_LOCATION_RIGHT_PEDAL  = 8 ,  /**<-- Right Pedal  */
+    BLE_SENSOR_LOCATION_FRONT_HUB    = 9 ,  /**<-- Front Hub    */
+    BLE_SENSOR_LOCATION_REAR_DROPOUT = 10,  /**<-- Rear Dropout */
+    BLE_SENSOR_LOCATION_CHAINSTAY    = 11,  /**<-- Chainstay    */
+    BLE_SENSOR_LOCATION_REAR_WHEEL   = 12,  /**<-- Rear Wheel   */
+    BLE_SENSOR_LOCATION_REAR_HUB     = 13,  /**<-- Rear Hub     */
+}ble_sensor_location_t;
+
+#define BLE_NB_MAX_SENSOR_LOCATIONS 14
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_SENSOR_LOCATION_H__

components/ble/common/ble_srv_common.c

@@ -0,0 +1,237 @@
+/**
+ * Copyright (c) 2012 - 2021, 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.
+ *
+ */
+/* Attention!
+ * To maintain compliance with Nordic Semiconductor ASA's Bluetooth profile
+ * qualification listings, this section of source code must not be modified.
+ */
+
+#include "ble_srv_common.h"
+#include <string.h>
+#include "nordic_common.h"
+#include "app_error.h"
+#include "ble.h"
+
+bool ble_srv_is_notification_enabled(uint8_t const * p_encoded_data)
+{
+    uint16_t cccd_value = uint16_decode(p_encoded_data);
+    return ((cccd_value & BLE_GATT_HVX_NOTIFICATION) != 0);
+}
+
+bool ble_srv_is_indication_enabled(uint8_t const * p_encoded_data)
+{
+    uint16_t cccd_value = uint16_decode(p_encoded_data);
+    return ((cccd_value & BLE_GATT_HVX_INDICATION) != 0);
+}
+
+uint8_t ble_srv_report_ref_encode(uint8_t                    * p_encoded_buffer,
+                                  const ble_srv_report_ref_t * p_report_ref)
+{
+    uint8_t len = 0;
+
+    p_encoded_buffer[len++] = p_report_ref->report_id;
+    p_encoded_buffer[len++] = p_report_ref->report_type;
+
+    APP_ERROR_CHECK_BOOL(len == BLE_SRV_ENCODED_REPORT_REF_LEN);
+    return len;
+}
+
+
+void ble_srv_ascii_to_utf8(ble_srv_utf8_str_t * p_utf8, char * p_ascii)
+{
+    p_utf8->length = (uint16_t)strlen(p_ascii);
+    p_utf8->p_str  = (uint8_t *)p_ascii;
+}
+
+
+/**@brief Function for setting security requirements of a characteristic.
+ *
+ * @param[in]  level   required security level.
+ * @param[out] p_perm  Characteristic security requirements.
+ *
+ * @return     encoded security level and security mode.
+ */
+static inline void set_security_req(security_req_t level, ble_gap_conn_sec_mode_t * p_perm)
+{
+
+
+    BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(p_perm);
+    switch (level)
+    {
+        case SEC_NO_ACCESS:
+            BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(p_perm);
+        break;
+        case SEC_OPEN:
+            BLE_GAP_CONN_SEC_MODE_SET_OPEN(p_perm);
+        break;
+        case SEC_JUST_WORKS:
+            BLE_GAP_CONN_SEC_MODE_SET_ENC_NO_MITM(p_perm);
+        break;
+        case SEC_MITM:
+            BLE_GAP_CONN_SEC_MODE_SET_ENC_WITH_MITM(p_perm);
+        break;
+        case SEC_SIGNED:
+            BLE_GAP_CONN_SEC_MODE_SET_SIGNED_NO_MITM(p_perm);
+        break;
+        case SEC_SIGNED_MITM:
+            BLE_GAP_CONN_SEC_MODE_SET_SIGNED_WITH_MITM(p_perm);
+        break;
+    }
+    return;
+}
+
+
+uint32_t characteristic_add(uint16_t                   service_handle,
+                            ble_add_char_params_t *    p_char_props,
+                            ble_gatts_char_handles_t * p_char_handle)
+{
+    ble_gatts_char_md_t char_md;
+    ble_gatts_attr_t    attr_char_value;
+    ble_uuid_t          char_uuid;
+    ble_gatts_attr_md_t attr_md;
+    ble_gatts_attr_md_t user_descr_attr_md;
+    ble_gatts_attr_md_t cccd_md;
+
+    if (p_char_props->uuid_type == 0)
+    {
+        char_uuid.type = BLE_UUID_TYPE_BLE;
+    }
+    else
+    {
+        char_uuid.type = p_char_props->uuid_type;
+    }
+    char_uuid.uuid = p_char_props->uuid;
+
+    memset(&attr_md, 0, sizeof(ble_gatts_attr_md_t));
+    set_security_req(p_char_props->read_access, &attr_md.read_perm);
+    set_security_req(p_char_props->write_access, & attr_md.write_perm);
+    attr_md.rd_auth    = (p_char_props->is_defered_read ? 1 : 0);
+    attr_md.wr_auth    = (p_char_props->is_defered_write ? 1 : 0);
+    attr_md.vlen       = (p_char_props->is_var_len ? 1 : 0);
+    attr_md.vloc       = (p_char_props->is_value_user ? BLE_GATTS_VLOC_USER : BLE_GATTS_VLOC_STACK);
+
+
+    memset(&char_md, 0, sizeof(ble_gatts_char_md_t));
+    if ((p_char_props->char_props.notify == 1)||(p_char_props->char_props.indicate == 1))
+    {
+
+        memset(&cccd_md, 0, sizeof(cccd_md));
+        set_security_req(p_char_props->cccd_write_access, &cccd_md.write_perm);
+        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
+
+        cccd_md.vloc       = BLE_GATTS_VLOC_STACK;
+
+        char_md.p_cccd_md  = &cccd_md;
+    }
+    char_md.char_props     = p_char_props->char_props;
+    char_md.char_ext_props = p_char_props->char_ext_props;
+
+    memset(&attr_char_value, 0, sizeof(ble_gatts_attr_t));
+    attr_char_value.p_uuid    = &char_uuid;
+    attr_char_value.p_attr_md = &attr_md;
+    attr_char_value.max_len   = p_char_props->max_len;
+    if (p_char_props->p_init_value != NULL)
+    {
+        attr_char_value.init_len  = p_char_props->init_len;
+        attr_char_value.p_value   = p_char_props->p_init_value;
+    }
+    if (p_char_props->p_user_descr != NULL)
+    {
+        memset(&user_descr_attr_md, 0, sizeof(ble_gatts_attr_md_t));
+        char_md.char_user_desc_max_size = p_char_props->p_user_descr->max_size;
+        char_md.char_user_desc_size     = p_char_props->p_user_descr->size;
+        char_md.p_char_user_desc        = p_char_props->p_user_descr->p_char_user_desc;
+
+        char_md.p_user_desc_md          = &user_descr_attr_md;
+
+        set_security_req(p_char_props->p_user_descr->read_access, &user_descr_attr_md.read_perm);
+        set_security_req(p_char_props->p_user_descr->write_access, &user_descr_attr_md.write_perm);
+
+        user_descr_attr_md.rd_auth      = (p_char_props->p_user_descr->is_defered_read ? 1 : 0);
+        user_descr_attr_md.wr_auth      = (p_char_props->p_user_descr->is_defered_write ? 1 : 0);
+        user_descr_attr_md.vlen         = (p_char_props->p_user_descr->is_var_len ? 1 : 0);
+        user_descr_attr_md.vloc         = (p_char_props->p_user_descr->is_value_user ? BLE_GATTS_VLOC_USER : BLE_GATTS_VLOC_STACK);
+    }
+    if (p_char_props->p_presentation_format != NULL)
+    {
+        char_md.p_char_pf = p_char_props->p_presentation_format;
+    }
+    return sd_ble_gatts_characteristic_add(service_handle,
+                                           &char_md,
+                                           &attr_char_value,
+                                           p_char_handle);
+}
+
+
+uint32_t descriptor_add(uint16_t                   char_handle,
+                        ble_add_descr_params_t *   p_descr_props,
+                        uint16_t *                 p_descr_handle)
+{
+    ble_gatts_attr_t    descr_params;
+    ble_uuid_t          desc_uuid;
+    ble_gatts_attr_md_t attr_md;
+
+    memset(&descr_params, 0, sizeof(descr_params));
+    if (p_descr_props->uuid_type == 0)
+    {
+        desc_uuid.type = BLE_UUID_TYPE_BLE;
+    }
+    else
+    {
+        desc_uuid.type = p_descr_props->uuid_type;
+    }
+    desc_uuid.uuid = p_descr_props->uuid;
+    descr_params.p_uuid = &desc_uuid;
+
+    set_security_req(p_descr_props->read_access, &attr_md.read_perm);
+    set_security_req(p_descr_props->write_access,&attr_md.write_perm);
+
+    attr_md.rd_auth        = (p_descr_props->is_defered_read ? 1 : 0);
+    attr_md.wr_auth        = (p_descr_props->is_defered_write ? 1 : 0);
+    attr_md.vlen           = (p_descr_props->is_var_len ? 1 : 0);
+    attr_md.vloc           = (p_descr_props->is_value_user ? BLE_GATTS_VLOC_USER : BLE_GATTS_VLOC_STACK);
+    descr_params.p_attr_md = &attr_md;
+
+    descr_params.init_len  = p_descr_props->init_len;
+    descr_params.init_offs = p_descr_props->init_offs;
+    descr_params.max_len   = p_descr_props->max_len;
+    descr_params.p_value   = p_descr_props->p_value;
+
+    return sd_ble_gatts_descriptor_add(char_handle, &descr_params, p_descr_handle);
+}

components/ble/common/ble_srv_common.h

@@ -0,0 +1,409 @@
+/**
+ * Copyright (c) 2012 - 2021, 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_srv_common Common service definitions
+ * @{
+ * @ingroup ble_sdk_srv
+ * @brief Constants, type definitions, and functions that are common to all services.
+ */
+
+#ifndef BLE_SRV_COMMON_H__
+#define BLE_SRV_COMMON_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "ble_types.h"
+#include "app_util.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "ble_gatt.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup UUID_SERVICES Service UUID definitions
+ * @{ */
+#define BLE_UUID_ALERT_NOTIFICATION_SERVICE                      0x1811     /**< Alert Notification service UUID. */
+#define BLE_UUID_BATTERY_SERVICE                                 0x180F     /**< Battery service UUID. */
+#define BLE_UUID_BLOOD_PRESSURE_SERVICE                          0x1810     /**< Blood Pressure service UUID. */
+#define BLE_UUID_CURRENT_TIME_SERVICE                            0x1805     /**< Current Time service UUID. */
+#define BLE_UUID_CYCLING_SPEED_AND_CADENCE                       0x1816     /**< Cycling Speed and Cadence service UUID. */
+#define BLE_UUID_LOCATION_AND_NAVIGATION_SERVICE                 0x1819     /**< Location and Navigation service UUID. */
+#define BLE_UUID_DEVICE_INFORMATION_SERVICE                      0x180A     /**< Device Information service UUID. */
+#define BLE_UUID_GLUCOSE_SERVICE                                 0x1808     /**< Glucose service UUID. */
+#define BLE_UUID_HEALTH_THERMOMETER_SERVICE                      0x1809     /**< Health Thermometer service UUID. */
+#define BLE_UUID_HEART_RATE_SERVICE                              0x180D     /**< Heart Rate service UUID. */
+#define BLE_UUID_HUMAN_INTERFACE_DEVICE_SERVICE                  0x1812     /**< Human Interface Device service UUID. */
+#define BLE_UUID_IMMEDIATE_ALERT_SERVICE                         0x1802     /**< Immediate Alert service UUID. */
+#define BLE_UUID_LINK_LOSS_SERVICE                               0x1803     /**< Link Loss service UUID. */
+#define BLE_UUID_NEXT_DST_CHANGE_SERVICE                         0x1807     /**< Next Dst Change service UUID. */
+#define BLE_UUID_PHONE_ALERT_STATUS_SERVICE                      0x180E     /**< Phone Alert Status service UUID. */
+#define BLE_UUID_REFERENCE_TIME_UPDATE_SERVICE                   0x1806     /**< Reference Time Update service UUID. */
+#define BLE_UUID_RUNNING_SPEED_AND_CADENCE                       0x1814     /**< Running Speed and Cadence service UUID. */
+#define BLE_UUID_SCAN_PARAMETERS_SERVICE                         0x1813     /**< Scan Parameters service UUID. */
+#define BLE_UUID_TX_POWER_SERVICE                                0x1804     /**< TX Power service UUID. */
+#define BLE_UUID_IPSP_SERVICE                                    0x1820     /**< Internet Protocol Support service UUID. */
+#define BLE_UUID_BMS_SERVICE                                     0x181E     /**< BOND MANAGEMENT service UUID*/
+#define BLE_UUID_CGM_SERVICE                                     0x181F     /**< Continuous Glucose Monitoring service UUID*/
+#define BLE_UUID_PLX_SERVICE                                     0x1822     /**< Pulse Oximeter Service UUID*/
+#define BLE_UUID_OTS_SERVICE                                     0x1825     /**< Object Transfer Service UUID*/
+
+/** @} */
+
+/** @defgroup UUID_CHARACTERISTICS Characteristic UUID definitions
+ * @{ */
+#define BLE_UUID_REMOVABLE_CHAR                                  0x2A3A     /**< Removable characteristic UUID. */
+#define BLE_UUID_SERVICE_REQUIRED_CHAR                           0x2A3B     /**< Service Required characteristic UUID. */
+#define BLE_UUID_ALERT_CATEGORY_ID_CHAR                          0x2A43     /**< Alert Category Id characteristic UUID. */
+#define BLE_UUID_ALERT_CATEGORY_ID_BIT_MASK_CHAR                 0x2A42     /**< Alert Category Id Bit Mask characteristic UUID. */
+#define BLE_UUID_ALERT_LEVEL_CHAR                                0x2A06     /**< Alert Level characteristic UUID. */
+#define BLE_UUID_ALERT_NOTIFICATION_CONTROL_POINT_CHAR           0x2A44     /**< Alert Notification Control Point characteristic UUID. */
+#define BLE_UUID_ALERT_STATUS_CHAR                               0x2A3F     /**< Alert Status characteristic UUID. */
+#define BLE_UUID_BATTERY_LEVEL_CHAR                              0x2A19     /**< Battery Level characteristic UUID. */
+#define BLE_UUID_BLOOD_PRESSURE_FEATURE_CHAR                     0x2A49     /**< Blood Pressure Feature characteristic UUID. */
+#define BLE_UUID_BLOOD_PRESSURE_MEASUREMENT_CHAR                 0x2A35     /**< Blood Pressure Measurement characteristic UUID. */
+#define BLE_UUID_BODY_SENSOR_LOCATION_CHAR                       0x2A38     /**< Body Sensor Location characteristic UUID. */
+#define BLE_UUID_BOOT_KEYBOARD_INPUT_REPORT_CHAR                 0x2A22     /**< Boot Keyboard Input Report characteristic UUID. */
+#define BLE_UUID_BOOT_KEYBOARD_OUTPUT_REPORT_CHAR                0x2A32     /**< Boot Keyboard Output Report characteristic UUID. */
+#define BLE_UUID_BOOT_MOUSE_INPUT_REPORT_CHAR                    0x2A33     /**< Boot Mouse Input Report characteristic UUID. */
+#define BLE_UUID_CURRENT_TIME_CHAR                               0x2A2B     /**< Current Time characteristic UUID. */
+#define BLE_UUID_DATE_TIME_CHAR                                  0x2A08     /**< Date Time characteristic UUID. */
+#define BLE_UUID_DAY_DATE_TIME_CHAR                              0x2A0A     /**< Day Date Time characteristic UUID. */
+#define BLE_UUID_DAY_OF_WEEK_CHAR                                0x2A09     /**< Day Of Week characteristic UUID. */
+#define BLE_UUID_DST_OFFSET_CHAR                                 0x2A0D     /**< Dst Offset characteristic UUID. */
+#define BLE_UUID_EXACT_TIME_256_CHAR                             0x2A0C     /**< Exact Time 256 characteristic UUID. */
+#define BLE_UUID_FIRMWARE_REVISION_STRING_CHAR                   0x2A26     /**< Firmware Revision String characteristic UUID. */
+#define BLE_UUID_GLUCOSE_FEATURE_CHAR                            0x2A51     /**< Glucose Feature characteristic UUID. */
+#define BLE_UUID_GLUCOSE_MEASUREMENT_CHAR                        0x2A18     /**< Glucose Measurement characteristic UUID. */
+#define BLE_UUID_GLUCOSE_MEASUREMENT_CONTEXT_CHAR                0x2A34     /**< Glucose Measurement Context characteristic UUID. */
+#define BLE_UUID_HARDWARE_REVISION_STRING_CHAR                   0x2A27     /**< Hardware Revision String characteristic UUID. */
+#define BLE_UUID_HEART_RATE_CONTROL_POINT_CHAR                   0x2A39     /**< Heart Rate Control Point characteristic UUID. */
+#define BLE_UUID_HEART_RATE_MEASUREMENT_CHAR                     0x2A37     /**< Heart Rate Measurement characteristic UUID. */
+#define BLE_UUID_HID_CONTROL_POINT_CHAR                          0x2A4C     /**< Hid Control Point characteristic UUID. */
+#define BLE_UUID_HID_INFORMATION_CHAR                            0x2A4A     /**< Hid Information characteristic UUID. */
+#define BLE_UUID_IEEE_REGULATORY_CERTIFICATION_DATA_LIST_CHAR    0x2A2A     /**< IEEE Regulatory Certification Data List characteristic UUID. */
+#define BLE_UUID_INTERMEDIATE_CUFF_PRESSURE_CHAR                 0x2A36     /**< Intermediate Cuff Pressure characteristic UUID. */
+#define BLE_UUID_INTERMEDIATE_TEMPERATURE_CHAR                   0x2A1E     /**< Intermediate Temperature characteristic UUID. */
+#define BLE_UUID_LOCAL_TIME_INFORMATION_CHAR                     0x2A0F     /**< Local Time Information characteristic UUID. */
+#define BLE_UUID_MANUFACTURER_NAME_STRING_CHAR                   0x2A29     /**< Manufacturer Name String characteristic UUID. */
+#define BLE_UUID_MEASUREMENT_INTERVAL_CHAR                       0x2A21     /**< Measurement Interval characteristic UUID. */
+#define BLE_UUID_MODEL_NUMBER_STRING_CHAR                        0x2A24     /**< Model Number String characteristic UUID. */
+#define BLE_UUID_UNREAD_ALERT_CHAR                               0x2A45     /**< Unread Alert characteristic UUID. */
+#define BLE_UUID_NEW_ALERT_CHAR                                  0x2A46     /**< New Alert characteristic UUID. */
+#define BLE_UUID_PNP_ID_CHAR                                     0x2A50     /**< PNP Id characteristic UUID. */
+#define BLE_UUID_PROTOCOL_MODE_CHAR                              0x2A4E     /**< Protocol Mode characteristic UUID. */
+#define BLE_UUID_RECORD_ACCESS_CONTROL_POINT_CHAR                0x2A52     /**< Record Access Control Point characteristic UUID. */
+#define BLE_UUID_REFERENCE_TIME_INFORMATION_CHAR                 0x2A14     /**< Reference Time Information characteristic UUID. */
+#define BLE_UUID_REPORT_CHAR                                     0x2A4D     /**< Report characteristic UUID. */
+#define BLE_UUID_REPORT_MAP_CHAR                                 0x2A4B     /**< Report Map characteristic UUID. */
+#define BLE_UUID_RINGER_CONTROL_POINT_CHAR                       0x2A40     /**< Ringer Control Point characteristic UUID. */
+#define BLE_UUID_RINGER_SETTING_CHAR                             0x2A41     /**< Ringer Setting characteristic UUID. */
+#define BLE_UUID_SCAN_INTERVAL_WINDOW_CHAR                       0x2A4F     /**< Scan Interval Window characteristic UUID. */
+#define BLE_UUID_SCAN_REFRESH_CHAR                               0x2A31     /**< Scan Refresh characteristic UUID. */
+#define BLE_UUID_SERIAL_NUMBER_STRING_CHAR                       0x2A25     /**< Serial Number String characteristic UUID. */
+#define BLE_UUID_SOFTWARE_REVISION_STRING_CHAR                   0x2A28     /**< Software Revision String characteristic UUID. */
+#define BLE_UUID_SUPPORTED_NEW_ALERT_CATEGORY_CHAR               0x2A47     /**< Supported New Alert Category characteristic UUID. */
+#define BLE_UUID_SUPPORTED_UNREAD_ALERT_CATEGORY_CHAR            0x2A48     /**< Supported Unread Alert Category characteristic UUID. */
+#define BLE_UUID_SYSTEM_ID_CHAR                                  0x2A23     /**< System Id characteristic UUID. */
+#define BLE_UUID_TEMPERATURE_MEASUREMENT_CHAR                    0x2A1C     /**< Temperature Measurement characteristic UUID. */
+#define BLE_UUID_TEMPERATURE_TYPE_CHAR                           0x2A1D     /**< Temperature Type characteristic UUID. */
+#define BLE_UUID_TIME_ACCURACY_CHAR                              0x2A12     /**< Time Accuracy characteristic UUID. */
+#define BLE_UUID_TIME_SOURCE_CHAR                                0x2A13     /**< Time Source characteristic UUID. */
+#define BLE_UUID_TIME_UPDATE_CONTROL_POINT_CHAR                  0x2A16     /**< Time Update Control Point characteristic UUID. */
+#define BLE_UUID_TIME_UPDATE_STATE_CHAR                          0x2A17     /**< Time Update State characteristic UUID. */
+#define BLE_UUID_TIME_WITH_DST_CHAR                              0x2A11     /**< Time With Dst characteristic UUID. */
+#define BLE_UUID_TIME_ZONE_CHAR                                  0x2A0E     /**< Time Zone characteristic UUID. */
+#define BLE_UUID_TX_POWER_LEVEL_CHAR                             0x2A07     /**< TX Power Level characteristic UUID. */
+#define BLE_UUID_CSC_FEATURE_CHAR                                0x2A5C     /**< Cycling Speed and Cadence Feature characteristic UUID. */
+#define BLE_UUID_CSC_MEASUREMENT_CHAR                            0x2A5B     /**< Cycling Speed and Cadence Measurement characteristic UUID. */
+#define BLE_UUID_RSC_FEATURE_CHAR                                0x2A54     /**< Running Speed and Cadence Feature characteristic UUID. */
+#define BLE_UUID_SC_CTRLPT_CHAR                                  0x2A55     /**< Speed and Cadence Control Point UUID. */
+#define BLE_UUID_RSC_MEASUREMENT_CHAR                            0x2A53     /**< Running Speed and Cadence Measurement characteristic UUID. */
+#define BLE_UUID_SENSOR_LOCATION_CHAR                            0x2A5D     /**< Sensor Location characteristic UUID. */
+#define BLE_UUID_EXTERNAL_REPORT_REF_DESCR                       0x2907     /**< External Report Reference descriptor UUID. */
+#define BLE_UUID_REPORT_REF_DESCR                                0x2908     /**< Report Reference descriptor UUID. */
+#define BLE_UUID_LN_FEATURE_CHAR                                 0x2A6A     /**< Location Navigation Service, Feature characteristic UUID. */
+#define BLE_UUID_LN_POSITION_QUALITY_CHAR                        0x2A69     /**< Location Navigation Service, Position quality UUID. */
+#define BLE_UUID_LN_LOCATION_AND_SPEED_CHAR                      0x2A67     /**< Location Navigation Service, Location and Speed characteristic UUID. */
+#define BLE_UUID_LN_NAVIGATION_CHAR                              0x2A68     /**< Location Navigation Service, Navigation characteristic UUID. */
+#define BLE_UUID_LN_CONTROL_POINT_CHAR                           0x2A6B     /**< Location Navigation Service, Control point characteristic UUID. */
+#define BLE_UUID_BMS_CTRLPT                                      0x2AA4     /**< BMS Control Point characteristic UUID. */
+#define BLE_UUID_BMS_FEATURE                                     0x2AA5     /**< BMS Feature characteristic UUID. */
+#define BLE_UUID_CGM_MEASUREMENT                                 0x2AA7     /**< CGM Service, Measurement characteristic UUID*/
+#define BLE_UUID_CGM_FEATURE                                     0x2AA8     /**< CGM Service, Feature characteristic UUID*/
+#define BLE_UUID_CGM_STATUS                                      0x2AA9     /**< CGM Service, Status characteristic UUID*/
+#define BLE_UUID_CGM_SESSION_START_TIME                          0x2AAA     /**< CGM Service, session start time characteristic UUID*/
+#define BLE_UUID_CGM_SESSION_RUN_TIME                            0x2AAB     /**< CGM Service, session run time characteristic UUID*/
+#define BLE_UUID_CGM_SPECIFIC_OPS_CTRLPT                         0x2AAC     /**< CGM Service, specific ops ctrlpt characteristic UUID*/
+#define BLE_UUID_PLX_SPOT_CHECK_MEAS                             0x2A5E     /**< PLX Service, spot check measurement characteristic UUID*/
+#define BLE_UUID_PLX_CONTINUOUS_MEAS                             0x2A5F     /**< PLX Service, continuous measurement characteristic UUID*/
+#define BLE_UUID_PLX_FEATURES                                    0x2A60     /**< PLX Service, feature characteristic UUID*/
+#define BLE_UUID_OTS_FEATURES                                    0x2ABD     /**< OTS Service, feature characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_NAME                                 0x2ABE     /**< OTS Service, Object Name characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_TYPE                                 0x2ABF     /**< OTS Service, Object Type characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_SIZE                                 0x2AC0     /**< OTS Service, Object Size characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_FIRST_CREATED                        0x2AC1     /**< OTS Service, Object First Created characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_LAST_MODIFIED                        0x2AC2     /**< OTS Service, Object Last Modified characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_ID                                   0x2AC3     /**< OTS Service, Object ID characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_PROPERTIES                           0x2AC4     /**< OTS Service, Object Properties characteristic UUID*/
+#define BLE_UUID_OTS_OACP                                        0x2AC5     /**< OTS Service, Object Action Control Point characteristic UUID*/
+#define BLE_UUID_OTS_OLCP                                        0x2AC6     /**< OTS Service, Object List Control Point characteristic UUID*/
+#define BLE_UUID_OTS_LF                                          0x2AC7     /**< OTS Service, Object List Filter characteristic UUID*/
+#define BLE_UUID_OTS_OBJECT_CHANGED                              0x2AC8     /**< OTS Service, Object Changed characteristic UUID*/
+
+
+
+
+/** @} */
+
+/** @defgroup ALERT_LEVEL_VALUES Definitions for the Alert Level characteristic values
+ * @{ */
+#define BLE_CHAR_ALERT_LEVEL_NO_ALERT                            0x00       /**< No Alert. */
+#define BLE_CHAR_ALERT_LEVEL_MILD_ALERT                          0x01       /**< Mild Alert. */
+#define BLE_CHAR_ALERT_LEVEL_HIGH_ALERT                          0x02       /**< High Alert. */
+/** @} */
+
+#define BLE_SRV_ENCODED_REPORT_REF_LEN                           2          /**< The length of an encoded Report Reference Descriptor. */
+#define BLE_CCCD_VALUE_LEN                                       2          /**< The length of a CCCD value. */
+
+/**@brief Type definition for error handler function that will be called in case of an error in
+ *        a service or a service library module. */
+typedef void (*ble_srv_error_handler_t) (uint32_t nrf_error);
+
+
+
+/**@brief Value of a Report Reference descriptor.
+ *
+ * @details This is mapping information that maps the parent characteristic to the Report ID(s) and
+ *          Report Type(s) defined within a Report Map characteristic.
+ */
+typedef struct
+{
+    uint8_t report_id;                                  /**< Non-zero value if there is more than one instance of the same Report Type */
+    uint8_t report_type;                                /**< Type of Report characteristic (see @ref BLE_HIDS_REPORT_TYPE) */
+} ble_srv_report_ref_t;
+
+/**@brief UTF-8 string data type.
+ *
+ * @note The type can only hold a pointer to the string data (i.e. not the actual data).
+ */
+typedef struct
+{
+    uint16_t  length;                                   /**< String length. */
+    uint8_t * p_str;                                    /**< String data. */
+} ble_srv_utf8_str_t;
+
+
+/**@brief Security settings structure.
+ * @details This structure contains the security options needed during initialization of the
+ *          service.
+ */
+typedef struct
+{
+    ble_gap_conn_sec_mode_t read_perm;                  /**< Read permissions. */
+    ble_gap_conn_sec_mode_t write_perm;                 /**< Write permissions. */
+} ble_srv_security_mode_t;
+
+/**@brief Security settings structure.
+ * @details This structure contains the security options needed during initialization of the
+ *          service. It can be used when the characteristics contains a CCCD.
+ */
+typedef struct
+{
+    ble_gap_conn_sec_mode_t cccd_write_perm;            /**< Write permissions for Client Characteristic Configuration Descriptor. */
+    ble_gap_conn_sec_mode_t read_perm;                  /**< Read permissions. */
+    ble_gap_conn_sec_mode_t write_perm;                 /**< Write permissions. */
+} ble_srv_cccd_security_mode_t;
+
+/**@brief Function for decoding a CCCD value, and then testing if notification is
+ *        enabled.
+ *
+ * @param[in]   p_encoded_data   Buffer where the encoded CCCD is stored.
+ *
+ * @retval      TRUE If notification is enabled.
+ * @retval      FALSE Otherwise.
+ */
+bool ble_srv_is_notification_enabled(uint8_t const * p_encoded_data);
+
+
+/**@brief Function for decoding a CCCD value, and then testing if indication is
+ *        enabled.
+ *
+ * @param[in]   p_encoded_data   Buffer where the encoded CCCD is stored.
+ *
+ * @retval      TRUE If indication is enabled.
+ * @retval      FALSE Otherwise.
+ */
+bool ble_srv_is_indication_enabled(uint8_t const * p_encoded_data);
+
+
+/**@brief Function for encoding a Report Reference Descriptor.
+ *
+ * @param[in]   p_encoded_buffer  The buffer of the encoded data.
+ * @param[in]   p_report_ref      Report Reference value to be encoded.
+ *
+ * @return      Length of the encoded data.
+ */
+uint8_t ble_srv_report_ref_encode(uint8_t *                    p_encoded_buffer,
+                                  const ble_srv_report_ref_t * p_report_ref);
+
+/**@brief Function for making a UTF-8 structure refer to an ASCII string.
+ *
+ * @param[out]  p_utf8   UTF-8 structure to be set.
+ * @param[in]   p_ascii  ASCII string to be referred to.
+ */
+void ble_srv_ascii_to_utf8(ble_srv_utf8_str_t * p_utf8, char * p_ascii);
+
+
+/**@brief Security Access enumeration.
+ * @details This enumeration gives the possible requirements for accessing a characteristic value.
+ */
+typedef enum
+{
+    SEC_NO_ACCESS    = 0,            /**< Not possible to access. */
+    SEC_OPEN         = 1,            /**< Access open. */
+    SEC_JUST_WORKS   = 2,            /**< Access possible with 'Just Works' security at least. */
+    SEC_MITM         = 3,            /**< Access possible with 'MITM' security at least. */
+    SEC_SIGNED       = 4,            /**< Access possible with 'signed' security at least. */
+    SEC_SIGNED_MITM  = 5             /**< Access possible with 'signed and MITM' security at least. */
+}security_req_t;
+
+
+/**@brief Characteristic User Descriptor parameters.
+ * @details This structure contains the parameters for User Descriptor.
+ */
+typedef struct
+{
+    uint16_t               max_size;                      /**< Maximum size of the user descriptor*/
+    uint16_t               size;                          /**< Size of the user descriptor*/
+    uint8_t                *p_char_user_desc;             /**< User descriptor content, pointer to a UTF-8 encoded string (non-NULL terminated)*/
+    bool                   is_var_len;                    /**< Indicates if the user descriptor has variable length.*/
+    ble_gatt_char_props_t  char_props;                    /**< user descriptor properties.*/
+    bool                   is_defered_read;               /**< Indicate if deferred read operations are supported.*/
+    bool                   is_defered_write;              /**< Indicate if deferred write operations are supported.*/
+    security_req_t         read_access;                   /**< Security requirement for reading the user descriptor.*/
+    security_req_t         write_access;                  /**< Security requirement for writing the user descriptor.*/
+    bool                   is_value_user;                 /**< Indicate if the content of the characteristic is to be stored in the application (user) or in the stack.*/
+}ble_add_char_user_desc_t;
+
+
+/**@brief Add characteristic parameters structure.
+ * @details This structure contains the parameters needed to use the @ref characteristic_add function.
+ */
+typedef struct
+{
+    uint16_t                    uuid;                     /**< Characteristic UUID (16 bits UUIDs).*/
+    uint8_t                     uuid_type;                /**< Base UUID. If 0, the Bluetooth SIG UUID will be used. Otherwise, this should be a value returned by @ref sd_ble_uuid_vs_add when adding the base UUID.*/
+    uint16_t                    max_len;                  /**< Maximum length of the characteristic value.*/
+    uint16_t                    init_len;                 /**< Initial length of the characteristic value.*/
+    uint8_t *                   p_init_value;             /**< Initial encoded value of the characteristic.*/
+    bool                        is_var_len;               /**< Indicates if the characteristic value has variable length.*/
+    ble_gatt_char_props_t       char_props;               /**< Characteristic properties.*/
+    ble_gatt_char_ext_props_t   char_ext_props;           /**< Characteristic extended properties.*/
+    bool                        is_defered_read;          /**< Indicate if deferred read operations are supported.*/
+    bool                        is_defered_write;         /**< Indicate if deferred write operations are supported.*/
+    security_req_t              read_access;              /**< Security requirement for reading the characteristic value.*/
+    security_req_t              write_access;             /**< Security requirement for writing the characteristic value.*/
+    security_req_t              cccd_write_access;        /**< Security requirement for writing the characteristic's CCCD.*/
+    bool                        is_value_user;            /**< Indicate if the content of the characteristic is to be stored in the application (user) or in the stack.*/
+    ble_add_char_user_desc_t    *p_user_descr;            /**< Pointer to user descriptor if needed*/
+    ble_gatts_char_pf_t         *p_presentation_format;   /**< Pointer to characteristic format if needed*/
+} ble_add_char_params_t;
+
+
+/**@brief Add descriptor parameters structure.
+ * @details This structure contains the parameters needed to use the @ref descriptor_add function.
+ */
+typedef struct
+{
+    uint16_t       uuid;                     /**< descriptor UUID (16 bits UUIDs).*/
+    uint8_t        uuid_type;                /**< Base UUID. If 0, the Bluetooth SIG UUID will be used. Otherwise, this should be a value returned by @ref sd_ble_uuid_vs_add when adding the base UUID.*/
+    bool           is_defered_read;          /**< Indicate if deferred read operations are supported.*/
+    bool           is_defered_write;         /**< Indicate if deferred write operations are supported.*/
+    bool           is_var_len;               /**< Indicates if the descriptor value has variable length.*/
+    security_req_t read_access;              /**< Security requirement for reading the descriptor value.*/
+    security_req_t write_access;             /**< Security requirement for writing the descriptor value.*/
+    bool           is_value_user;            /**< Indicate if the content of the characteristic is to be stored in the application (user) or in the stack.*/
+    uint16_t       init_len;                 /**< Initial descriptor value length in bytes. */
+    uint16_t       init_offs;                /**< Initial descriptor value offset in bytes. If different from zero, the first init_offs bytes of the attribute value will be left uninitialized. */
+    uint16_t       max_len;                  /**< Maximum descriptor value length in bytes, see @ref BLE_GATTS_ATTR_LENS_MAX for maximum values. */
+    uint8_t*       p_value;                  /**< Pointer to the value of the descriptor*/
+} ble_add_descr_params_t;
+
+
+/**@brief Function for adding a characteristic to a given service.
+ *
+ * If no pointer is given for the initial value,
+ * the initial length parameter will be ignored and the initial length will be 0.
+ *
+ * @param[in]  service_handle Handle of the service to which the characteristic is to be added.
+ * @param[in]  p_char_props   Information needed to add the characteristic.
+ * @param[out] p_char_handle  Handle of the added characteristic.
+ *
+ * @retval      NRF_SUCCESS If the characteristic was added successfully. Otherwise, an error code is returned.
+ */
+uint32_t characteristic_add(uint16_t                   service_handle,
+                            ble_add_char_params_t *    p_char_props,
+                            ble_gatts_char_handles_t * p_char_handle);
+
+
+/**@brief Function for adding a characteristic's descriptor to a given characteristic.
+ *
+ * @param[in]  char_handle    Handle of the characteristic to which the descriptor is to be added, if @ref BLE_GATT_HANDLE_INVALID is used, it will be placed sequentially.
+ * @param[in]  p_descr_props  Information needed to add the descriptor.
+ * @param[out] p_descr_handle Handle of the added descriptor.
+ *
+ * @retval      NRF_SUCCESS If the characteristic was added successfully. Otherwise, an error code is returned.
+ */
+uint32_t descriptor_add(uint16_t                   char_handle,
+                        ble_add_descr_params_t *   p_descr_props,
+                        uint16_t *                 p_descr_handle);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLE_SRV_COMMON_H__
+
+/** @} */

components/ble/nrf_ble_gatt/nrf_ble_gatt.c

@@ -0,0 +1,555 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(NRF_BLE_GATT)
+
+#include "nrf_ble_gatt.h"
+
+#define NRF_LOG_MODULE_NAME nrf_ble_gatt
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+#include "nrf_strerror.h"
+
+#define BLE_GAP_DATA_LENGTH_DEFAULT     27          //!< The stack's default data length.
+#define BLE_GAP_DATA_LENGTH_MAX         251         //!< Maximum data length.
+
+
+STATIC_ASSERT(NRF_SDH_BLE_GAP_DATA_LENGTH < 252);
+
+
+/**@brief Initialize a link's parameters to defaults. */
+static void link_init(nrf_ble_gatt_link_t * p_link)
+{
+    p_link->att_mtu_desired            = NRF_SDH_BLE_GATT_MAX_MTU_SIZE;
+    p_link->att_mtu_effective          = BLE_GATT_ATT_MTU_DEFAULT;
+    p_link->att_mtu_exchange_pending   = false;
+    p_link->att_mtu_exchange_requested = false;
+#if !defined (S112) && !defined(S312) && !defined (S122)
+    p_link->data_length_desired        = NRF_SDH_BLE_GAP_DATA_LENGTH;
+    p_link->data_length_effective      = BLE_GAP_DATA_LENGTH_DEFAULT;
+#endif // !defined (S112) && !defined(S312) && !defined (S122)
+}
+
+/**@brief   Start a data length update request procedure on a given connection. */
+#if !defined (S112) && !defined(S312) && !defined(S122)
+static ret_code_t data_length_update(uint16_t conn_handle, uint16_t data_length)
+{
+    NRF_LOG_DEBUG("Updating data length to %u on connection 0x%x.",
+                  data_length, conn_handle);
+
+    ble_gap_data_length_params_t const dlp =
+    {
+        .max_rx_octets  = data_length,
+        .max_tx_octets  = data_length,
+        .max_rx_time_us = BLE_GAP_DATA_LENGTH_AUTO,
+        .max_tx_time_us = BLE_GAP_DATA_LENGTH_AUTO,
+    };
+
+    ble_gap_data_length_limitation_t dll = {0};
+
+    ret_code_t err_code = sd_ble_gap_data_length_update(conn_handle, &dlp, &dll);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("sd_ble_gap_data_length_update() (request) on connection 0x%x returned %s.",
+                      conn_handle, nrf_strerror_get(err_code));
+
+        if (   (dll.tx_payload_limited_octets != 0)
+            || (dll.rx_payload_limited_octets != 0))
+        {
+            NRF_LOG_ERROR("The requested TX/RX packet length is too long by %u/%u octets.",
+                          dll.tx_payload_limited_octets, dll.rx_payload_limited_octets);
+        }
+
+        if (dll.tx_rx_time_limited_us != 0)
+        {
+            NRF_LOG_ERROR("The requested combination of TX and RX packet lengths "
+                          "is too long by %u microseconds.",
+                          dll.tx_rx_time_limited_us);
+        }
+    }
+
+    return err_code;
+}
+#endif // !defined (S112) && !defined(S312) && !defined (S122)
+
+
+/**@brief Handle a connected event.
+ *
+ * Begins an ATT MTU exchange procedure, followed by a data length update request as necessary.
+ *
+ * @param[in]   p_gatt      GATT structure.
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+static void on_connected_evt(nrf_ble_gatt_t * p_gatt, ble_evt_t const * p_ble_evt)
+{
+    uint16_t              conn_handle = p_ble_evt->evt.common_evt.conn_handle;
+    nrf_ble_gatt_link_t * p_link      = &p_gatt->links[conn_handle];
+
+    // Update the link desired settings to reflect the current global settings.
+#if !defined (S112) && !defined(S312) && !defined(S122)
+    p_link->data_length_desired = p_gatt->data_length;
+#endif // !defined (S112) && !defined(S312) && !defined (S122)
+
+    switch (p_ble_evt->evt.gap_evt.params.connected.role)
+    {
+#if !defined (S122)
+        case BLE_GAP_ROLE_PERIPH:
+            p_link->att_mtu_desired = p_gatt->att_mtu_desired_periph;
+            break;
+#endif // !defined (S122)
+
+#if !defined (S112) && !defined(S312) && !defined(S113)
+        case BLE_GAP_ROLE_CENTRAL:
+            p_link->att_mtu_desired = p_gatt->att_mtu_desired_central;
+            break;
+#endif // !defined (S112) && !defined(S312) && !defined(S113)
+
+        default:
+            // Ignore.
+            break;
+    }
+
+#if NRF_BLE_GATT_MTU_EXCHANGE_INITIATION_ENABLED
+    // Begin an ATT MTU exchange if necessary.
+    if (p_link->att_mtu_desired > p_link->att_mtu_effective)
+    {
+        ret_code_t err_code;
+
+        NRF_LOG_DEBUG("Requesting to update ATT MTU to %u bytes on connection 0x%x.",
+                      p_link->att_mtu_desired, conn_handle);
+
+        err_code = sd_ble_gattc_exchange_mtu_request(conn_handle, p_link->att_mtu_desired);
+
+        if (err_code == NRF_SUCCESS)
+        {
+            p_link->att_mtu_exchange_requested = true;
+        }
+        else if (err_code == NRF_ERROR_BUSY)
+        {
+            p_link->att_mtu_exchange_pending = true;
+            NRF_LOG_DEBUG("sd_ble_gattc_exchange_mtu_request()"
+                          " on connection 0x%x returned busy, will retry.", conn_handle);
+        }
+        else
+        {
+            NRF_LOG_ERROR("sd_ble_gattc_exchange_mtu_request() returned %s.",
+                          nrf_strerror_get(err_code));
+        }
+    }
+#endif // NRF_BLE_GATT_MTU_EXCHANGE_INITIATION_ENABLED
+
+#if !defined (S112) && !defined(S312) && !defined (S122)
+    // Send a data length update request if necessary.
+    if (p_link->data_length_desired > p_link->data_length_effective)
+    {
+        (void) data_length_update(conn_handle, p_link->data_length_desired);
+    }
+#endif // !defined (S112) && !defined(S312) && !defined (S122)
+}
+
+
+static void on_disconnected_evt(nrf_ble_gatt_t * p_gatt, ble_evt_t const * p_ble_evt)
+{
+    // Reset connection parameters.
+    link_init(&p_gatt->links[p_ble_evt->evt.gap_evt.conn_handle]);
+}
+
+
+/**@brief   Handle a BLE_GATTC_EVT_EXCHANGE_MTU_RSP event.
+ *
+ * @details The effective ATT MTU is set to the lowest between what we requested and the peer's
+ *          response. This events concludes the ATT MTU exchange. An event is sent to the user
+ *          and a data length update procedure is started if necessary.
+ *
+ * @param[in]   p_gatt      GATT structure.
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+static void on_exchange_mtu_rsp_evt(nrf_ble_gatt_t * p_gatt, ble_evt_t const * p_ble_evt)
+{
+    uint16_t conn_handle   = p_ble_evt->evt.gattc_evt.conn_handle;
+    uint16_t server_rx_mtu = p_ble_evt->evt.gattc_evt.params.exchange_mtu_rsp.server_rx_mtu;
+
+    nrf_ble_gatt_link_t * p_link = &p_gatt->links[conn_handle];
+
+    // Determine the lowest MTU between our own desired MTU and the peer's.
+    // The MTU may not be less than BLE_GATT_ATT_MTU_DEFAULT.
+    p_link->att_mtu_effective = MIN(server_rx_mtu, p_link->att_mtu_desired);
+    p_link->att_mtu_effective = MAX(p_link->att_mtu_effective, BLE_GATT_ATT_MTU_DEFAULT);
+
+    NRF_LOG_DEBUG("ATT MTU updated to %u bytes on connection 0x%x (response).",
+                  p_link->att_mtu_effective, conn_handle);
+
+    // Trigger an event indicating that the ATT MTU size has changed.
+    // Send an event to the application only if an ATT MTU exchange was requested.
+    if ((p_gatt->evt_handler != NULL) && (p_link->att_mtu_exchange_requested))
+    {
+        nrf_ble_gatt_evt_t const evt =
+        {
+            .evt_id                   = NRF_BLE_GATT_EVT_ATT_MTU_UPDATED,
+            .conn_handle              = conn_handle,
+            .params.att_mtu_effective = p_link->att_mtu_effective,
+        };
+
+        p_gatt->evt_handler(p_gatt, &evt);
+    }
+
+    p_link->att_mtu_exchange_requested = false;
+    p_link->att_mtu_exchange_pending   = false;
+}
+
+
+/**@brief Handle a BLE_GATTS_EVT_EXCHANGE_MTU_REQUEST event.
+ *
+ * @param[in]   p_gatt      GATT structure.
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+static void on_exchange_mtu_request_evt(nrf_ble_gatt_t * p_gatt, ble_evt_t const * p_ble_evt)
+{
+    ret_code_t err_code;
+    uint16_t   conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
+    uint16_t   client_mtu  = p_ble_evt->evt.gatts_evt.params.exchange_mtu_request.client_rx_mtu;
+
+    nrf_ble_gatt_link_t * p_link = &p_gatt->links[conn_handle];
+
+    NRF_LOG_DEBUG("Peer on connection 0x%x requested an ATT MTU of %u bytes.",
+                  conn_handle, client_mtu);
+
+    client_mtu = MAX(client_mtu, BLE_GATT_ATT_MTU_DEFAULT);
+    p_link->att_mtu_effective = MIN(client_mtu, p_link->att_mtu_desired);
+    p_link->att_mtu_exchange_pending = false;
+
+    NRF_LOG_DEBUG("Updating ATT MTU to %u bytes (desired: %u) on connection 0x%x.",
+                  p_link->att_mtu_effective, p_link->att_mtu_desired, conn_handle);
+
+    err_code = sd_ble_gatts_exchange_mtu_reply(conn_handle, p_link->att_mtu_desired);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("sd_ble_gatts_exchange_mtu_reply() returned %s.", nrf_strerror_get(err_code));
+    }
+
+    // If an ATT_MTU exchange was requested to the peer, defer sending
+    // the data length update request and the event to the application until
+    // the response for that request is received.
+    if (p_link->att_mtu_exchange_requested)
+    {
+        return;
+    }
+
+    // The ATT MTU exchange has finished. Send an event to the application.
+    if (p_gatt->evt_handler != NULL)
+    {
+        nrf_ble_gatt_evt_t const evt =
+        {
+            .evt_id                   = NRF_BLE_GATT_EVT_ATT_MTU_UPDATED,
+            .conn_handle              = conn_handle,
+            .params.att_mtu_effective = p_link->att_mtu_effective,
+        };
+
+        p_gatt->evt_handler(p_gatt, &evt);
+    }
+}
+
+
+/**@brief   Handle a BLE_GAP_EVT_DATA_LENGTH_UPDATE event.
+ *
+ * @details Update the connection data length and send an event to the user.
+ *
+ * @param[in]   p_gatt      GATT structure.
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+#if !defined (S112) && !defined(S312) && !defined (S122)
+static void on_data_length_update_evt(nrf_ble_gatt_t * p_gatt, ble_evt_t const * p_ble_evt)
+{
+    ble_gap_evt_t const gap_evt     = p_ble_evt->evt.gap_evt;
+    uint16_t      const conn_handle = gap_evt.conn_handle;
+
+    // Update the connection data length.
+    // The SoftDevice only supports symmetric RX/TX data length settings.
+    p_gatt->links[conn_handle].data_length_effective =
+        gap_evt.params.data_length_update.effective_params.max_tx_octets;
+
+    NRF_LOG_DEBUG("Data length updated to %u on connection 0x%0x.",
+                  p_gatt->links[conn_handle].data_length_effective,
+                  conn_handle);
+
+    NRF_LOG_DEBUG("max_rx_octets: %u",
+                  gap_evt.params.data_length_update.effective_params.max_rx_octets);
+    NRF_LOG_DEBUG("max_tx_octets: %u",
+                  gap_evt.params.data_length_update.effective_params.max_tx_octets);
+    NRF_LOG_DEBUG("max_rx_time: %u",
+                  gap_evt.params.data_length_update.effective_params.max_rx_time_us);
+    NRF_LOG_DEBUG("max_tx_time: %u",
+                  gap_evt.params.data_length_update.effective_params.max_tx_time_us);
+
+    if (p_gatt->evt_handler != NULL)
+    {
+        nrf_ble_gatt_evt_t const evt =
+        {
+            .evt_id             = NRF_BLE_GATT_EVT_DATA_LENGTH_UPDATED,
+            .conn_handle        = conn_handle,
+            .params.data_length = p_gatt->links[conn_handle].data_length_effective,
+        };
+
+        p_gatt->evt_handler(p_gatt, &evt);
+    }
+}
+
+
+/**@brief   Handle a BLE_GAP_EVT_DATA_LENGTH_UPDATE_REQUEST event.
+ *
+ *@details  Reply with a sd_ble_gap_data_length_update() call, using the minimum between the
+ *          link's preferred data length, and what requested by the peer.
+ *          The link preferred data length is set to the global preferred data length
+ *          upon connection and can be overridden by calling nrf_ble_gatt_data_length_set().
+ *          The default is NRF_SDH_BLE_GAP_DATA_LENGTH.
+ *
+ *@note     The SoftDevice will not send any BLE_GAP_EVT_DATA_LENGTH_UPDATE events on this side.
+ *          Therefore, the connection data length is updated immediately and an event is sent
+ *          to the user.
+ *
+ * @param[in]   p_gatt      GATT structure.
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+static void on_data_length_update_request_evt(nrf_ble_gatt_t * p_gatt, ble_evt_t const * p_ble_evt)
+{
+    ble_gap_evt_t       const * p_gap_evt = &p_ble_evt->evt.gap_evt;
+    nrf_ble_gatt_link_t       * p_link    = &p_gatt->links[p_gap_evt->conn_handle];
+
+    // The SoftDevice only supports symmetric RX/TX data length settings.
+    uint8_t const data_length_requested =
+        p_gap_evt->params.data_length_update_request.peer_params.max_tx_octets;
+
+    NRF_LOG_DEBUG("Peer on connection 0x%x requested a data length of %u bytes.",
+                  p_gap_evt->conn_handle, data_length_requested);
+
+    uint8_t const data_length_effective = MIN(p_link->data_length_desired, data_length_requested);
+
+    (void) data_length_update(p_gap_evt->conn_handle, data_length_effective);
+}
+#endif // !defined (S112) && !defined(S312) && !defined (S122)
+
+
+ret_code_t nrf_ble_gatt_init(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_handler_t evt_handler)
+{
+    VERIFY_PARAM_NOT_NULL(p_gatt);
+
+    p_gatt->evt_handler             = evt_handler;
+    p_gatt->att_mtu_desired_periph  = NRF_SDH_BLE_GATT_MAX_MTU_SIZE;
+    p_gatt->att_mtu_desired_central = NRF_SDH_BLE_GATT_MAX_MTU_SIZE;
+    p_gatt->data_length             = NRF_SDH_BLE_GAP_DATA_LENGTH;
+
+    for (uint32_t i = 0; i < NRF_BLE_GATT_LINK_COUNT; i++)
+    {
+        link_init(&p_gatt->links[i]);
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t nrf_ble_gatt_att_mtu_periph_set(nrf_ble_gatt_t * p_gatt, uint16_t desired_mtu)
+{
+    VERIFY_PARAM_NOT_NULL(p_gatt);
+
+    if ((desired_mtu < BLE_GATT_ATT_MTU_DEFAULT) || (desired_mtu > NRF_SDH_BLE_GATT_MAX_MTU_SIZE))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    p_gatt->att_mtu_desired_periph = desired_mtu;
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t nrf_ble_gatt_att_mtu_central_set(nrf_ble_gatt_t * p_gatt, uint16_t desired_mtu)
+{
+    VERIFY_PARAM_NOT_NULL(p_gatt);
+
+    if ((desired_mtu < BLE_GATT_ATT_MTU_DEFAULT) || (desired_mtu > NRF_SDH_BLE_GATT_MAX_MTU_SIZE))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    p_gatt->att_mtu_desired_central = desired_mtu;
+    return NRF_SUCCESS;
+}
+
+
+uint16_t nrf_ble_gatt_eff_mtu_get(nrf_ble_gatt_t const * p_gatt, uint16_t conn_handle)
+{
+    if ((p_gatt == NULL) || (conn_handle >= NRF_BLE_GATT_LINK_COUNT))
+    {
+        return 0;
+    }
+
+    return p_gatt->links[conn_handle].att_mtu_effective;
+}
+
+#if !defined (S112) && !defined(S312) && !defined (S122)
+ret_code_t nrf_ble_gatt_data_length_set(nrf_ble_gatt_t * p_gatt,
+                                        uint16_t         conn_handle,
+                                        uint8_t          data_length)
+{
+    if (p_gatt == NULL)
+    {
+        return NRF_ERROR_NULL;
+    }
+
+    // Check early to avoid requesting an invalid data length for upcoming connections.
+    if (   (data_length > BLE_GAP_DATA_LENGTH_MAX)
+        || (data_length < BLE_GAP_DATA_LENGTH_DEFAULT))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    if (conn_handle == BLE_CONN_HANDLE_INVALID)
+    {
+        // Save value and request upon connection.
+        p_gatt->data_length = data_length;
+        return NRF_SUCCESS;
+    }
+
+    if (conn_handle >= NRF_BLE_GATT_LINK_COUNT)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    // Request data length on existing link.
+    p_gatt->links[conn_handle].data_length_desired = data_length;
+
+    return data_length_update(conn_handle, data_length);
+}
+
+
+ret_code_t nrf_ble_gatt_data_length_get(nrf_ble_gatt_t const * p_gatt,
+                                        uint16_t               conn_handle,
+                                        uint8_t              * p_data_length)
+{
+    if ((p_gatt == NULL) || (p_data_length == NULL))
+    {
+        return NRF_ERROR_NULL;
+    }
+
+    if (conn_handle == BLE_CONN_HANDLE_INVALID)
+    {
+        *p_data_length = p_gatt->data_length;
+        return NRF_SUCCESS;
+    }
+
+    if (conn_handle >= NRF_BLE_GATT_LINK_COUNT)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    *p_data_length = p_gatt->links[conn_handle].data_length_effective;
+    return NRF_SUCCESS;
+}
+#endif // !defined (S112) && !defined(S312) && !defined(S122)
+
+
+void nrf_ble_gatt_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    nrf_ble_gatt_t * p_gatt      = (nrf_ble_gatt_t *)p_context;
+    uint16_t         conn_handle = p_ble_evt->evt.common_evt.conn_handle;
+
+    if (conn_handle >= NRF_BLE_GATT_LINK_COUNT)
+    {
+        return;
+    }
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            on_connected_evt(p_gatt, p_ble_evt);
+            break;
+
+        case BLE_GAP_EVT_DISCONNECTED:
+            on_disconnected_evt(p_gatt, p_ble_evt);
+            break;
+
+        case BLE_GATTC_EVT_EXCHANGE_MTU_RSP:
+            on_exchange_mtu_rsp_evt(p_gatt, p_ble_evt);
+            break;
+
+        case BLE_GATTS_EVT_EXCHANGE_MTU_REQUEST:
+            on_exchange_mtu_request_evt(p_gatt, p_ble_evt);
+            break;
+
+#if !defined (S112) && !defined(S312) && !defined (S122)
+        case BLE_GAP_EVT_DATA_LENGTH_UPDATE:
+            on_data_length_update_evt(p_gatt, p_ble_evt);
+            break;
+
+        case BLE_GAP_EVT_DATA_LENGTH_UPDATE_REQUEST:
+            on_data_length_update_request_evt(p_gatt, p_ble_evt);
+            break;
+#endif // !defined (S112) && !defined(S312) && !defined (S122)
+
+        default:
+            break;
+    }
+
+    if (p_gatt->links[conn_handle].att_mtu_exchange_pending)
+    {
+        ret_code_t err_code;
+
+        err_code = sd_ble_gattc_exchange_mtu_request(conn_handle,
+                                                     p_gatt->links[conn_handle].att_mtu_desired);
+
+        if (err_code == NRF_SUCCESS)
+        {
+            p_gatt->links[conn_handle].att_mtu_exchange_pending   = false;
+            p_gatt->links[conn_handle].att_mtu_exchange_requested = true;
+
+            NRF_LOG_DEBUG("Requesting to update ATT MTU to %u bytes on connection 0x%x (retry).",
+                          p_gatt->links[conn_handle].att_mtu_desired, conn_handle);
+        }
+        else if (err_code != NRF_ERROR_BUSY)
+        {
+            NRF_LOG_ERROR("sd_ble_gattc_exchange_mtu_request() returned %s.",
+                          nrf_strerror_get(err_code));
+        }
+    }
+}
+
+#endif //NRF_BLE_GATT_ENABLED

components/ble/nrf_ble_gatt/nrf_ble_gatt.h

@@ -0,0 +1,242 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_ble_gatt GATT module
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Module for negotiating and keeping track of GATT connection parameters and updating the data length.
+ */
+
+#ifndef NRF_BLE_GATT_H__
+#define NRF_BLE_GATT_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "ble.h"
+#include "ble_gatt.h"
+#include "sdk_config.h"
+#include "sdk_errors.h"
+#include "app_util.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief   Macro for defining a nrf_ble_gatt instance.
+ *
+ * @param   _name   Name of the instance.
+ * @hideinitializer
+ */
+#define NRF_BLE_GATT_DEF(_name)                                                                     \
+static nrf_ble_gatt_t _name;                                                                        \
+NRF_SDH_BLE_OBSERVER(_name ## _obs,                                                                 \
+                     NRF_BLE_GATT_BLE_OBSERVER_PRIO,                                                \
+                     nrf_ble_gatt_on_ble_evt, &_name)
+
+/**@brief   The maximum number of peripheral and central connections combined.
+ *          This value is based on what is configured in the SoftDevice handler sdk_config.
+ */
+#define NRF_BLE_GATT_LINK_COUNT (NRF_SDH_BLE_PERIPHERAL_LINK_COUNT + NRF_SDH_BLE_CENTRAL_LINK_COUNT)
+
+
+/**@brief   GATT module event types. */
+typedef enum
+{
+  NRF_BLE_GATT_EVT_ATT_MTU_UPDATED     = 0xA77,  //!< The ATT_MTU size was updated.
+  NRF_BLE_GATT_EVT_DATA_LENGTH_UPDATED = 0xDA7A, //!< The data length was updated.
+} nrf_ble_gatt_evt_id_t;
+
+/**@brief   GATT module event. */
+typedef struct
+{
+    nrf_ble_gatt_evt_id_t evt_id;       //!< Event ID.
+    uint16_t              conn_handle;  //!< Connection handle on which the event happened.
+    union
+    {
+        uint16_t att_mtu_effective;     //!< Effective ATT_MTU.
+#if !defined (S112) && !defined(S312)
+        uint8_t  data_length;           //!< Data length value.
+#endif // !defined (S112) && !defined(S312)
+    } params;
+} nrf_ble_gatt_evt_t;
+
+// Forward declaration of the nrf_ble_gatt_t type.
+typedef struct nrf_ble_gatt_s nrf_ble_gatt_t;
+
+/**@brief   GATT module event handler type.
+ *
+ * The GATT module calls a function of this type when a parameter value is changed.
+ */
+typedef void (*nrf_ble_gatt_evt_handler_t) (nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_t const * p_evt);
+
+/**@brief   GATT information for each connection. */
+typedef struct
+{
+    uint16_t att_mtu_desired;               //!< Requested ATT_MTU size (in bytes).
+    uint16_t att_mtu_effective;             //!< Effective ATT_MTU size (in bytes).
+    bool     att_mtu_exchange_pending;      //!< Indicates that an ATT_MTU exchange request is pending (the call to @ref sd_ble_gattc_exchange_mtu_request returned @ref NRF_ERROR_BUSY).
+    bool     att_mtu_exchange_requested;    //!< Indicates that an ATT_MTU exchange request was made.
+#if !defined (S112) && !defined(S312)
+    uint8_t  data_length_desired;           //!< Desired data length (in bytes).
+    uint8_t  data_length_effective;         //!< Requested data length (in bytes).
+#endif // !defined (S112) && !defined(S312)
+} nrf_ble_gatt_link_t;
+
+
+/**@brief   GATT structure that contains status information for the GATT module. */
+struct nrf_ble_gatt_s
+{
+    uint16_t                   att_mtu_desired_periph;          //!< Requested ATT_MTU size for the next peripheral connection that is established.
+    uint16_t                   att_mtu_desired_central;         //!< Requested ATT_MTU size for the next central connection that is established.
+    uint8_t                    data_length;                     //!< Data length to use for the next connection that is established.
+    nrf_ble_gatt_link_t        links[NRF_BLE_GATT_LINK_COUNT];  //!< GATT related information for all active connections.
+    nrf_ble_gatt_evt_handler_t evt_handler;                     //!< GATT event handler.
+};
+
+
+/**@brief   Function for initializing the GATT module.
+ *
+ * @param[in]   evt_handler Event handler.
+ * @param[out]  p_gatt      Pointer to the GATT structure.
+ *
+ * @retval NRF_SUCCESS      If the operation was successful.
+ * @retval NRF_ERROR_NULL   If @p p_gatt is NULL.
+ */
+ret_code_t nrf_ble_gatt_init(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_handler_t evt_handler);
+
+
+/**@brief   Function for setting the ATT_MTU size for the next connection that is established as peripheral.
+ *
+ * @param[in]   p_gatt      Pointer to the GATT structure.
+ * @param[in]   desired_mtu Requested ATT_MTU size.
+ *
+ * @retval NRF_SUCCESS              If the operation was successful.
+ * @retval NRF_ERROR_NULL           If @p p_gatt is NULL.
+ * @retval NRF_ERROR_INVALID_PARAM  If the size of @p desired_mtu is bigger than
+ *                                  @ref NRF_SDH_BLE_GATT_MAX_MTU_SIZE or smaller than
+ *                                  @ref BLE_GATT_ATT_MTU_DEFAULT.
+ */
+ret_code_t nrf_ble_gatt_att_mtu_periph_set(nrf_ble_gatt_t * p_gatt, uint16_t desired_mtu);
+
+
+/**@brief   Function for setting the ATT_MTU size for the next connection that is established as central.
+ *
+ * @param[in,out]   p_gatt      Pointer to the GATT structure.
+ * @param[in]       desired_mtu Requested ATT_MTU size.
+ *
+ * @retval NRF_SUCCESS              If the operation was successful.
+ * @retval NRF_ERROR_NULL           If @p p_gatt is NULL.
+ * @retval NRF_ERROR_INVALID_PARAM  If the size of @p desired_mtu is bigger than
+ *                                  @ref NRF_SDH_BLE_GATT_MAX_MTU_SIZE or smaller
+ *                                  than @ref BLE_GATT_ATT_MTU_DEFAULT.
+ */
+ret_code_t nrf_ble_gatt_att_mtu_central_set(nrf_ble_gatt_t * p_gatt, uint16_t desired_mtu);
+
+
+/**@brief   Function for setting the data length for a connection.
+ *
+ * @details If @p conn_handle is a handle to an existing connection, a data length update
+ *          request is sent on that connection.
+ *          If @p conn_handle is @ref BLE_CONN_HANDLE_INVALID, a data length update request
+ *          is sent on the next connection that is established after the ATT_MTU
+ *          exchange has completed. If no ATT_MTU exchange procedure is carried
+ *          out (for example, if a default ATT_MTU size is used), the data length
+ *          is not changed.
+ */
+#if !defined (S112) && !defined(S312)
+ret_code_t nrf_ble_gatt_data_length_set(nrf_ble_gatt_t * p_gatt,
+                                        uint16_t         conn_handle,
+                                        uint8_t          data_length);
+#endif // !defined (S112) && !defined(S312)
+
+/**@brief   Function for retrieving the data length of a connection.
+ *
+ * @details If @p conn_handle is @ref BLE_CONN_HANDLE_INVALID, the function retrieves the data
+ *          length that will be requested for the next connection.
+ *          If @p conn_handle is a handle to an existing connection, the function retrieves
+ *          the effective data length that was negotiated for that connection.
+ *
+ * @param[in,out]   p_gatt          Pointer to the GATT structure.
+ * @param[in]       conn_handle     The connection for which to retrieve the data length, or
+ *                                  @ref BLE_CONN_HANDLE_INVALID to retrieve the requested data length
+ *                                  for the next connection.
+ * @param[out]      p_data_length   The connection data length.
+ *
+ * @retval NRF_SUCCESS              If the operation was successful.
+ * @retval NRF_ERROR_NULL           If @p p_gatt or @p p_data_length is NULL.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p conn_handle is larger than @ref NRF_BLE_GATT_LINK_COUNT.
+ */
+#if !defined (S112) && !defined(S312)
+ret_code_t nrf_ble_gatt_data_length_get(nrf_ble_gatt_t const * p_gatt,
+                                        uint16_t               conn_handle,
+                                        uint8_t              * p_data_length);
+#endif // !defined (S112) && !defined(S312)
+
+/**@brief   Function for handling BLE stack events.
+ *
+ * @details This function handles events from the BLE stack that are of interest to the module.
+ *
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ * @param[in]   p_context   Pointer to the GATT structure.
+ */
+void nrf_ble_gatt_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context);
+
+
+/**@brief   Function for getting the current ATT_MTU size for a given connection.
+ *
+ * @param[in]   p_gatt      Pointer to the GATT structure.
+ * @param[in]   conn_handle Connection handle of the connection.
+ *
+ * @return  ATT_MTU size for the given connection.
+ * @retval  0       If @p p_gatt is NULL or if @p conn_handle is larger than
+ *                  the supported maximum number of connections.
+ */
+uint16_t nrf_ble_gatt_eff_mtu_get(nrf_ble_gatt_t const * p_gatt, uint16_t conn_handle);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_BLE_GATT_H__
+
+/** @} */

components/ble/nrf_ble_gq/nrf_ble_gq.c

@@ -0,0 +1,611 @@
+/**
+ * Copyright (c) 2018 - 2021, 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.
+ *
+ */
+
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(NRF_BLE_GQ)
+
+#include "nrf_ble_gq.h"
+
+#define NRF_LOG_MODULE_NAME nrf_ble_gq
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+/**@brief Pointer used to describe memory allocator for GATT request. */
+typedef ret_code_t (* req_data_alloc_t) (nrf_memobj_pool_t const * p_data_pool, 
+                                         nrf_ble_gq_req_t  * const p_req);
+
+
+/**@brief Function allocates memory for data associated with @ref NRF_BLE_GQ_REQ_GATTC_WRITE
+ *        request.
+ *
+ * @param[in] p_data_pool  Pointer to general memory pool.
+ * @param[in] p_req        Pointer to GATTC write request.
+ *
+ * @retval    NRF_SUCCESS              If the write data was allocated successfully.
+ * @retval    NRF_ERROR_INVALID_LENGTH If data to be written is too long.
+ * @retval    NRF_ERROR_NO_MEM         If there was no room either in the data pool for new allocation.
+ */
+static ret_code_t gattc_write_alloc(nrf_memobj_pool_t const * p_data_pool,
+                                    nrf_ble_gq_req_t  * const p_req)
+{
+    nrf_ble_gq_gattc_write_t * p_gattc_write = &p_req->params.gattc_write;
+
+    // Check if the payload data is not too long.
+    if (p_gattc_write->len > NRF_BLE_GQ_GATTC_WRITE_MAX_DATA_LEN)
+    {
+        return NRF_ERROR_INVALID_LENGTH;
+    }
+
+    // Allocate memory for GATTC write request.
+    p_req->p_mem_obj = nrf_memobj_alloc(p_data_pool,
+                                        p_gattc_write->len);
+    if (p_req->p_mem_obj == NULL)
+    {
+        return NRF_ERROR_NO_MEM;
+    }
+
+    // Copy relevant data to the pool.
+    nrf_memobj_write(p_req->p_mem_obj, (void *) p_gattc_write->p_value, p_gattc_write->len, 0);
+
+    NRF_LOG_DEBUG("Pointer to allocated memory block: %p.", p_req->p_mem_obj);
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function allocates memory for data associated with @ref NRF_BLE_GQ_REQ_GATTS_HVX
+ *        request.
+ *
+ * @param[in] p_data_pool  Pointer to general memory pool.
+ * @param[in] p_req        Pointer to GATTS hvx request.
+ *
+ * @retval    NRF_SUCCESS              If the notification or indication data was allocated successfully.
+ * @retval    NRF_ERROR_INVALID_LENGTH If data to be written is too long.
+ * @retval    NRF_ERROR_NO_MEM         If there was no room either in the data pool for new allocation.
+ */
+static ret_code_t gatts_hvx_alloc(nrf_memobj_pool_t const * p_data_pool,
+                                  nrf_ble_gq_req_t  * const p_req)
+{
+    nrf_ble_gq_gatts_hvx_t * p_gatts_hvx = &p_req->params.gatts_hvx;
+
+    // Check if the payload data is not too long.
+    if (*p_gatts_hvx->p_len > NRF_BLE_GQ_GATTS_HVX_MAX_DATA_LEN)
+    {
+        return NRF_ERROR_INVALID_LENGTH;
+    }
+
+    // Allocate memory for GATTS notification or indication request.
+    p_req->p_mem_obj = nrf_memobj_alloc(p_data_pool,
+                                        *p_gatts_hvx->p_len + sizeof(uint16_t));
+    if (p_req->p_mem_obj == NULL)
+    {
+        return NRF_ERROR_NO_MEM;
+    }
+
+    // Copy relevant data to the pool.
+    nrf_memobj_write(p_req->p_mem_obj, (void *)p_gatts_hvx->p_len, sizeof(uint16_t), 0);
+    nrf_memobj_write(p_req->p_mem_obj, 
+                     (void *)p_gatts_hvx->p_data, 
+                     *p_gatts_hvx->p_len, 
+                     sizeof(uint16_t));
+
+    NRF_LOG_DEBUG("Pointer to allocated memory block: %p.", p_req->p_mem_obj);
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Array of memory allocators for different types of @ref nrf_ble_gq_req_t. */
+static const req_data_alloc_t m_req_data_alloc[NRF_BLE_GQ_REQ_NUM] =
+{
+    [NRF_BLE_GQ_REQ_GATTC_READ]     = NULL,
+    [NRF_BLE_GQ_REQ_GATTC_WRITE]    = gattc_write_alloc,
+    [NRF_BLE_GQ_REQ_SRV_DISCOVERY]  = NULL,
+    [NRF_BLE_GQ_REQ_CHAR_DISCOVERY] = NULL,
+    [NRF_BLE_GQ_REQ_DESC_DISCOVERY] = NULL,
+    [NRF_BLE_GQ_REQ_GATTS_HVX]      = gatts_hvx_alloc
+};
+
+
+/**@brief Function handles error codes returned by GATT requests.
+ *
+ * @param[in] p_req       Pointer to GATT request.
+ * @param[in] err_code    Error code returned by SoftDevice.
+ * @param[in] conn_handle Connection handle.
+ */
+__STATIC_INLINE void request_err_code_handle(nrf_ble_gq_req_t const * const p_req,
+                                             uint16_t                       conn_handle,
+                                             ret_code_t                     err_code)
+{
+    if (err_code == NRF_SUCCESS)
+    {
+        NRF_LOG_DEBUG("SD GATT procedure (%d) succeeded on connection handle: %d.",
+                       p_req->type,
+                       conn_handle);
+    }
+    else
+    {
+        NRF_LOG_ERROR("SD GATT procedure (%d) failed on connection handle %d with error: 0x%08X.",
+                      p_req->type, conn_handle, err_code);
+        if (p_req->error_handler.cb != NULL)
+        {
+             p_req->error_handler.cb(err_code, p_req->error_handler.p_ctx, conn_handle);
+        }
+    }
+}
+
+
+/**@brief Function processes subsequent requests from the BGQ instance queue.
+ *
+ * @param[in] p_queue      Pointer to the queue instance.
+ * @param[in] conn_handle  Connection handle.
+ */
+static void queue_process(nrf_queue_t const * const p_queue, uint16_t conn_handle)
+{
+    ret_code_t       err_code;
+    nrf_ble_gq_req_t ble_req;
+
+    NRF_LOG_DEBUG("Processing the request queue...");
+
+    err_code = nrf_queue_peek(p_queue, &ble_req);
+    if (err_code == NRF_SUCCESS) // Queue is not empty
+    {
+        switch (ble_req.type)
+        {
+            case NRF_BLE_GQ_REQ_GATTC_READ:
+                NRF_LOG_DEBUG("GATTC Read Request");
+                err_code = sd_ble_gattc_read(conn_handle,
+                                             ble_req.params.gattc_read.handle,
+                                             ble_req.params.gattc_read.offset);
+                break;
+
+            case NRF_BLE_GQ_REQ_GATTC_WRITE:
+            {
+                uint8_t write_data[NRF_BLE_GQ_GATTC_WRITE_MAX_DATA_LEN];
+
+                // Retrieve allocated data.
+                ble_req.params.gattc_write.p_value = write_data;
+                nrf_memobj_read(ble_req.p_mem_obj,
+                                (void *) ble_req.params.gattc_write.p_value,
+                                ble_req.params.gattc_write.len, 0);
+
+                NRF_LOG_DEBUG("GATTC Write Request");
+                err_code = sd_ble_gattc_write(conn_handle,
+                                              &ble_req.params.gattc_write);
+            } break;
+
+            case NRF_BLE_GQ_REQ_SRV_DISCOVERY:
+            {
+                NRF_LOG_DEBUG("GATTC Primary Service Discovery Request");
+                err_code = sd_ble_gattc_primary_services_discover(conn_handle,
+                                                                  ble_req.params.gattc_srv_disc.start_handle,
+                                                                  &ble_req.params.gattc_srv_disc.srvc_uuid);
+            } break;
+
+            case NRF_BLE_GQ_REQ_CHAR_DISCOVERY:
+            {
+                NRF_LOG_DEBUG("GATTC Characteristic Discovery Request");
+                err_code = sd_ble_gattc_characteristics_discover(conn_handle,
+                                                                 &ble_req.params.gattc_char_disc);
+            } break;
+
+            case NRF_BLE_GQ_REQ_DESC_DISCOVERY:
+            {
+                NRF_LOG_DEBUG("GATTC Characteristic Descriptor Discovery Request")
+                err_code = sd_ble_gattc_descriptors_discover(conn_handle,
+                                                             &ble_req.params.gattc_desc_disc);
+            } break;
+
+            case NRF_BLE_GQ_REQ_GATTS_HVX:
+            {
+                uint8_t  hvx_data[NRF_BLE_GQ_GATTS_HVX_MAX_DATA_LEN];
+                uint16_t len;
+                uint16_t hvx_len;
+
+                // Retrieve allocated data.
+                ble_req.params.gatts_hvx.p_data = hvx_data;
+                nrf_memobj_read(ble_req.p_mem_obj,
+                                (void *) &hvx_len,
+                                sizeof(uint16_t),
+                                0);
+                ble_req.params.gatts_hvx.p_len = &hvx_len;
+                nrf_memobj_read(ble_req.p_mem_obj,
+                                (void *) ble_req.params.gatts_hvx.p_data,
+                                *ble_req.params.gatts_hvx.p_len,
+                                sizeof(uint16_t));
+
+                len = hvx_len;
+
+                NRF_LOG_DEBUG("GATTS HVX");
+                err_code = sd_ble_gatts_hvx(conn_handle,
+                                            &ble_req.params.gatts_hvx);
+
+                if ((err_code == NRF_SUCCESS) &&
+                    (len != hvx_len))
+                {
+                    err_code = NRF_ERROR_DATA_SIZE;
+                }
+            } break;
+
+            default:
+                NRF_LOG_WARNING("Unimplemented GATT Request");
+                break;
+        }
+
+        if (err_code == NRF_ERROR_BUSY) // Softdevice is processing another GATT request.
+        {
+            NRF_LOG_DEBUG("SD is currently busy. The GATT request procedure will be attempted \
+                          again later.");
+        }
+        else
+        {
+            // Remove last request descriptor from the queue and free data associated with it.
+            if (m_req_data_alloc[ble_req.type] != NULL)
+            {
+                nrf_memobj_free(ble_req.p_mem_obj);
+                NRF_LOG_DEBUG("Pointer to freed memory block: %p.", ble_req.p_mem_obj);
+            }
+            UNUSED_RETURN_VALUE(nrf_queue_pop(p_queue, &ble_req));
+
+            request_err_code_handle(&ble_req, conn_handle, err_code);
+        }
+    }
+}
+
+
+/**@brief Function purges all requests from BGQ instance queues that are
+ *        no longer used by any connection.
+ *
+ * @param[in] p_gatt_queue Pointer to the BGQ instance.
+ */
+static void queues_purge(nrf_ble_gq_t const * const p_gatt_queue)
+{
+    ret_code_t err_code;
+    uint16_t   conn_id;
+
+    err_code = nrf_queue_pop(p_gatt_queue->p_purge_queue, &conn_id);
+
+    while (err_code == NRF_SUCCESS)
+    {
+        nrf_ble_gq_req_t    ble_req;
+        nrf_queue_t const * p_queue;
+
+        NRF_LOG_DEBUG("Purging request queue with id: %d", conn_id);
+
+        p_queue  = &p_gatt_queue->p_req_queue[conn_id];
+        err_code = nrf_queue_pop(p_queue, &ble_req);
+
+        while (err_code == NRF_SUCCESS)
+        {
+            // Free data associated with this request if there is any.
+            if (m_req_data_alloc[ble_req.type] != NULL)
+            {
+                nrf_memobj_free(ble_req.p_mem_obj);
+                NRF_LOG_DEBUG("Pointer to freed memory block: %p.", ble_req.p_mem_obj);
+            }
+
+            err_code = nrf_queue_pop(p_queue, &ble_req);
+        }
+
+        err_code = nrf_queue_pop(p_gatt_queue->p_purge_queue, &conn_id);
+    }
+}
+
+
+/**@brief Function processes single GATT request without queue.
+ *
+ * @param[in] p_req        Pointer to GATT request.
+ * @param[in] conn_handle  Connection handle.
+ *
+ * @retval  true   If request is accepted by Softdevice.
+ * @retval  false  If Softdevice is busy and the request should be queued.
+ */
+static bool request_process(nrf_ble_gq_req_t const * const p_req, uint16_t conn_handle)
+{
+    ret_code_t err_code = NRF_SUCCESS;
+
+    switch (p_req->type)
+    {
+        case NRF_BLE_GQ_REQ_GATTC_READ:
+            NRF_LOG_DEBUG("GATTC Read Request");
+            err_code = sd_ble_gattc_read(conn_handle,
+                                         p_req->params.gattc_read.handle,
+                                         p_req->params.gattc_read.offset);
+            break;
+
+        case NRF_BLE_GQ_REQ_GATTC_WRITE:
+            NRF_LOG_DEBUG("GATTC Write Request");
+            err_code = sd_ble_gattc_write(conn_handle,
+                                          &p_req->params.gattc_write);
+            break;
+
+        case NRF_BLE_GQ_REQ_SRV_DISCOVERY:
+            NRF_LOG_DEBUG("GATTC Primary Services Discovery Request");
+            err_code = sd_ble_gattc_primary_services_discover(conn_handle,
+                                                              p_req->params.gattc_srv_disc.start_handle,
+                                                              &p_req->params.gattc_srv_disc.srvc_uuid);
+            break;
+
+        case NRF_BLE_GQ_REQ_CHAR_DISCOVERY:
+            NRF_LOG_DEBUG("GATTC Characteristic Discovery Request");
+            err_code = sd_ble_gattc_characteristics_discover(conn_handle,
+                                                             &p_req->params.gattc_char_disc);
+            break;
+
+        case NRF_BLE_GQ_REQ_DESC_DISCOVERY:
+            NRF_LOG_DEBUG("GATTC Characteristic Descriptor Request");
+            err_code = sd_ble_gattc_descriptors_discover(conn_handle,
+                                                         &p_req->params.gattc_desc_disc);
+            break;
+
+        case NRF_BLE_GQ_REQ_GATTS_HVX:
+        {
+            uint16_t len = *p_req->params.gatts_hvx.p_len;
+
+            NRF_LOG_DEBUG("GATTS Notification or Indication");
+
+            err_code = sd_ble_gatts_hvx(conn_handle,
+                                        &p_req->params.gatts_hvx);
+
+            if ((err_code == NRF_SUCCESS) &&
+                (len != *p_req->params.gatts_hvx.p_len))
+            {
+                err_code = NRF_ERROR_DATA_SIZE;
+            }
+
+        } break;
+
+        default:
+            NRF_LOG_WARNING("Unimplemented GATT Request");
+            break;
+    }
+
+    if (err_code == NRF_ERROR_BUSY) // Softdevice is processing another GATT request.
+    {
+        NRF_LOG_DEBUG("SD is currently busy. The GATT request procedure will be attempted \
+                      again later.");
+        return false;
+    }
+    else
+    {
+        request_err_code_handle(p_req, conn_handle, err_code);
+        return true;
+    }
+}
+
+
+/**@brief Function finds ID for the provided connection handle within nrf_ble_gq_t instance registry.
+ *
+ * @param[in] p_gatt_queue  Pointer to the nrf_ble_gq_t instance.
+ * @param[in] conn_handle   Connection handle.
+ *
+ * @return    Connection ID.
+ */
+static uint16_t conn_handle_id_find(nrf_ble_gq_t const * const p_gatt_queue, uint16_t conn_handle)
+{
+    uint16_t id;
+
+    for (id = 0; id < p_gatt_queue->max_conns; id++)
+    {
+        if (conn_handle == p_gatt_queue->p_conn_handles[id])
+        {
+            return id;
+        }
+    }
+    return id;
+}
+
+
+/**@brief Function registers provided connection handle within nrf_ble_gq_t instance registry.
+ *
+ * @param[in] p_gatt_queue  Pointer to the nrf_ble_gq_t instance.
+ * @param[in] conn_handle   Connection handle.
+ *
+ * @retval    NRF_SUCCESS       If the registration was successful.
+ * @retval    NRF_ERROR_NO_MEM  If there was no space for another connection handle.
+ */
+static ret_code_t conn_handle_register(nrf_ble_gq_t const * const p_gatt_queue, uint16_t conn_handle)
+{
+    for (uint16_t id = 0; id < p_gatt_queue->max_conns; id++)
+    {
+        if (p_gatt_queue->p_conn_handles[id] == BLE_CONN_HANDLE_INVALID)
+        {
+            p_gatt_queue->p_conn_handles[id] = conn_handle;
+            return NRF_SUCCESS;
+        }
+    }
+    return NRF_ERROR_NO_MEM;
+}
+
+
+/**@brief Function checks if any connection handle is registered in nrf_ble_gq_t instance.
+ *
+ * @param[in] p_gatt_queue Pointer to the nrf_ble_gq_t instance.
+ *
+ * @retval    true   There is at least one registered connection handle.
+ * @retval    false  Connection handle registry is empty.
+ */
+static bool is_any_conn_handle_registered(nrf_ble_gq_t const * const p_gatt_queue)
+{
+    for (uint16_t id = 0; id < p_gatt_queue->max_conns; id++)
+    {
+        if (p_gatt_queue->p_conn_handles[id] != BLE_CONN_HANDLE_INVALID)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+ret_code_t nrf_ble_gq_item_add(nrf_ble_gq_t const * const p_gatt_queue,
+                               nrf_ble_gq_req_t   * const p_req,
+                               uint16_t                   conn_handle)
+{
+    ret_code_t err_code = NRF_SUCCESS;
+    uint16_t   conn_id;
+
+    NRF_LOG_DEBUG("Adding item to the request queue");
+
+    VERIFY_PARAM_NOT_NULL(p_gatt_queue);
+    VERIFY_PARAM_NOT_NULL(p_req);
+
+    // Purge queues that are no longer used by any connection.
+    queues_purge(p_gatt_queue);
+
+    // Check if connection handle is registered and if GATT request is valid.
+    conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
+    if ((p_req->type >= NRF_BLE_GQ_REQ_NUM) || (conn_id == p_gatt_queue->max_conns))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    // Try processing a request without buffering.
+    if (nrf_queue_is_empty(&p_gatt_queue->p_req_queue[conn_id]))
+    {
+        bool req_processed = request_process(p_req, conn_handle);
+        if (req_processed)
+        {
+            return err_code;
+        }
+    }
+
+    // Prepare request for buffering and add it to the queue.
+    if (m_req_data_alloc[p_req->type] != NULL)
+    {
+        VERIFY_PARAM_NOT_NULL(p_gatt_queue->p_data_pool);
+
+        err_code = m_req_data_alloc[p_req->type](p_gatt_queue->p_data_pool, p_req);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    err_code = nrf_queue_push(&p_gatt_queue->p_req_queue[conn_id], p_req);
+    if ((err_code != NRF_SUCCESS) && (m_req_data_alloc[p_req->type] != NULL))
+    {
+        nrf_memobj_free(p_req->p_mem_obj);
+        NRF_LOG_DEBUG("Pointer to freed memory block: %p.", p_req->p_mem_obj);
+    }
+
+    // Check if Softdevice is still busy.
+    queue_process(&p_gatt_queue->p_req_queue[conn_id], conn_handle);
+    return err_code;
+}
+
+
+ret_code_t nrf_ble_gq_conn_handle_register(nrf_ble_gq_t * const p_gatt_queue, uint16_t conn_handle)
+{
+    ret_code_t err_code = NRF_SUCCESS;
+    uint16_t   conn_id;
+
+    VERIFY_PARAM_NOT_NULL(p_gatt_queue);
+
+    // Purge queues that are no longer used by any connection.
+    queues_purge(p_gatt_queue);
+
+    // Allow instance to claim connection handle only if it has not been claimed already.
+    conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
+    if (conn_id == p_gatt_queue->max_conns)
+    {
+        NRF_LOG_DEBUG("Registering connection handle: 0x%04X", conn_handle);
+
+        // Initialize/reset data pool if possible.
+        if (!is_any_conn_handle_registered(p_gatt_queue))
+        {
+            err_code = nrf_memobj_pool_init(p_gatt_queue->p_data_pool);
+        }
+
+        err_code = conn_handle_register(p_gatt_queue, conn_handle);
+        VERIFY_SUCCESS(err_code);
+    }
+    return err_code;
+}
+
+
+void nrf_ble_gq_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    nrf_ble_gq_t * p_gatt_queue = (nrf_ble_gq_t *) p_context;
+    uint16_t       conn_handle;
+    uint16_t       conn_id;
+
+    if ((p_ble_evt == NULL) || (p_gatt_queue == NULL))
+    {
+        return;
+    }
+
+    // Obtain connection handle and filter out the events that do not trigger queue processing.
+    if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
+    {
+        conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+    }
+    else if ((p_ble_evt->header.evt_id >= BLE_GATTC_EVT_BASE) &&
+             (p_ble_evt->header.evt_id <= BLE_GATTC_EVT_LAST))
+    {
+        conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;
+    }
+    else if ((p_ble_evt->header.evt_id >= BLE_GATTS_EVT_BASE) &&
+             (p_ble_evt->header.evt_id <= BLE_GATTS_EVT_LAST))
+    {
+        conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
+    }
+    else
+    {
+        // These events are irrelevant for this module.
+        return;
+    }
+
+    // Check if connection handle is registered.
+    conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
+    if (conn_id == p_gatt_queue->max_conns)
+    {
+        return;
+    }
+
+    // Perform operations on the queue.
+    if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
+    {
+        p_gatt_queue->p_conn_handles[conn_id] = BLE_CONN_HANDLE_INVALID;
+        UNUSED_RETURN_VALUE(nrf_queue_push(p_gatt_queue->p_purge_queue, &conn_id));
+    }
+    else
+    {
+        queue_process(&p_gatt_queue->p_req_queue[conn_id], conn_handle);
+    }
+}
+
+
+#endif // NRF_MODULE_ENABLED(NRF_BLE_GQ)

components/ble/nrf_ble_gq/nrf_ble_gq.h

@@ -0,0 +1,260 @@
+/**
+ * Copyright (c) 2018 - 2021, 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 nrf_ble_gq BLE GATT Queue
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief  Queue for the BLE GATT requests.
+ *
+ * @details The BLE GATT Queue (BGQ) module can be used to queue BLE GATT requests if the SoftDevice is not
+ *          able to handle them at the moment. In this case, processing of queued request is
+ *          postponed. Later on, when corresponding BLE event indicates that the SoftDevice may be
+ *          free, the request is retried. For conceptual documentation of this module, see
+ *          @ref lib_ble_gatt_queue.
+ *
+ */
+#ifndef NRF_BLE_GQ_H__
+#define NRF_BLE_GQ_H__
+
+#include <stdint.h>
+#include "sdk_common.h"
+#include "nrf_memobj.h"
+#include "nrf_queue.h"
+#include "nrf_sdh_ble.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief   Macro for defining a nrf_ble_gq_t instance with default parameters.
+ *
+ * @param   _name            Name of the instance.
+ * @param   _max_connections The maximal number of connection handles that can be registered.
+ * @param   _queue_size      The maximal number of nrf_ble_gq_req_t instances that queue can hold.
+ * @hideinitializer
+ */
+#define NRF_BLE_GQ_DEF(_name, _max_connections, _queue_size)        \
+    NRF_BLE_GQ_CUSTOM_DEF(_name,                                    \
+                          _max_connections,                         \
+                          _queue_size,                              \
+                          NRF_BLE_GQ_DATAPOOL_ELEMENT_SIZE,         \
+                          NRF_BLE_GQ_DATAPOOL_ELEMENT_COUNT)
+
+
+#if !(defined(__LINT__))
+/**@brief   Macro for defining a nrf_ble_gq_t instance.
+ *
+ * @param   _name            Name of the instance.
+ * @param   _max_connections The maximal number of connection handles that can be registered.
+ * @param   _queue_size      The maximal number of nrf_ble_gq_req_t instances that queue can hold.
+ * @param   _pool_elem_size  Size of a single element in the pool of memory objects.
+ * @param   _pool_elem_count Number of elements in the pool of memory objects.
+ * @hideinitializer
+ */
+#define NRF_BLE_GQ_CUSTOM_DEF(_name, _max_connections, _queue_size, _pool_elem_size, _pool_elem_count) \
+    static uint16_t CONCAT_2(_name, conn_handles_arr)[] =                                              \
+    {                                                                                                  \
+        MACRO_REPEAT(_max_connections, NRF_BLE_GQ_CONN_HANDLE_INIT)                                    \
+    };                                                                                                 \
+    STATIC_ASSERT(ARRAY_SIZE(CONCAT_2(_name, conn_handles_arr)) == (_max_connections));                \
+    NRF_QUEUE_ARRAY_DEF(nrf_ble_gq_req_t, CONCAT_2(_name, req_queue), _queue_size,                     \
+                        NRF_QUEUE_MODE_NO_OVERFLOW, _max_connections);                                 \
+    NRF_QUEUE_DEF(uint16_t, CONCAT_2(_name, purge_queue), _max_connections,                            \
+                  NRF_QUEUE_MODE_NO_OVERFLOW);                                                         \
+    NRF_MEMOBJ_POOL_DEF(CONCAT_2(_name, pool), _pool_elem_size, _pool_elem_count);                     \
+    static nrf_ble_gq_t _name =                                                                        \
+    {                                                                                                  \
+        .max_conns      = (_max_connections),                                                          \
+        .p_conn_handles = CONCAT_2(_name, conn_handles_arr),                                           \
+        .p_req_queue    = CONCAT_2(_name, req_queue),                                                  \
+        .p_purge_queue  = &CONCAT_2(_name, purge_queue),                                               \
+        .p_data_pool    = &CONCAT_2(_name, pool)                                                       \
+    };                                                                                                 \
+    NRF_SDH_BLE_OBSERVER(_name ## _obs,                                                                \
+                         NRF_BLE_GQ_BLE_OBSERVER_PRIO,                                                 \
+                         nrf_ble_gq_on_ble_evt, &_name)
+#else
+#define NRF_BLE_GQ_CUSTOM_DEF(_name, _max_connections, _queue_size, _pool_elem_size, _pool_elem_count) \
+    static nrf_ble_gq_t _name;
+#endif // !(defined(__LINT__))
+
+/**@brief Helping macro used to properly initialize connection handle array for nrf_ble_gq_t instance.
+ *        Used in @ref NRF_BLE_GQ_CUSTOM_DEF.
+ */
+#define NRF_BLE_GQ_CONN_HANDLE_INIT(_arg) BLE_CONN_HANDLE_INVALID,
+
+/**@brief BLE GATT request types. */
+typedef enum
+{
+    NRF_BLE_GQ_REQ_GATTC_READ,     /**< GATTC Read Request. See @ref nrf_ble_gq_gattc_read_t and @ref sd_ble_gattc_read */
+    NRF_BLE_GQ_REQ_GATTC_WRITE,    /**< GATTC Write Request. See @ref nrf_ble_gq_gattc_write_t and @ref sd_ble_gattc_write */
+    NRF_BLE_GQ_REQ_SRV_DISCOVERY,  /**< GATTC Service Discovery Request. See @ref nrf_ble_gq_gattc_write_t and @ref sd_ble_gattc_primary_services_discover. */
+    NRF_BLE_GQ_REQ_CHAR_DISCOVERY, /**< GATTC Characteristic Discovery Request. See @ref nrf_ble_gq_gattc_char_disc_t and @ref sd_ble_gattc_characteristics_discover. */
+    NRF_BLE_GQ_REQ_DESC_DISCOVERY, /**< GATTC Characteristic Descriptor Discovery Request. See @ref nrf_ble_gq_gattc_desc_disc_t and @ref sd_ble_gattc_descriptors_discover*/
+    NRF_BLE_GQ_REQ_GATTS_HVX,      /**< GATTS Handle Value Notification or Indication. See @ref nrf_ble_gq_gatts_hvx_t and @ref ble_gatts_hvx_params_t */
+    NRF_BLE_GQ_REQ_NUM             /**< Total number of different GATT Request types */
+} nrf_ble_gq_req_type_t;
+
+/**@brief Pointer used to describe error handler for GATTC request. */
+typedef void (* nrf_ble_gq_req_error_cb_t) (uint32_t   nrf_error,
+                                            void     * p_context,
+                                            uint16_t   conn_handle);
+
+/**@brief Structure used to describe @ref NRF_BLE_GQ_REQ_GATTC_READ request type. */
+typedef struct
+{
+    uint16_t handle; /**< Handle of the Attribute to be read. */
+    uint16_t offset; /**< Offset into the Attribute Value to be read. */
+} nrf_ble_gq_gattc_read_t;
+
+/**@brief Structure used to describe @ref NRF_BLE_GQ_REQ_GATTC_WRITE request type. */
+typedef ble_gattc_write_params_t nrf_ble_gq_gattc_write_t;
+
+/**@brief Structure used to describe @ref NRF_BLE_GQ_REQ_SRV_DISCOVERY request type. */
+typedef struct
+{
+    uint16_t   start_handle;    /**< The start handle value used during service discovery. */
+    ble_uuid_t srvc_uuid;       /**< The service UUID to be found. */
+} nrf_ble_gq_gattc_srv_discovery_t;
+
+/**@brief Structure used to describe @ref NRF_BLE_GQ_REQ_CHAR_DISCOVERY request type. */
+typedef ble_gattc_handle_range_t nrf_ble_gq_gattc_char_disc_t;
+
+/**@brief Structure used to describe @ref NRF_BLE_GQ_REQ_DESC_DISCOVERY request type. */
+typedef ble_gattc_handle_range_t nrf_ble_gq_gattc_desc_disc_t;
+
+/**@brief Structure used to describe @ref NRF_BLE_GQ_REQ_GATTS_HVX request type. */
+typedef ble_gatts_hvx_params_t nrf_ble_gq_gatts_hvx_t;
+
+/**@brief Structure used to handle SoftDevice error. */
+typedef struct
+{
+    nrf_ble_gq_req_error_cb_t   cb;    /**< Error handler to be called in case of an error from SoftDevice. */
+    void                      * p_ctx; /**< Parameter to the error handler. */
+} nrf_ble_gq_req_error_handler_t;
+
+
+/**@brief Structure used to describe BLE GATT request. */
+typedef struct
+{
+    nrf_ble_gq_req_type_t            type;          /**< Type of request. */
+    nrf_memobj_t                   * p_mem_obj;     /**< Memory object for data that cannot be contained in request descriptor. */
+    nrf_ble_gq_req_error_handler_t   error_handler; /**< Error handler structure. */
+    union
+    {
+        nrf_ble_gq_gattc_read_t          gattc_read;      /**< GATTC read parameters. Filled when nrf_ble_gq_req_t::type is @ref NRF_BLE_GQ_REQ_GATTC_READ. */
+        nrf_ble_gq_gattc_write_t         gattc_write;     /**< GATTC write parameters. Filled when nrf_ble_gq_req_t::type is @ref NRF_BLE_GQ_REQ_GATTC_WRITE. */
+        nrf_ble_gq_gattc_srv_discovery_t gattc_srv_disc;  /**< GATTC Service discovery parameters. Filled when nrf_ble_gq_req_t::type is @ref NRF_BLE_GQ_REQ_SRV_DISCOVERY. */
+        nrf_ble_gq_gattc_char_disc_t     gattc_char_disc; /**< GATTC characteristic discovery parameters. Filled when nrf_ble_gq_req_t::type is @ref NRF_BLE_GQ_REQ_CHAR_DISCOVERY. */
+        nrf_ble_gq_gattc_desc_disc_t     gattc_desc_disc; /**< GATTC characteristic descriptor discovery parameters. Filled when nrf_ble_gq_req_t::type is NRF_BLE_GQ_REQ_DESC_DISCOVERY. */
+        nrf_ble_gq_gatts_hvx_t           gatts_hvx;       /**< GATTS Handle Value Notification or Indication Parameters. Filled when nrf_ble_gq_req_t::type is @ref NRF_BLE_GQ_REQ_GATTS_HVX. */
+    } params;
+} nrf_ble_gq_req_t;
+
+/**@brief Descriptor for the BLE GATT Queue instance. */
+typedef struct
+{
+    uint16_t            const max_conns;      /**< Maximal number of connection handles that can be registered. */
+    uint16_t                * p_conn_handles; /**< Pointer to array with registered connection handles.*/
+    nrf_queue_t const * const p_req_queue;    /**< Pointer to array of queue instances used to hold nrf_ble_gq_req_t instances.*/
+    nrf_queue_t const * const p_purge_queue;  /**< Pointer to the queue instance used to hold indexes of queues to purge.*/
+    nrf_memobj_pool_t const * p_data_pool;    /**< Memory pool used to obtain nrf_memobj_t instances.*/
+} nrf_ble_gq_t;
+
+
+/**@brief Function for adding a GATT request to the BGQ instance.
+ *
+ * @details This function adds a request to the BGQ instance and allocates necessary memory
+ *          for data that can be held within the request descriptor. If the SoftDevice is free,
+ *          this request will be processed immediately. Otherwise, the request remains in
+ *          in the queue and is processed later.
+ *
+ * @param[in] p_gatt_queue  Pointer to the BGQ instance.
+ * @param[in] p_req         Pointer to the request.
+ * @param[in] conn_handle   Connection handle associated with the request.
+ *
+ * @retval    NRF_SUCCESS             If the request was added successfully.
+ * @retval    NRF_ERROR_NULL          Any parameter was NULL.
+ * @retval    NRF_ERROR_NO_MEM        There was no room in the queue or in the data pool.
+ * @retval    NRF_ERROR_INVALID_PARAM If \p conn_handle is not registered or type of request -
+ *                                    \p p_req is not valid.
+ * @retval    err_code				  Other request specific error codes may be returned.
+ */
+ret_code_t nrf_ble_gq_item_add(nrf_ble_gq_t const * const p_gatt_queue,
+                               nrf_ble_gq_req_t   * const p_req,
+                               uint16_t                   conn_handle);
+
+
+/**@brief Function for registering connection handle in the BGQ instance.
+ *
+ * @details This function is used for registering connection handle in the BGQ instance. From this
+ *          point, the BGQ instance can handle GATT requests associated with the handle until connection
+ *          is no longer valid (disconnect event occurs).
+ *
+ * @param[in] p_gatt_queue  Pointer to the BGQ instance.
+ * @param[in] conn_handle   Connection handle.
+ *
+ * @retval    NRF_SUCCESS      If the registration was successful.
+ * @retval    NRF_ERROR_NULL   If \p p_gatt_queue was NULL.
+ * @retval    NRF_ERROR_NO_MEM If there was no space for another connection handle.
+ */
+ret_code_t nrf_ble_gq_conn_handle_register(nrf_ble_gq_t * const p_gatt_queue, uint16_t conn_handle);
+
+
+/**@brief     Function for handling BLE events from the SoftDevice.
+ *
+ * @details   This function handles the BLE events received from the SoftDevice. If a BLE
+ *            event is relevant to the BGQ module, it is used to update internal variables,
+ *            process queued GATT requests and, if necessary, send errors to the application.
+ *
+ * @param[in] p_ble_evt   Pointer to the BLE event.
+ * @param[in] p_context   Pointer to the BGQ instance.
+ */
+void nrf_ble_gq_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_BLE_GQ_H__
+
+/** @} */

components/ble/nrf_ble_qwr/nrf_ble_qwr.c

@@ -0,0 +1,488 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(NRF_BLE_QWR)
+#include <stdlib.h>
+#include "nrf_ble_qwr.h"
+#include "ble.h"
+#include "ble_srv_common.h"
+
+
+#define NRF_BLE_QWR_INITIALIZED 0xDE // Non-zero value used to make sure the given structure has been initialized by the module.
+#define MODULE_INITIALIZED      (p_qwr->initialized == NRF_BLE_QWR_INITIALIZED)
+#include "sdk_macros.h"
+
+ret_code_t nrf_ble_qwr_init(nrf_ble_qwr_t            * p_qwr,
+                            nrf_ble_qwr_init_t const * p_qwr_init)
+{
+    VERIFY_PARAM_NOT_NULL(p_qwr);
+    VERIFY_PARAM_NOT_NULL(p_qwr_init);
+    if (MODULE_INITIALIZED)
+    {
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    p_qwr->error_handler = p_qwr_init->error_handler;
+    p_qwr->initialized   = NRF_BLE_QWR_INITIALIZED;
+    p_qwr->conn_handle   = BLE_CONN_HANDLE_INVALID;
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+    memset(p_qwr->attr_handles, 0, sizeof(p_qwr->attr_handles));
+    p_qwr->nb_registered_attr        = 0;
+    p_qwr->is_user_mem_reply_pending = false;
+    p_qwr->mem_buffer                = p_qwr_init->mem_buffer;
+    p_qwr->callback                  = p_qwr_init->callback;
+    p_qwr->nb_written_handles        = 0;
+#endif
+    return NRF_SUCCESS;
+}
+
+
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+ret_code_t nrf_ble_qwr_attr_register(nrf_ble_qwr_t * p_qwr, uint16_t attr_handle)
+{
+    VERIFY_PARAM_NOT_NULL(p_qwr);
+    VERIFY_MODULE_INITIALIZED();
+
+    if ((p_qwr->nb_registered_attr == NRF_BLE_QWR_MAX_ATTR)
+        || (p_qwr->mem_buffer.p_mem == NULL)
+        || (p_qwr->mem_buffer.len == 0))
+    {
+        return (NRF_ERROR_NO_MEM);
+    }
+
+    if (attr_handle == BLE_GATT_HANDLE_INVALID)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    p_qwr->attr_handles[p_qwr->nb_registered_attr] = attr_handle;
+    p_qwr->nb_registered_attr++;
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t nrf_ble_qwr_value_get(nrf_ble_qwr_t * p_qwr,
+                                 uint16_t        attr_handle,
+                                 uint8_t       * p_mem,
+                                 uint16_t      * p_len)
+{
+    VERIFY_PARAM_NOT_NULL(p_qwr);
+    VERIFY_PARAM_NOT_NULL(p_mem);
+    VERIFY_PARAM_NOT_NULL(p_len);
+    VERIFY_MODULE_INITIALIZED();
+
+    uint16_t i          = 0;
+    uint16_t handle     = BLE_GATT_HANDLE_INVALID;
+    uint16_t val_len    = 0;
+    uint16_t val_offset = 0;
+    uint32_t cur_len    = 0;
+
+    do
+    {
+        handle = uint16_decode(&(p_qwr->mem_buffer.p_mem[i]));
+
+        if (handle == BLE_GATT_HANDLE_INVALID)
+        {
+            break;
+        }
+
+        i         += sizeof(uint16_t);
+        val_offset = uint16_decode(&(p_qwr->mem_buffer.p_mem[i]));
+        i         += sizeof(uint16_t);
+        val_len    = uint16_decode(&(p_qwr->mem_buffer.p_mem[i]));
+        i         += sizeof(uint16_t);
+
+        if (handle == attr_handle)
+        {
+            cur_len = val_offset + val_len;
+            if (cur_len <= *p_len)
+            {
+                memcpy((p_mem + val_offset), &(p_qwr->mem_buffer.p_mem[i]), val_len);
+            }
+            else
+            {
+                return NRF_ERROR_NO_MEM;
+            }
+        }
+
+        i += val_len;
+    }
+    while (i < p_qwr->mem_buffer.len);
+
+    *p_len = cur_len;
+    return NRF_SUCCESS;
+}
+#endif
+
+
+ret_code_t nrf_ble_qwr_conn_handle_assign(nrf_ble_qwr_t * p_qwr,
+                                          uint16_t        conn_handle)
+{
+    VERIFY_PARAM_NOT_NULL(p_qwr);
+    VERIFY_MODULE_INITIALIZED();
+    p_qwr->conn_handle = conn_handle;
+    return NRF_SUCCESS;
+}
+
+
+/**@brief checks if a user_mem_reply is pending, if so attempts to send it.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ */
+static void user_mem_reply(nrf_ble_qwr_t * p_qwr)
+{
+    if (p_qwr->is_user_mem_reply_pending)
+    {
+        ret_code_t err_code;
+#if (NRF_BLE_QWR_MAX_ATTR == 0)
+        err_code = sd_ble_user_mem_reply(p_qwr->conn_handle, NULL);
+#else
+        err_code = sd_ble_user_mem_reply(p_qwr->conn_handle, &p_qwr->mem_buffer);
+#endif
+        if (err_code == NRF_SUCCESS)
+        {
+            p_qwr->is_user_mem_reply_pending = false;
+        }
+        else if (err_code == NRF_ERROR_BUSY)
+        {
+            p_qwr->is_user_mem_reply_pending = true;
+        }
+        else
+        {
+            p_qwr->error_handler(err_code);
+        }
+    }
+}
+
+
+/**@brief Handle a user memory request event.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ * @param[in]   p_common_evt User_mem_request event to be handled.
+ */
+static void on_user_mem_request(nrf_ble_qwr_t          * p_qwr,
+                                ble_common_evt_t const * p_common_evt)
+{
+    if ((p_common_evt->params.user_mem_request.type == BLE_USER_MEM_TYPE_GATTS_QUEUED_WRITES) &&
+        (p_common_evt->conn_handle == p_qwr->conn_handle))
+    {
+        p_qwr->is_user_mem_reply_pending = true;
+        user_mem_reply(p_qwr);
+    }
+}
+
+
+/**@brief Handle a user memory release event.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ * @param[in]   p_common_evt User_mem_release event to be handled.
+ */
+static void on_user_mem_release(nrf_ble_qwr_t          * p_qwr,
+                                ble_common_evt_t const * p_common_evt)
+{
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+    if ((p_common_evt->params.user_mem_release.type == BLE_USER_MEM_TYPE_GATTS_QUEUED_WRITES) &&
+        (p_common_evt->conn_handle == p_qwr->conn_handle))
+    {
+        // Cancel the current operation.
+        p_qwr->nb_written_handles = 0;
+    }
+#endif
+}
+
+
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+/**@brief Handle a prepare write event.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ * @param[in]   p_evt_write  WRITE event to be handled.
+ */
+static void on_prepare_write(nrf_ble_qwr_t               * p_qwr,
+                             ble_gatts_evt_write_t const * p_evt_write)
+{
+    uint32_t                              err_code;
+    ble_gatts_rw_authorize_reply_params_t auth_reply;
+    memset(&auth_reply, 0, sizeof(auth_reply));
+
+    auth_reply.params.write.gatt_status = NRF_BLE_QWR_REJ_REQUEST_ERR_CODE;
+    auth_reply.type                     = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
+
+    uint32_t i;
+
+    for (i = 0; i < p_qwr->nb_written_handles; i++)
+    {
+        if (p_qwr->written_attr_handles[i] == p_evt_write->handle)
+        {
+            auth_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
+            break;
+        }
+    }
+
+    if (auth_reply.params.write.gatt_status != BLE_GATT_STATUS_SUCCESS)
+    {
+        for (i = 0; i < p_qwr->nb_registered_attr; i++)
+        {
+            if (p_qwr->attr_handles[i] == p_evt_write->handle)
+            {
+                auth_reply.params.write.gatt_status                      = BLE_GATT_STATUS_SUCCESS;
+                p_qwr->written_attr_handles[p_qwr->nb_written_handles++] = p_evt_write->handle;
+                break;
+            }
+        }
+    }
+
+    err_code = sd_ble_gatts_rw_authorize_reply(p_qwr->conn_handle, &auth_reply);
+    if (err_code != NRF_SUCCESS)
+    {
+        // Cancel the current operation.
+        p_qwr->nb_written_handles = 0;
+
+        // Report error to application.
+        p_qwr->error_handler(err_code);
+    }
+
+}
+
+
+/**@brief Handle an execute write event.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ * @param[in]   p_evt_write  EXEC WRITE event to be handled.
+ */
+static void on_execute_write(nrf_ble_qwr_t               * p_qwr,
+                             ble_gatts_evt_write_t const * p_evt_write)
+{
+    uint32_t                              err_code;
+    ble_gatts_rw_authorize_reply_params_t auth_reply;
+    memset(&auth_reply, 0, sizeof(auth_reply));
+
+    auth_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
+    auth_reply.type                     = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
+
+    if (p_qwr->nb_written_handles == 0)
+    {
+        auth_reply.params.write.gatt_status = NRF_BLE_QWR_REJ_REQUEST_ERR_CODE;
+        err_code = sd_ble_gatts_rw_authorize_reply(p_qwr->conn_handle, &auth_reply);
+        if (err_code != NRF_SUCCESS)
+        {
+            // Report error to application.
+            p_qwr->error_handler(err_code);
+        }
+        return;
+    }
+
+    for (uint16_t i = 0; i < p_qwr->nb_written_handles; i++)
+    {
+        nrf_ble_qwr_evt_t evt;
+        uint16_t          ret_val;
+
+        evt.evt_type    = NRF_BLE_QWR_EVT_AUTH_REQUEST;
+        evt.attr_handle = p_qwr->written_attr_handles[i];
+        ret_val         = p_qwr->callback(p_qwr, &evt);
+        if (ret_val != BLE_GATT_STATUS_SUCCESS)
+        {
+            auth_reply.params.write.gatt_status = ret_val;
+        }
+    }
+
+    err_code = sd_ble_gatts_rw_authorize_reply(p_qwr->conn_handle, &auth_reply);
+    if (err_code != NRF_SUCCESS)
+    {
+        // Report error to application.
+        p_qwr->error_handler(err_code);
+    }
+
+    // If the execute has not been rejected by any of the registered applications, propagate execute write event to all written handles. */
+    if (auth_reply.params.write.gatt_status == BLE_GATT_STATUS_SUCCESS)
+    {
+        for (uint16_t i = 0; i < p_qwr->nb_written_handles; i++)
+        {
+            nrf_ble_qwr_evt_t evt;
+            evt.evt_type    = NRF_BLE_QWR_EVT_EXECUTE_WRITE;
+            evt.attr_handle = p_qwr->written_attr_handles[i];
+            /*lint -e534 -save "Ignoring return value of function" */
+            p_qwr->callback(p_qwr, &evt);
+            /*lint -restore*/
+
+            auth_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
+        }
+    }
+    p_qwr->nb_written_handles = 0;
+}
+
+
+/**@brief Handle a cancel write event.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ * @param[in]   p_evt_write  EXEC WRITE event to be handled.
+ */
+static void on_cancel_write(nrf_ble_qwr_t               * p_qwr,
+                            ble_gatts_evt_write_t const * p_evt_write)
+{
+    uint32_t                              err_code;
+    ble_gatts_rw_authorize_reply_params_t auth_reply;
+    memset(&auth_reply, 0, sizeof(auth_reply));
+
+    auth_reply.type                     = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
+    auth_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
+
+    err_code = sd_ble_gatts_rw_authorize_reply(p_qwr->conn_handle, &auth_reply);
+    if (err_code != NRF_SUCCESS)
+    {
+        // Report error to application.
+        p_qwr->error_handler(err_code);
+    }
+    p_qwr->nb_written_handles = 0;
+}
+#endif
+
+/**@brief Handle a rw_authorize_request event.
+ *
+ * @param[in]   p_qwr        QWR structure.
+ * @param[in]   p_gatts_evt  RW_authorize_request event to be handled.
+ */
+static void on_rw_authorize_request(nrf_ble_qwr_t         * p_qwr,
+                                    ble_gatts_evt_t const * p_gatts_evt)
+{
+    if (p_gatts_evt->conn_handle != p_qwr->conn_handle)
+    {
+        return;
+    }
+
+    ble_gatts_evt_rw_authorize_request_t const * p_auth_req = &p_gatts_evt->params.authorize_request;
+
+    if (p_auth_req->type != BLE_GATTS_AUTHORIZE_TYPE_WRITE)
+    {
+        return;
+    }
+
+#if (NRF_BLE_QWR_MAX_ATTR == 0)
+    // Handle only queued write related operations.
+    if ((p_auth_req->request.write.op != BLE_GATTS_OP_PREP_WRITE_REQ) &&
+        (p_auth_req->request.write.op != BLE_GATTS_OP_EXEC_WRITE_REQ_NOW) &&
+        (p_auth_req->request.write.op != BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL))
+    {
+        return;
+    }
+
+    // Prepare the response.
+    ble_gatts_rw_authorize_reply_params_t auth_reply = {0};
+
+    auth_reply.type                     = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
+    auth_reply.params.write.gatt_status = NRF_BLE_QWR_REJ_REQUEST_ERR_CODE;
+    if (p_auth_req->request.write.op == BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL)
+    {
+        auth_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
+    }
+
+    ret_code_t err_code = sd_ble_gatts_rw_authorize_reply(p_gatts_evt->conn_handle, &auth_reply);
+    if (err_code != NRF_SUCCESS)
+    {
+        // Report error to application.
+        p_qwr->error_handler(err_code);
+    }
+#else
+    switch (p_auth_req->request.write.op)
+    {
+        case BLE_GATTS_OP_PREP_WRITE_REQ:
+            on_prepare_write(p_qwr, &p_auth_req->request.write);
+            break; // BLE_GATTS_OP_PREP_WRITE_REQ
+
+        case BLE_GATTS_OP_EXEC_WRITE_REQ_NOW:
+            on_execute_write(p_qwr, &p_auth_req->request.write);
+            break; // BLE_GATTS_OP_EXEC_WRITE_REQ_NOW
+
+        case BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL:
+            on_cancel_write(p_qwr, &p_auth_req->request.write);
+            break; // BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL
+
+        default:
+            // No implementation needed.
+            break;
+    }
+#endif
+}
+
+
+void nrf_ble_qwr_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
+{
+    VERIFY_PARAM_NOT_NULL_VOID(p_context);
+    VERIFY_PARAM_NOT_NULL_VOID(p_ble_evt);
+
+    nrf_ble_qwr_t * p_qwr = (nrf_ble_qwr_t *)p_context;
+
+    VERIFY_MODULE_INITIALIZED_VOID();
+
+    if (p_ble_evt->evt.common_evt.conn_handle == p_qwr->conn_handle)
+    {
+        user_mem_reply(p_qwr);
+    }
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_EVT_USER_MEM_REQUEST:
+            on_user_mem_request(p_qwr, &p_ble_evt->evt.common_evt);
+            break; // BLE_EVT_USER_MEM_REQUEST
+
+        case BLE_EVT_USER_MEM_RELEASE:
+            on_user_mem_release(p_qwr, &p_ble_evt->evt.common_evt);
+            break; // BLE_EVT_USER_MEM_REQUEST
+
+        case BLE_GATTS_EVT_RW_AUTHORIZE_REQUEST:
+            on_rw_authorize_request(p_qwr, &p_ble_evt->evt.gatts_evt);
+            break; // BLE_GATTS_EVT_RW_AUTHORIZE_REQUEST
+
+        case BLE_GAP_EVT_DISCONNECTED:
+            if (p_ble_evt->evt.gap_evt.conn_handle == p_qwr->conn_handle)
+            {
+                p_qwr->conn_handle = BLE_CONN_HANDLE_INVALID;
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+                p_qwr->nb_written_handles = 0;
+#endif
+            }
+            break; // BLE_GAP_EVT_DISCONNECTED
+
+        default:
+            break;
+    }
+
+}
+#endif // NRF_MODULE_ENABLED(NRF_BLE_QWR)

components/ble/nrf_ble_qwr/nrf_ble_qwr.h

@@ -0,0 +1,246 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_ble_qwr Queued Writes module
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Module for handling Queued Write operations.
+ *
+ * @details This module handles prepare write, execute write, and cancel write
+ * commands. It also manages memory requests related to these operations.
+ *
+ * @note     The application must propagate BLE stack events to this module by calling
+ *           @ref nrf_ble_qwr_on_ble_evt().
+ */
+
+#ifndef NRF_BLE_QUEUED_WRITES_H__
+#define NRF_BLE_QUEUED_WRITES_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include "nordic_common.h"
+#include "sdk_common.h"
+#include "ble.h"
+#include "ble_srv_common.h"
+
+/**@brief   Macro for defining a nrf_ble_qwr instance.
+ *
+ * @param   _name   Name of the instance.
+ * @hideinitializer
+ */
+#define NRF_BLE_QWR_DEF(_name)                          \
+    static nrf_ble_qwr_t _name;                         \
+    NRF_SDH_BLE_OBSERVER(_name ## _obs,                 \
+                         NRF_BLE_QWR_BLE_OBSERVER_PRIO, \
+                         nrf_ble_qwr_on_ble_evt,        \
+                         &_name)
+
+/**@brief   Macro for defining an array of nrf_ble_qwr instance.
+ *
+ * @param   _name   Name of the array.
+ * @param   _cnt    Size of the array.
+ * @hideinitializer
+ */
+#define NRF_BLE_QWRS_DEF(_name, _cnt)                    \
+    static nrf_ble_qwr_t _name[_cnt];                    \
+    NRF_SDH_BLE_OBSERVERS(_name ## _obs,                 \
+                          NRF_BLE_QWR_BLE_OBSERVER_PRIO, \
+                          nrf_ble_qwr_on_ble_evt,        \
+                          &_name,                        \
+                          _cnt)
+
+
+#define NRF_BLE_QWR_REJ_REQUEST_ERR_CODE    BLE_GATT_STATUS_ATTERR_APP_BEGIN + 0 //!< Error code used by the module to reject prepare write requests on non-registered attributes.
+
+
+/**@brief Queued Writes module event types. */
+typedef enum
+{
+    NRF_BLE_QWR_EVT_EXECUTE_WRITE, //!< Event that indicates that an execute write command was received for a registered handle and that the received data was actually written and is now ready.
+    NRF_BLE_QWR_EVT_AUTH_REQUEST,  //!< Event that indicates that an execute write command was received for a registered handle and that the write request must now be accepted or rejected.
+} nrf_ble_qwr_evt_type_t;
+
+/**@brief Queued Writes module events. */
+typedef struct
+{
+    nrf_ble_qwr_evt_type_t evt_type;    //!< Type of the event.
+    uint16_t               attr_handle; //!< Handle of the attribute to which the event relates.
+} nrf_ble_qwr_evt_t;
+
+// Forward declaration of the nrf_ble_qwr_t type.
+struct nrf_ble_qwr_t;
+
+/**@brief Queued Writes module event handler type.
+ *
+ * If the provided event is of type @ref NRF_BLE_QWR_EVT_AUTH_REQUEST,
+ * this function must accept or reject the execute write request by returning
+ * one of the @ref BLE_GATT_STATUS_CODES.*/
+typedef uint16_t (* nrf_ble_qwr_evt_handler_t) (struct nrf_ble_qwr_t * p_qwr,
+                                                nrf_ble_qwr_evt_t    * p_evt);
+
+/**@brief Queued Writes structure.
+ * @details This structure contains status information for the Queued Writes module. */
+typedef struct nrf_ble_qwr_t
+{
+    uint8_t                   initialized;                                //!< Flag that indicates whether the module has been initialized.
+    uint16_t                  conn_handle;                                //!< Connection handle.
+    ble_srv_error_handler_t   error_handler;                              //!< Error handler.
+    bool                      is_user_mem_reply_pending;                  //!< Flag that indicates whether a mem_reply is pending (because a previous attempt returned busy).
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+    uint16_t                  attr_handles[NRF_BLE_QWR_MAX_ATTR];         //!< List of handles for registered attributes, for which the module accepts and handles prepare write operations.
+    uint8_t                   nb_registered_attr;                         //!< Number of registered attributes.
+    uint16_t                  written_attr_handles[NRF_BLE_QWR_MAX_ATTR]; //!< List of attribute handles that have been written to during the current prepare write or execute write operation.
+    uint8_t                   nb_written_handles;                         //!< Number of attributes that have been written to during the current prepare write or execute write operation.
+    ble_user_mem_block_t      mem_buffer;                                 //!< Memory buffer that is provided to the SoftDevice on an ON_USER_MEM_REQUEST event.
+    nrf_ble_qwr_evt_handler_t callback;                                   //!< Event handler function that is called for events concerning the handles of all registered attributes.
+#endif
+} nrf_ble_qwr_t;
+
+/**@brief Queued Writes init structure.
+ * @details This structure contains all information
+ *          that is needed to initialize the Queued Writes module. */
+typedef struct
+{
+    ble_srv_error_handler_t   error_handler; //!< Error handler.
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+    ble_user_mem_block_t      mem_buffer;    //!< Memory buffer that is provided to the SoftDevice on an ON_USER_MEM_REQUEST event.
+    nrf_ble_qwr_evt_handler_t callback;      //!< Event handler function that is called for events concerning the handles of all registered attributes.
+#endif
+} nrf_ble_qwr_init_t;
+
+
+/**@brief Function for initializing the Queued Writes module.
+ *
+ * @details Call this function in the main entry of your application to
+ * initialize the Queued Writes module. It must be called only once with a
+ * given Queued Writes structure.
+ *
+ * @param[out]  p_qwr     Queued Writes structure. This structure must be
+ *                        supplied by the application. It is initialized by this function
+ *                        and is later used to identify the particular Queued Writes instance.
+ * @param[in]  p_qwr_init Initialization structure.
+ *
+ * @retval NRF_SUCCESS             If the Queued Writes module was initialized successfully.
+ * @retval NRF_ERROR_NULL          If any of the given pointers is NULL.
+ * @retval NRF_ERROR_INVALID_STATE If the given context has already been initialized.
+ */
+ret_code_t nrf_ble_qwr_init(nrf_ble_qwr_t            * p_qwr,
+                            nrf_ble_qwr_init_t const * p_qwr_init);
+
+
+/**@brief Function for assigning a connection handle to a given instance of the Queued Writes module.
+ *
+ * @details   Call this function when a link with a peer has been established to
+ *            associate this link to the instance of the module. This makes it
+ *            possible to handle several links and associate each link to a particular
+ *            instance of this module.
+ *
+ * @param[in]  p_qwr       Queued Writes structure.
+ * @param[in]  conn_handle Connection handle to be associated with the given Queued Writes instance.
+ *
+ * @retval NRF_SUCCESS             If the assignment was successful.
+ * @retval NRF_ERROR_NULL          If any of the given pointers is NULL.
+ * @retval NRF_ERROR_INVALID_STATE If the given context has not been initialized.
+ */
+ret_code_t nrf_ble_qwr_conn_handle_assign(nrf_ble_qwr_t * p_qwr,
+                                          uint16_t        conn_handle);
+
+
+/**@brief Function for handling BLE stack events.
+ *
+ * @details Handles all events from the BLE stack that are of interest to the Queued Writes module.
+ *
+ * @param[in]  p_ble_evt    Event received from the BLE stack.
+ * @param[in]  p_context    Queued Writes structure.
+ */
+void nrf_ble_qwr_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context);
+
+
+#if (NRF_BLE_QWR_MAX_ATTR > 0)
+/**@brief Function for registering an attribute with the Queued Writes module.
+ *
+ * @details Call this function for each attribute that you want to enable for
+ * Queued Writes (thus a series of prepare write and execute write operations).
+ *
+ * @param[in]  p_qwr       Queued Writes structure.
+ * @param[in]  attr_handle Handle of the attribute to register.
+ *
+ * @retval NRF_SUCCESS             If the registration was successful.
+ * @retval NRF_ERROR_NO_MEM        If no more memory is available to add this registration.
+ * @retval NRF_ERROR_NULL          If any of the given pointers is NULL.
+ * @retval NRF_ERROR_INVALID_STATE If the given context has not been initialized.
+ */
+ret_code_t nrf_ble_qwr_attr_register(nrf_ble_qwr_t * p_qwr, uint16_t attr_handle);
+
+
+/**@brief Function for retrieving the received data for a given attribute.
+ *
+ * @details Call this function after receiving an @ref NRF_BLE_QWR_EVT_AUTH_REQUEST
+ * event to retrieve a linear copy of the data that was received for the given attribute.
+ *
+ * @param[in]     p_qwr       Queued Writes structure.
+ * @param[in]     attr_handle Handle of the attribute.
+ * @param[out]    p_mem       Pointer to the application buffer where the received data will be copied.
+ * @param[in,out] p_len       Input: length of the input buffer. Output: length of the received data.
+ *
+ *
+ * @retval NRF_SUCCESS             If the data was retrieved and stored successfully.
+ * @retval NRF_ERROR_NO_MEM        If the provided buffer was smaller than the received data.
+ * @retval NRF_ERROR_NULL          If any of the given pointers is NULL.
+ * @retval NRF_ERROR_INVALID_STATE If the given context has not been initialized.
+ */
+ret_code_t nrf_ble_qwr_value_get(nrf_ble_qwr_t * p_qwr,
+                                 uint16_t        attr_handle,
+                                 uint8_t       * p_mem,
+                                 uint16_t      * p_len);
+#endif // (NRF_BLE_QWR_MAX_ATTR > 0)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_BLE_QUEUED_WRITES_H__
+
+/** @} */

components/ble/nrf_ble_scan/nrf_ble_scan.h

@@ -0,0 +1,500 @@
+/**
+ * Copyright (c) 2018 - 2021, 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 nrf_ble_scan Scanning Module
+ * @{
+ * @ingroup ble_sdk_lib
+ * @brief Module for handling the BLE scanning.
+ *
+ * @details The Scanning Module handles the BLE scanning for your application.
+ *          The module offers several criteria for filtering the devices available for connection,
+ *          and it can also work in the simple mode without using the filtering.
+ *          If an event handler is provided, your main application can react to a filter match or to the need of setting the whitelist.
+ *          The module can also be configured to automatically
+ *          connect after it matches a filter or a device from the whitelist.
+ *
+ * @note    The Scanning Module also supports applications with a multicentral link.
+ */
+
+#ifndef NRF_BLE_SCAN_H__
+#define NRF_BLE_SCAN_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "ble.h"
+#include "ble_gap.h"
+#include "app_util.h"
+#include "sdk_errors.h"
+#include "sdk_config.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**@defgroup NRF_BLE_SCAN_FILTER_MODE Filter modes
+ * @{ */
+#define NRF_BLE_SCAN_NAME_FILTER       (0x01) /**< Filters the device name. */
+#define NRF_BLE_SCAN_ADDR_FILTER       (0x02) /**< Filters the device address. */
+#define NRF_BLE_SCAN_UUID_FILTER       (0x04) /**< Filters the UUID. */
+#define NRF_BLE_SCAN_APPEARANCE_FILTER (0x08) /**< Filters the appearance. */
+#define NRF_BLE_SCAN_SHORT_NAME_FILTER (0x10) /**< Filters the device short name. */
+#define NRF_BLE_SCAN_ALL_FILTER        (0x1F) /**< Uses the combination of all filters. */
+/* @} */
+
+/**@brief Macro for defining a nrf_ble_scan instance.
+ *
+ * @param   _name   Name of the instance.
+ * @hideinitializer
+ */
+#define NRF_BLE_SCAN_DEF(_name)                            \
+    static nrf_ble_scan_t _name;                           \
+    NRF_SDH_BLE_OBSERVER(_name ## _ble_obs,                \
+                         NRF_BLE_SCAN_OBSERVER_PRIO,       \
+                         nrf_ble_scan_on_ble_evt, &_name); \
+
+
+/**@brief Enumeration for scanning events.
+ *
+ * @details These events are propagated to the main application if a handler is provided during
+ *          the initialization of the Scanning Module. @ref NRF_BLE_SCAN_EVT_WHITELIST_REQUEST cannot be
+ *          ignored if whitelist is used.
+ */
+typedef enum
+{
+    NRF_BLE_SCAN_EVT_FILTER_MATCH,         /**< A filter is matched or all filters are matched in the multifilter mode. */
+    NRF_BLE_SCAN_EVT_WHITELIST_REQUEST,    /**< Request the whitelist from the main application. For whitelist scanning to work, the whitelist must be set when this event occurs. */
+    NRF_BLE_SCAN_EVT_WHITELIST_ADV_REPORT, /**< Send notification to the main application when a device from the whitelist is found. */
+    NRF_BLE_SCAN_EVT_NOT_FOUND,            /**< The filter was not matched for the scan data. */
+    NRF_BLE_SCAN_EVT_SCAN_TIMEOUT,         /**< Scan timeout. */
+    NRF_BLE_SCAN_EVT_CONNECTING_ERROR,     /**< Error occurred when establishing the connection. In this event, an error is passed from the function call @ref sd_ble_gap_connect. */
+    NRF_BLE_SCAN_EVT_CONNECTED             /**< Connected to device. */
+} nrf_ble_scan_evt_t;
+
+
+/**@brief Types of filters.
+ */
+typedef enum
+{
+    SCAN_NAME_FILTER,       /**< Filter for names. */
+    SCAN_SHORT_NAME_FILTER, /**< Filter for short names. */
+    SCAN_ADDR_FILTER,       /**< Filter for addresses. */
+    SCAN_UUID_FILTER,       /**< Filter for UUIDs. */
+    SCAN_APPEARANCE_FILTER, /**< Filter for appearances. */
+} nrf_ble_scan_filter_type_t;
+
+
+typedef struct
+{
+    char const * p_short_name;       /**< Pointer to the short name. */
+    uint8_t      short_name_min_len; /**< Minimum length of the short name. */
+} nrf_ble_scan_short_name_t;
+
+/**@brief Structure for Scanning Module initialization.
+ */
+typedef struct
+{
+    ble_gap_scan_params_t const * p_scan_param;     /**< BLE GAP scan parameters required to initialize the module. Can be initialized as NULL. If NULL, the parameters required to initialize the module are loaded from the static configuration. */
+    bool                          connect_if_match; /**< If set to true, the module automatically connects after a filter match or successful identification of a device from the whitelist. */
+    ble_gap_conn_params_t const * p_conn_param;     /**< Connection parameters. Can be initialized as NULL. If NULL, the default static configuration is used. */
+    uint8_t                       conn_cfg_tag;     /**< Variable to keep track of what connection settings will be used if a filer match or a whitelist match results in a connection. */
+} nrf_ble_scan_init_t;
+
+
+/**@brief Structure for setting the filter status.
+ *
+ * @details This structure is used for sending filter status to the main application.
+ */
+typedef struct
+{
+    uint8_t name_filter_match       : 1; /**< Set to 1 if name filter is matched. */
+    uint8_t address_filter_match    : 1; /**< Set to 1 if address filter is matched. */
+    uint8_t uuid_filter_match       : 1; /**< Set to 1 if uuid filter is matched. */
+    uint8_t appearance_filter_match : 1; /**< Set to 1 if appearance filter is matched. */
+    uint8_t short_name_filter_match : 1; /**< Set to 1 if short name filter is matched. */
+} nrf_ble_scan_filter_match;
+
+
+/**@brief Event structure for @ref NRF_BLE_SCAN_EVT_FILTER_MATCH.
+ */
+typedef struct
+{
+    ble_gap_evt_adv_report_t const * p_adv_report; /**< Event structure for @ref BLE_GAP_EVT_ADV_REPORT. This data allows the main application to establish connection. */
+    nrf_ble_scan_filter_match        filter_match; /**< Matching filters. Information about matched filters. */
+} nrf_ble_scan_evt_filter_match_t;
+
+
+/**@brief Event structure for @ref NRF_BLE_SCAN_EVT_CONNECTING_ERROR.
+ */
+typedef struct
+{
+    ret_code_t err_code; /**< Indicates success or failure of an API procedure. In case of failure, a comprehensive error code indicating the cause or reason for failure is provided. */
+} nrf_ble_scan_evt_connecting_err_t;
+
+
+/**@brief Event structure for @ref NRF_BLE_SCAN_EVT_CONNECTED.
+ */
+typedef struct
+{
+    ble_gap_evt_connected_t const * p_connected; /**< Connected event parameters. */
+    uint16_t                        conn_handle; /**< Connection handle of the device on which the event occurred. */
+} nrf_ble_scan_evt_connected_t;
+
+
+/**@brief Structure for Scanning Module event data.
+ *
+ * @details This structure is used to send module event data to the main application when an event occurs.
+ */
+typedef struct
+{
+    nrf_ble_scan_evt_t scan_evt_id; /**< Type of event propagated to the main application. */
+    union
+    {
+        nrf_ble_scan_evt_filter_match_t   filter_match;           /**< Scan filter match. */
+        ble_gap_evt_timeout_t             timeout;                /**< Timeout event parameters. */
+        ble_gap_evt_adv_report_t const  * p_whitelist_adv_report; /**< Advertising report event parameters for whitelist. */
+        ble_gap_evt_adv_report_t const  * p_not_found;            /**< Advertising report event parameters when filter is not found. */
+        nrf_ble_scan_evt_connected_t      connected;              /**< Connected event parameters. */
+        nrf_ble_scan_evt_connecting_err_t connecting_err;         /**< Error event when connecting. Propagates the error code returned by the SoftDevice API @ref sd_ble_gap_scan_start. */
+    } params;
+    ble_gap_scan_params_t const * p_scan_params;                  /**< GAP scanning parameters. These parameters are needed to establish connection. */
+} scan_evt_t;
+
+
+/**@brief BLE scanning event handler type.
+ */
+typedef void (*nrf_ble_scan_evt_handler_t)(scan_evt_t const * p_scan_evt);
+
+
+#if (NRF_BLE_SCAN_FILTER_ENABLE == 1)
+
+#if (NRF_BLE_SCAN_NAME_CNT > 0)
+typedef struct
+{
+    char    target_name[NRF_BLE_SCAN_NAME_CNT][NRF_BLE_SCAN_NAME_MAX_LEN]; /**< Names that the main application will scan for, and that will be advertised by the peripherals. */
+    uint8_t name_cnt;                                                      /**< Name filter counter. */
+    bool    name_filter_enabled;                                           /**< Flag to inform about enabling or disabling this filter. */
+} nrf_ble_scan_name_filter_t;
+#endif
+
+#if (NRF_BLE_SCAN_SHORT_NAME_CNT > 0)
+typedef struct
+{
+    struct
+    {
+        char    short_target_name[NRF_BLE_SCAN_SHORT_NAME_MAX_LEN]; /**< Short names that the main application will scan for, and that will be advertised by the peripherals. */
+        uint8_t short_name_min_len;                                 /**< Minimum length of the short name. */
+    } short_name[NRF_BLE_SCAN_SHORT_NAME_CNT];
+    uint8_t name_cnt;                                               /**< Short name filter counter. */
+    bool    short_name_filter_enabled;                              /**< Flag to inform about enabling or disabling this filter. */
+} nrf_ble_scan_short_name_filter_t;
+#endif
+
+#if (NRF_BLE_SCAN_ADDRESS_CNT > 0)
+typedef struct
+{
+    ble_gap_addr_t target_addr[NRF_BLE_SCAN_ADDRESS_CNT]; /**< Addresses in the same format as the format used by the SoftDevice that the main application will scan for, and that will be advertised by the peripherals. */
+    uint8_t        addr_cnt;                              /**< Address filter counter. */
+    bool           addr_filter_enabled;                   /**< Flag to inform about enabling or disabling this filter. */
+} nrf_ble_scan_addr_filter_t;
+#endif
+
+#if (NRF_BLE_SCAN_UUID_CNT > 0)
+typedef struct
+{
+    ble_uuid_t uuid[NRF_BLE_SCAN_UUID_CNT]; /**< UUIDs that the main application will scan for, and that will be advertised by the peripherals. */
+    uint8_t    uuid_cnt;                    /**< UUID filter counter. */
+    bool       uuid_filter_enabled;         /**< Flag to inform about enabling or disabling this filter. */
+} nrf_ble_scan_uuid_filter_t;
+#endif
+
+#if (NRF_BLE_SCAN_APPEARANCE_CNT > 0)
+typedef struct
+{
+    uint16_t appearance[NRF_BLE_SCAN_APPEARANCE_CNT]; /**< Apperances that the main application will scan for, and that will be advertised by the peripherals. */
+    uint8_t  appearance_cnt;                          /**< Appearance filter counter. */
+    bool     appearance_filter_enabled;               /**< Flag to inform about enabling or disabling this filter. */
+} nrf_ble_scan_appearance_filter_t;
+#endif
+
+/**@brief Filters data.
+ *
+ * @details This structure contains all filter data and the information about enabling and disabling any type of filters.
+ *          Flag all_filter_mode informs about the filter mode. If this flag is set, then all types of enabled
+ *          filters must be matched for the module to send a notification to the main application. Otherwise, it is enough to match
+ *          one of filters to send notification.
+ */
+typedef struct
+{
+#if (NRF_BLE_SCAN_NAME_CNT > 0)
+    nrf_ble_scan_name_filter_t name_filter; /**< Name filter data. */
+#endif
+#if (NRF_BLE_SCAN_SHORT_NAME_CNT > 0)
+    nrf_ble_scan_short_name_filter_t short_name_filter; /**< Short name filter data. */
+#endif
+#if (NRF_BLE_SCAN_ADDRESS_CNT > 0)
+    nrf_ble_scan_addr_filter_t addr_filter; /**< Address filter data. */
+#endif
+#if (NRF_BLE_SCAN_UUID_CNT > 0)
+    nrf_ble_scan_uuid_filter_t uuid_filter; /**< UUID filter data. */
+#endif
+#if (NRF_BLE_SCAN_APPEARANCE_CNT > 0)
+    nrf_ble_scan_appearance_filter_t appearance_filter; /**< Appearance filter data. */
+#endif
+    bool all_filters_mode;                              /**< Filter mode. If true, all set filters must be matched to generate an event.*/
+} nrf_ble_scan_filters_t;
+
+#endif // NRF_BLE_SCAN_FILTER_ENABLE
+
+/**@brief Scan module instance. Options for the different scanning modes.
+ *
+ * @details This structure stores all module settings. It is used to enable or disable scanning modes
+ *          and to configure filters.
+ */
+typedef struct
+{
+#if (NRF_BLE_SCAN_FILTER_ENABLE == 1)
+    nrf_ble_scan_filters_t scan_filters;                              /**< Filter data. */
+#endif
+    bool                       connect_if_match;                      /**< If set to true, the module automatically connects after a filter match or successful identification of a device from the whitelist. */
+    ble_gap_conn_params_t      conn_params;                           /**< Connection parameters. */
+    uint8_t                    conn_cfg_tag;                          /**< Variable to keep track of what connection settings will be used if a filer match or a whitelist match results in a connection. */
+    ble_gap_scan_params_t      scan_params;                           /**< GAP scanning parameters. */
+    nrf_ble_scan_evt_handler_t evt_handler;                           /**< Handler for the scanning events. Can be initialized as NULL if no handling is implemented in the main application. */
+    uint8_t                    scan_buffer_data[NRF_BLE_SCAN_BUFFER]; /**< Buffer where advertising reports will be stored by the SoftDevice. */
+    ble_data_t                 scan_buffer;                           /**< Structure-stored pointer to the buffer where advertising reports will be stored by the SoftDevice. */
+} nrf_ble_scan_t;
+
+
+/**@brief Function for indicating that the Scanning Module is using the whitelist.
+ *
+ * @param[in] p_scan_ctx Pointer to the Scanning Module instance.
+ *
+ * @return Whether the whitelist is used.
+ */
+bool is_whitelist_used(nrf_ble_scan_t const * const p_scan_ctx);
+
+
+/**@brief Function for initializing the Scanning Module.
+ *
+ * @param[out] p_scan_ctx   Pointer to the Scanning Module instance. This structure must be supplied by
+ *                          the application. It is initialized by this function and is later used
+ *                          to identify this particular module instance.
+ * 
+ * @param[in]  p_init       Can be initialized as NULL. If NULL, the parameters required to initialize
+ *                          the module are loaded from static configuration.
+ *                          If module is to establish the connection automatically, this must be initialized
+ *                          with the relevant data.
+ * @param[in]  evt_handler  Handler for the scanning events.
+ *                          Can be initialized as NULL if no handling is implemented in the main application.
+ *
+ * @retval NRF_SUCCESS      If initialization was successful.
+ * @retval NRF_ERROR_NULL   When the NULL pointer is passed as input.
+ */
+ret_code_t nrf_ble_scan_init(nrf_ble_scan_t            * const p_scan_ctx,
+                             nrf_ble_scan_init_t const * const p_init,
+                             nrf_ble_scan_evt_handler_t        evt_handler);
+
+
+/**@brief Function for starting scanning.
+ *
+ * @details This function starts the scanning according to the configuration set during the initialization.
+ *
+ * @param[in] p_scan_ctx       Pointer to the Scanning Module instance.
+ *
+ * @retval    NRF_SUCCESS      If scanning started. Otherwise, an error code is returned.
+ * @retval    NRF_ERROR_NULL   If NULL pointer is passed as input.
+ *
+ * @return                     This API propagates the error code returned by the
+ *                             SoftDevice API @ref sd_ble_gap_scan_start.
+ */
+ret_code_t nrf_ble_scan_start(nrf_ble_scan_t const * const p_scan_ctx);
+
+
+/**@brief Function for stopping scanning.
+ */
+void nrf_ble_scan_stop(void);
+
+
+#if (NRF_BLE_SCAN_FILTER_ENABLE == 1)
+
+/**@brief Function for enabling filtering.
+ *
+ * @details The filters can be combined with each other. For example, you can enable one filter or several filters.
+ *          For example, (NRF_BLE_SCAN_NAME_FILTER | NRF_BLE_SCAN_UUID_FILTER) enables UUID and name filters.
+ *
+ * @param[in] mode                  Filter mode: @ref NRF_BLE_SCAN_FILTER_MODE.
+ * @param[in] match_all             If this flag is set, all types of enabled filters must be matched
+ *                                  before generating @ref NRF_BLE_SCAN_EVT_FILTER_MATCH to the main application. Otherwise, it is enough to match
+ *                                  one filter to trigger the filter match event.
+ * @param[in] p_scan_ctx            Pointer to the Scanning Module instance.
+ *
+ * @retval NRF_SUCCESS              If the filters are enabled successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If the filter mode is incorrect. Available filter modes: @ref NRF_BLE_SCAN_FILTER_MODE.
+ * @retval NRF_ERROR_NULL           If a NULL pointer is passed as input.
+ */
+ret_code_t nrf_ble_scan_filters_enable(nrf_ble_scan_t * const p_scan_ctx,
+                                       uint8_t                mode,
+                                       bool                   match_all);
+
+
+/**@brief Function for disabling filtering.
+ *
+ * @details This function disables all filters.
+ *          Even if the automatic connection establishing is enabled,
+ *          the connection will not be established with the first
+            device found after this function is called.
+ *
+ * @param[in] p_scan_ctx            Pointer to the Scanning Module instance.
+ *
+ * @retval NRF_SUCCESS              If filters are disabled successfully.
+ * @retval NRF_ERROR_NULL           If a NULL pointer is passed as input.
+ */
+ret_code_t nrf_ble_scan_filters_disable(nrf_ble_scan_t * const p_scan_ctx);
+
+
+/**@brief Function for getting filter status.
+ *
+ * @details This function returns the filter setting and whether it is enabled or disabled.
+
+ * @param[out] p_status              Filter status.
+ * @param[in]  p_scan_ctx            Pointer to the Scanning Module instance.
+ *
+ * @retval NRF_SUCCESS               If filter status is returned.
+ * @retval NRF_ERROR_NULL            If a NULL pointer is passed as input.
+ */
+ret_code_t nrf_ble_scan_filter_get(nrf_ble_scan_t   * const p_scan_ctx,
+                                   nrf_ble_scan_filters_t * p_status);
+
+
+/**@brief Function for adding any type of filter to the scanning.
+ *
+ * @details This function adds a new filter by type @ref nrf_ble_scan_filter_type_t.
+ *          The filter will be added if the number of filters of a given type does not exceed @ref NRF_BLE_SCAN_UUID_CNT,
+ *          @ref NRF_BLE_SCAN_NAME_CNT, @ref NRF_BLE_SCAN_ADDRESS_CNT, or @ref NRF_BLE_SCAN_APPEARANCE_CNT, depending on the filter type,
+ *          and if the same filter has not already been set.
+ *
+ * @param[in,out] p_scan_ctx        Pointer to the Scanning Module instance.
+ * @param[in]     type              Filter type.
+ * @param[in]     p_data            The filter data to add.
+ *
+ * @retval NRF_SUCCESS                    If the filter is added successfully.
+ * @retval NRF_ERROR_NULL                 If a NULL pointer is passed as input.
+ * @retval NRF_ERROR_DATA_SIZE            If the name filter length is too long. Maximum name filter length corresponds to @ref NRF_BLE_SCAN_NAME_MAX_LEN.
+ * @retval NRF_ERROR_NO_MEMORY            If the number of available filters is exceeded.
+ * @retval NRF_ERROR_INVALID_PARAM        If the filter type is incorrect. Available filter types: @ref nrf_ble_scan_filter_type_t.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR If the BLE address type is invalid.
+ */
+ret_code_t nrf_ble_scan_filter_set(nrf_ble_scan_t     * const p_scan_ctx,
+                                   nrf_ble_scan_filter_type_t type,
+                                   void const               * p_data);
+
+
+/**@brief Function for removing all set filters.
+ *
+ * @details The function removes all previously set filters.
+ *
+ * @note After using this function the filters are still enabled.
+ *
+ * @param[in,out] p_scan_ctx Pointer to the Scanning Module instance.
+ *
+ * @retval NRF_SUCCESS       If all filters are removed successfully.
+ */
+ret_code_t nrf_ble_scan_all_filter_remove(nrf_ble_scan_t * const p_scan_ctx);
+
+
+#endif // NRF_BLE_SCAN_FILTER_ENABLE
+
+
+/**@brief Function for changing the scanning parameters.
+ *
+ **@details Use this function to change scanning parameters. During the parameter change
+ *         the scan is stopped. To resume scanning, use @ref nrf_ble_scan_start.
+ *         Scanning parameters can be set to NULL. If so, the default static configuration
+ *         is used. For example, use this function when the @ref NRF_BLE_SCAN_EVT_WHITELIST_ADV_REPORT event is generated.
+ *         The generation of this event means that there is a risk that the whitelist is empty. In such case, this function can change
+ *         the scanning parameters, so that the whitelist is not used, and you avoid the error caused by scanning with the whitelist
+ *         when there are no devices on the whitelist.
+ *
+ * @param[in,out] p_scan_ctx     Pointer to the Scanning Module instance.
+ * @param[in]     p_scan_param   GAP scanning parameters. Can be initialized as NULL.
+ *
+ * @retval NRF_SUCCESS          If parameters are changed successfully.
+ * @retval NRF_ERROR_NULL       If a NULL pointer is passed as input.
+ */
+ret_code_t nrf_ble_scan_params_set(nrf_ble_scan_t        * const p_scan_ctx,
+                                   ble_gap_scan_params_t const * p_scan_param);
+
+
+/**@brief Function for handling the BLE stack events of the application.
+ *
+ * @param[in]     p_ble_evt     Pointer to the BLE event received.
+ * @param[in,out] p_scan        Pointer to the Scanning Module instance.
+ */
+void nrf_ble_scan_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_scan);
+
+
+/**@brief Function for converting the raw address to the SoftDevice GAP address.
+ *
+ * @details This function inverts the byte order in the address. If you enter the address as it is displayed
+ *          (for example, on a phone screen from left to right), you must use this function to
+ *          convert the address to the SoftDevice address type.
+ *
+ * @note This function does not decode an address type.
+ *
+ * @param[out] p_gap_addr The Bluetooth Low Energy address.
+ * @param[in]  addr       Address to be converted to the SoftDevice address.
+ *
+ * @retval NRF_ERROR_NULL                 If a NULL pointer is passed as input.
+ * @retval NRF_SUCCESS                    If the address is copied and converted successfully.
+ */
+ret_code_t nrf_ble_scan_copy_addr_to_sd_gap_addr(ble_gap_addr_t * p_gap_addr,
+                                                 uint8_t    const addr[BLE_GAP_ADDR_LEN]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_BLE_SCAN_H__
+
+/** @} */

components/ble/peer_manager/auth_status_tracker.c

@@ -0,0 +1,340 @@
+/**
+ * Copyright (c) 2018 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER) && NRF_MODULE_ENABLED(PM_RA_PROTECTION)
+#include "auth_status_tracker.h"
+
+#include "app_timer.h"
+#include "id_manager.h"
+
+#define NRF_LOG_MODULE_NAME     peer_manager_ast
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+#include "nrf_strerror.h"
+
+// Assume that waiting interval doubles with each failed authentication.
+//lint --emacro((647),PENALITY_LVL_TO_PENALITY_MS)
+#define PAIR_REWARD_TICKS                    APP_TIMER_TICKS(PM_RA_PROTECTION_REWARD_PERIOD)
+#define PENALITY_LVL_TO_PENALITY_MS(_lvl)    (PM_RA_PROTECTION_MIN_WAIT_INTERVAL * (1 << _lvl))
+#define PENALITY_LVL_TO_PENALITY_TICKS(_lvl) APP_TIMER_TICKS(PENALITY_LVL_TO_PENALITY_MS(_lvl))
+#define PENALITY_LVL_NEXT_SET(_lvl)                                                      \
+    _lvl = (PENALITY_LVL_TO_PENALITY_MS(_lvl) >= (PM_RA_PROTECTION_MAX_WAIT_INTERVAL)) ? \
+           (_lvl) : (_lvl + 1)
+
+
+/**@brief Tracked peer state. */
+typedef struct
+{
+    ble_gap_addr_t peer_addr;      /**< BLE address, used to identify peer. */
+    uint32_t       reward_ticks;   /**< Accumulated reward ticks, used to decrease penality level
+                                        after achieving certain threshold. */
+    uint32_t       penality_ticks; /**< Accumulated penality ticks, used to determine remaining time
+                                        in which pairing attempts should be rejected. */
+    uint8_t        penality_lvl;   /**< Accumulated penality level, used to determine waiting interval
+                                        after failed authorization attempt. */
+    bool           is_active;      /**< Flag indicating that the waiting interval for this peer has not
+                                        passed yet. */
+    bool           is_valid;       /**< Flag indicating that this entry is valid in the peer blacklist. */
+} blacklisted_peer_t;
+
+APP_TIMER_DEF(m_pairing_attempt_timer);
+static blacklisted_peer_t m_blacklisted_peers[PM_RA_PROTECTION_TRACKED_PEERS_NUM];
+static uint32_t           m_ticks_cnt;
+
+
+/**@brief Function for updating the state of blacklisted peers after timer has been stopped or
+ *        timed out.
+ *
+ * @param[in]  ticks_passed  The number of ticks since the timer has started.
+ */
+static uint32_t blacklisted_peers_state_update(uint32_t ticks_passed)
+{
+    uint32_t minimal_ticks = UINT32_MAX;
+
+    for (uint32_t id = 0; id < ARRAY_SIZE(m_blacklisted_peers); id++)
+    {
+        blacklisted_peer_t * p_bl_peer = &m_blacklisted_peers[id];
+
+        if (p_bl_peer->is_valid)
+        {
+            if (p_bl_peer->is_active)
+            {
+                if (p_bl_peer->penality_ticks > ticks_passed)
+                {
+                    p_bl_peer->penality_ticks -= ticks_passed;
+                    minimal_ticks              = MIN(minimal_ticks, p_bl_peer->penality_ticks);
+                }
+                else
+                {
+                    p_bl_peer->is_active = false;
+
+                    if (p_bl_peer->penality_lvl == 0)
+                    {
+                        p_bl_peer->is_valid = false;
+                        NRF_LOG_DEBUG("Peer has been removed from the blacklist, its address:");
+                        NRF_LOG_HEXDUMP_DEBUG(p_bl_peer->peer_addr.addr,
+                                              sizeof(p_bl_peer->peer_addr.addr));
+                    }
+                    else
+                    {
+                        minimal_ticks = MIN(minimal_ticks, PAIR_REWARD_TICKS);
+                    }
+
+                    NRF_LOG_DEBUG("Pairing waiting interval has expired for:");
+                    NRF_LOG_HEXDUMP_DEBUG(p_bl_peer->peer_addr.addr,
+                                          sizeof(p_bl_peer->peer_addr.addr));
+                }
+            }
+            else
+            {
+                if (p_bl_peer->penality_lvl == 0)
+                {
+                    p_bl_peer->is_valid = false;
+                    NRF_LOG_DEBUG("Peer has been removed from the blacklist, its address:");
+                    NRF_LOG_HEXDUMP_DEBUG(p_bl_peer->peer_addr.addr,
+                                          sizeof(p_bl_peer->peer_addr.addr));
+                }
+                else
+                {
+                    p_bl_peer->reward_ticks += ticks_passed;
+                    if (p_bl_peer->reward_ticks >= PAIR_REWARD_TICKS)
+                    {
+                        p_bl_peer->penality_lvl--;
+                        p_bl_peer->reward_ticks -= PAIR_REWARD_TICKS;
+                        NRF_LOG_DEBUG("Peer penality level has decreased to %d for device:",
+                                      p_bl_peer->penality_lvl);
+                        NRF_LOG_HEXDUMP_DEBUG(p_bl_peer->peer_addr.addr,
+                                              sizeof(p_bl_peer->peer_addr.addr));
+                    }
+
+                    minimal_ticks = MIN(minimal_ticks,
+                                        (PAIR_REWARD_TICKS - p_bl_peer->reward_ticks));
+                }
+            }
+        }
+    }
+
+    return minimal_ticks;
+}
+
+
+/**@brief Function for handling state transition of blacklisted peers.
+ *
+ * @param[in]  context  Context containing the number of ticks since the timer has started.
+ */
+static void blacklisted_peers_state_transition_handle(void * context)
+{
+    ret_code_t err_code;
+    uint32_t   minimal_ticks;
+    uint32_t   ticks_passed = (uint32_t) context;
+
+    minimal_ticks = blacklisted_peers_state_update(ticks_passed);
+    m_ticks_cnt   = app_timer_cnt_get();
+
+    if (minimal_ticks != UINT32_MAX)
+    {
+        err_code = app_timer_start(m_pairing_attempt_timer,
+                                   minimal_ticks,
+                                   (void *) minimal_ticks);
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_WARNING("app_timer_start() returned %s", nrf_strerror_get(err_code));
+        }
+        NRF_LOG_DEBUG("Restarting the timer");
+    }
+}
+
+
+ret_code_t ast_init(void)
+{
+    ret_code_t err_code = app_timer_create(&m_pairing_attempt_timer,
+                                           APP_TIMER_MODE_SINGLE_SHOT,
+                                           blacklisted_peers_state_transition_handle);
+
+    return err_code;
+}
+
+
+void ast_auth_error_notify(uint16_t conn_handle)
+{
+    ret_code_t     err_code;
+    ble_gap_addr_t peer_addr;
+    uint32_t       new_timeout;
+    uint32_t       free_id      = ARRAY_SIZE(m_blacklisted_peers);
+    bool           new_bl_entry = true;
+
+    // Get the peer address associated with connection handle.
+    err_code = im_ble_addr_get(conn_handle, &peer_addr);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_WARNING("im_ble_addr_get() returned %s. conn_handle: %d. "
+                        "Link was likely disconnected.",
+                        nrf_strerror_get(err_code),
+                        conn_handle);
+        return;
+    }
+
+    // Stop the timer and update the state of all blacklisted peers.
+    err_code = app_timer_stop(m_pairing_attempt_timer);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_WARNING("app_timer_stop() returned %s", nrf_strerror_get(err_code));
+        return;
+    }
+
+    new_timeout = blacklisted_peers_state_update(app_timer_cnt_diff_compute(app_timer_cnt_get(),
+                                                                            m_ticks_cnt));
+    m_ticks_cnt = app_timer_cnt_get();
+
+    // Check if authorization has failed for already blacklisted peer.
+    for (uint32_t id = 0; id < ARRAY_SIZE(m_blacklisted_peers); id++)
+    {
+        blacklisted_peer_t * p_bl_peer = &m_blacklisted_peers[id];
+
+        if (p_bl_peer->is_valid)
+        {
+            if (memcmp(peer_addr.addr, p_bl_peer->peer_addr.addr, BLE_GAP_ADDR_LEN) == 0)
+            {
+                uint8_t lvl = p_bl_peer->penality_lvl;
+
+                PENALITY_LVL_NEXT_SET(lvl);
+                p_bl_peer->penality_lvl   = lvl;
+                p_bl_peer->reward_ticks   = 0;
+                p_bl_peer->penality_ticks = PENALITY_LVL_TO_PENALITY_TICKS(lvl);
+
+                new_timeout = MIN(new_timeout, p_bl_peer->penality_ticks);
+
+                p_bl_peer->is_active = true;
+                new_bl_entry         = false;
+
+                NRF_LOG_DEBUG("Pairing waiting interval has been renewed. "
+                              "Penality level: %d for device:",
+                              lvl);
+                NRF_LOG_HEXDUMP_DEBUG(p_bl_peer->peer_addr.addr,
+                                      sizeof(p_bl_peer->peer_addr.addr));
+            }
+        }
+        else
+        {
+            free_id = id;
+        }
+    }
+
+    // Add a new peer to the blacklist.
+    if (new_bl_entry)
+    {
+        if (free_id < ARRAY_SIZE(m_blacklisted_peers))
+        {
+            blacklisted_peer_t * p_bl_peer = &m_blacklisted_peers[free_id];
+
+            memcpy(&p_bl_peer->peer_addr, &peer_addr, sizeof(peer_addr));
+
+            p_bl_peer->penality_lvl   = 0;
+            p_bl_peer->reward_ticks   = 0;
+            p_bl_peer->penality_ticks = PENALITY_LVL_TO_PENALITY_TICKS(p_bl_peer->penality_lvl);
+
+            new_timeout = MIN(new_timeout, p_bl_peer->penality_ticks);
+
+            p_bl_peer->is_active = true;
+            p_bl_peer->is_valid  = true;
+            NRF_LOG_DEBUG("New peer has been added to the blacklist:");
+            NRF_LOG_HEXDUMP_DEBUG(p_bl_peer->peer_addr.addr, sizeof(p_bl_peer->peer_addr.addr));
+        }
+        else
+        {
+            NRF_LOG_WARNING("No space to blacklist another peer ID");
+        }
+    }
+
+    // Restart the timer.
+    if (new_timeout != UINT32_MAX)
+    {
+        err_code = app_timer_start(m_pairing_attempt_timer,
+                                   new_timeout,
+                                   (void *) new_timeout);
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_WARNING("app_timer_start() returned %s", nrf_strerror_get(err_code));
+        }
+    }
+}
+
+
+bool ast_peer_blacklisted(uint16_t conn_handle)
+{
+    ret_code_t     err_code;
+    ble_gap_addr_t peer_addr;
+
+    err_code = im_ble_addr_get(conn_handle, &peer_addr);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_WARNING("im_ble_addr_get() returned %s. conn_handle: %d. "
+                        "Link was likely disconnected.",
+                        nrf_strerror_get(err_code),
+                        conn_handle);
+        return true;
+    }
+
+    for (uint32_t id = 0; id < ARRAY_SIZE(m_blacklisted_peers); id++)
+    {
+        blacklisted_peer_t * p_bl_peer = &m_blacklisted_peers[id];
+
+        if (p_bl_peer->is_valid)
+        {
+            if ((memcmp(peer_addr.addr, p_bl_peer->peer_addr.addr, BLE_GAP_ADDR_LEN) == 0) &&
+                (p_bl_peer->is_active))
+            {
+                return true;
+            }
+        }
+    }
+
+    return false;
+}
+
+
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER) && NRF_MODULE_ENABLED(PM_RA_PROTECTION)

components/ble/peer_manager/auth_status_tracker.h

@@ -0,0 +1,97 @@
+/**
+ * Copyright (c) 2018 - 2021, 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.
+ *
+ */
+#ifndef AUTH_STATUS_TRACKER_H__
+#define AUTH_STATUS_TRACKER_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble_gap.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup auth_status_tracker Authorization Status Tracker
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for tracking peers with failed
+ *        authorization attempts. It uses tracking policy, which is described in Bluetooth
+ *        Core Specification v5.0, Vol 3, Part H, Section 2.3.6.
+ */
+
+
+/**@brief Function for initializing the Authorization Status Tracker module.
+ *
+ * @retval NRF_SUCCESS Initialization was successful.
+ * @retval Other       Other error codes might be returned by the @ref app_timer_create function.
+ */
+ret_code_t ast_init(void);
+
+
+/**@brief Function for notifying about failed authorization attempts.
+ *
+ * @param[in]  conn_handle  Connection handle on which authorization attempt has failed.
+ */
+void ast_auth_error_notify(uint16_t conn_handle);
+
+
+/**@brief Function for checking if pairing request must be rejected.
+ *
+ * @param[in]  conn_handle  Connection handle on which this check must be performed.
+ *
+ * @retval  true   If the connected peer is blacklisted.
+ * @retval  false  If the connected peer is not blacklisted.
+ */
+bool ast_peer_blacklisted(uint16_t conn_handle);
+
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* AUTH_STATUS_TRACKER_H__ */

components/ble/peer_manager/gatt_cache_manager.c

@@ -0,0 +1,842 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "gatt_cache_manager.h"
+
+#include "ble_gap.h"
+#include "ble_err.h"
+#include "ble_conn_state.h"
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+#include "id_manager.h"
+#include "gatts_cache_manager.h"
+#include "peer_data_storage.h"
+#include "peer_database.h"
+#include "nrf_mtx.h"
+
+#define NRF_LOG_MODULE_NAME peer_manager_gcm
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+#include "nrf_strerror.h"
+
+// The number of registered event handlers.
+#define GCM_EVENT_HANDLERS_CNT      (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
+
+// GATT Cache Manager event handler in Peer Manager.
+extern void pm_gcm_evt_handler(pm_evt_t * p_gcm_evt);
+
+// GATT Cache Manager events' handlers.
+// The number of elements in this array is GCM_EVENT_HANDLERS_CNT.
+static pm_evt_handler_internal_t m_evt_handlers[] =
+{
+    pm_gcm_evt_handler
+};
+
+static bool                           m_module_initialized;
+static nrf_mtx_t                      m_db_update_in_progress_mutex;  /**< Mutex indicating whether a local DB write operation is ongoing. */
+static ble_conn_state_user_flag_id_t  m_flag_local_db_update_pending; /**< Flag ID for flag collection to keep track of which connections need a local DB update procedure. */
+static ble_conn_state_user_flag_id_t  m_flag_local_db_apply_pending;  /**< Flag ID for flag collection to keep track of which connections need a local DB apply procedure. */
+static ble_conn_state_user_flag_id_t  m_flag_service_changed_pending; /**< Flag ID for flag collection to keep track of which connections need to be sent a service changed indication. */
+static ble_conn_state_user_flag_id_t  m_flag_service_changed_sent;    /**< Flag ID for flag collection to keep track of which connections have been sent a service changed indication and are waiting for a handle value confirmation. */
+static ble_conn_state_user_flag_id_t  m_flag_car_update_pending;      /**< Flag ID for flag collection to keep track of which connections need to have their Central Address Resolution value stored. */
+static ble_conn_state_user_flag_id_t  m_flag_car_handle_queried;      /**< Flag ID for flag collection to keep track of which connections are pending Central Address Resolution handle reply. */
+static ble_conn_state_user_flag_id_t  m_flag_car_value_queried;       /**< Flag ID for flag collection to keep track of which connections are pending Central Address Resolution value reply. */
+
+#ifdef PM_SERVICE_CHANGED_ENABLED
+    STATIC_ASSERT(PM_SERVICE_CHANGED_ENABLED || !NRF_SDH_BLE_SERVICE_CHANGED,
+                 "PM_SERVICE_CHANGED_ENABLED should be enabled if NRF_SDH_BLE_SERVICE_CHANGED is enabled.");
+#else
+    #define PM_SERVICE_CHANGED_ENABLED 1
+#endif
+
+/**@brief Function for resetting the module variable(s) of the GSCM module.
+ *
+ * @param[out]  The instance to reset.
+ */
+static void internal_state_reset()
+{
+    m_module_initialized = false;
+}
+
+
+static void evt_send(pm_evt_t * p_gcm_evt)
+{
+    p_gcm_evt->peer_id = im_peer_id_get_by_conn_handle(p_gcm_evt->conn_handle);
+
+    for (uint32_t i = 0; i < GCM_EVENT_HANDLERS_CNT; i++)
+    {
+        m_evt_handlers[i](p_gcm_evt);
+    }
+}
+
+
+/**@brief Function for checking a write event for whether a CCCD was written during the write
+ *        operation.
+ *
+ * @param[in]  p_write_evt  The parameters of the write event.
+ *
+ * @return  Whether the write was on a CCCD.
+ */
+static bool cccd_written(ble_gatts_evt_write_t const * p_write_evt)
+{
+    return (    (p_write_evt->op        == BLE_GATTS_OP_WRITE_REQ)
+             && (p_write_evt->uuid.type == BLE_UUID_TYPE_BLE)
+             && (p_write_evt->uuid.uuid == BLE_UUID_DESCRIPTOR_CLIENT_CHAR_CONFIG)
+           );
+}
+
+
+/**@brief Function for sending an PM_EVT_ERROR_UNEXPECTED event.
+ *
+ * @param[in]  conn_handle  The connection handle the event pertains to.
+ * @param[in]  err_code     The unexpected error that occurred.
+ */
+static void send_unexpected_error(uint16_t conn_handle, ret_code_t err_code)
+{
+    pm_evt_t error_evt =
+    {
+        .evt_id = PM_EVT_ERROR_UNEXPECTED,
+        .conn_handle = conn_handle,
+        .params =
+        {
+            .error_unexpected =
+            {
+                .error = err_code
+            }
+        }
+    };
+    evt_send(&error_evt);
+}
+
+
+/**@brief Function for performing the local DB update procedure in an event context, where no return
+ *        code can be given.
+ *
+ * @details This function will do the procedure, and check the result, set a flag if needed, and
+ *          send an event if needed.
+ *
+ * @param[in]  conn_handle  The connection to perform the procedure on.
+ */
+static void local_db_apply_in_evt(uint16_t conn_handle)
+{
+    bool set_procedure_as_pending = false;
+    ret_code_t err_code;
+    pm_evt_t event =
+    {
+        .conn_handle = conn_handle,
+    };
+
+    if (conn_handle == BLE_CONN_HANDLE_INVALID)
+    {
+        return;
+    }
+
+    err_code = gscm_local_db_cache_apply(conn_handle);
+
+    switch (err_code)
+    {
+        case NRF_SUCCESS:
+            event.evt_id = PM_EVT_LOCAL_DB_CACHE_APPLIED;
+
+            evt_send(&event);
+            break;
+
+        case NRF_ERROR_BUSY:
+            set_procedure_as_pending = true;
+            break;
+
+        case NRF_ERROR_INVALID_DATA:
+            event.evt_id = PM_EVT_LOCAL_DB_CACHE_APPLY_FAILED;
+
+            NRF_LOG_WARNING("The local database has changed, so some subscriptions to notifications "\
+                            "and indications could not be restored for conn_handle %d",
+                            conn_handle);
+            evt_send(&event);
+            break;
+
+        case BLE_ERROR_INVALID_CONN_HANDLE:
+            /* Do nothing */
+            break;
+
+        default:
+            NRF_LOG_ERROR("gscm_local_db_cache_apply() returned %s which should not happen. "\
+                          "conn_handle: %d",
+                          nrf_strerror_get(err_code),
+                          conn_handle);
+            send_unexpected_error(conn_handle, err_code);
+            break;
+    }
+
+    ble_conn_state_user_flag_set(conn_handle, m_flag_local_db_apply_pending, set_procedure_as_pending);
+}
+
+
+/**@brief Function for asynchronously starting a DB update procedure.
+ *
+ * @note This procedure can only be started asynchronously.
+ *
+ * @param[in]  conn_handle  The connection to perform the procedure on.
+ * @param[in]  update       Whether to perform the procedure.
+ */
+static __INLINE void local_db_update(uint16_t conn_handle, bool update)
+{
+    ble_conn_state_user_flag_set(conn_handle, m_flag_local_db_update_pending, update);
+}
+
+
+/**@brief Function for performing the local DB update procedure in an event context, where no return
+ *        code can be given.
+ *
+ * @details This function will do the procedure, and check the result, set a flag if needed, and
+ *          send an event if needed.
+ *
+ * @param[in]  conn_handle  The connection to perform the procedure on.
+ */
+static bool local_db_update_in_evt(uint16_t conn_handle)
+{
+    bool set_procedure_as_pending = false;
+    bool success = false;
+    ret_code_t err_code = gscm_local_db_cache_update(conn_handle);
+
+    switch (err_code)
+    {
+        case NRF_SUCCESS:
+            success = true;
+            break;
+
+        case NRF_ERROR_INVALID_DATA:
+            /* Fallthrough */
+        case BLE_ERROR_INVALID_CONN_HANDLE:
+            /* Do nothing */
+            break;
+
+        case NRF_ERROR_BUSY:
+            set_procedure_as_pending = true;
+            break;
+
+        case NRF_ERROR_STORAGE_FULL:
+        {
+            pm_evt_t event =
+            {
+                .evt_id      = PM_EVT_STORAGE_FULL,
+                .conn_handle = conn_handle,
+            };
+
+            NRF_LOG_WARNING("Flash full. Could not store data for conn_handle: %d", conn_handle);
+            evt_send(&event);
+            break;
+        }
+
+        default:
+            NRF_LOG_ERROR("gscm_local_db_cache_update() returned %s for conn_handle: %d",
+                          nrf_strerror_get(err_code),
+                          conn_handle);
+            send_unexpected_error(conn_handle, err_code);
+            break;
+    }
+
+    local_db_update(conn_handle, set_procedure_as_pending);
+
+    return success;
+}
+
+#if PM_SERVICE_CHANGED_ENABLED
+
+/**@brief Function for getting the value of the CCCD for the service changed characteristic.
+ *
+ * @details This function will search all system handles consecutively.
+ *
+ * @param[in]  conn_handle  The connection to check.
+ * @param[out] p_cccd       The CCCD value of the service changed characteristic for this link.
+ *
+ * @return Any error from @ref sd_ble_gatts_value_get or @ref sd_ble_gatts_attr_get.
+ */
+static ret_code_t service_changed_cccd(uint16_t conn_handle, uint16_t * p_cccd)
+{
+    bool       sc_found = false;
+    uint16_t   end_handle;
+
+    ret_code_t err_code = sd_ble_gatts_initial_user_handle_get(&end_handle);
+    ASSERT(err_code == NRF_SUCCESS);
+
+    for (uint16_t handle = 1; handle < end_handle; handle++)
+    {
+        ble_uuid_t uuid;
+        ble_gatts_value_t value = {.p_value = (uint8_t *)&uuid.uuid, .len = 2, .offset = 0};
+
+        err_code = sd_ble_gatts_attr_get(handle, &uuid, NULL);
+        if (err_code != NRF_SUCCESS)
+        {
+            return err_code;
+        }
+        else if (!sc_found && (uuid.uuid == BLE_UUID_GATT_CHARACTERISTIC_SERVICE_CHANGED))
+        {
+            sc_found = true;
+        }
+        else if (sc_found && (uuid.uuid == BLE_UUID_DESCRIPTOR_CLIENT_CHAR_CONFIG))
+        {
+            value.p_value = (uint8_t *)p_cccd;
+            return sd_ble_gatts_value_get(conn_handle, handle, &value);
+        }
+    }
+    return NRF_ERROR_NOT_FOUND;
+}
+
+/**@brief Function for sending a service changed indication in an event context, where no return
+ *        code can be given.
+ *
+ * @details This function will do the procedure, and check the result, set a flag if needed, and
+ *          send an event if needed.
+ *
+ * @param[in]  conn_handle  The connection to perform the procedure on.
+ */
+static void service_changed_send_in_evt(uint16_t conn_handle)
+{
+    bool sc_pending_state = true;
+    bool sc_sent_state = false;
+    ret_code_t err_code = gscm_service_changed_ind_send(conn_handle);
+
+    switch (err_code)
+    {
+        case NRF_SUCCESS:
+        {
+            pm_evt_t event =
+            {
+                .evt_id      = PM_EVT_SERVICE_CHANGED_IND_SENT,
+                .conn_handle = conn_handle,
+            };
+
+            sc_sent_state = true;
+
+            evt_send(&event);
+            break;
+        }
+
+        case NRF_ERROR_BUSY:
+            // Do nothing.
+            break;
+
+        case NRF_ERROR_INVALID_STATE:
+        {
+            uint16_t cccd;
+            err_code = service_changed_cccd(conn_handle, &cccd);
+            if ((err_code == NRF_SUCCESS) && cccd)
+            {
+                // Possible ATT_MTU exchange ongoing.
+                // Do nothing, treat as busy.
+                break;
+            }
+            else
+            {
+                if (err_code != NRF_SUCCESS)
+                {
+                    NRF_LOG_DEBUG("Unexpected error when looking for service changed CCCD: %s",
+                                  nrf_strerror_get(err_code));
+                }
+                // CCCDs not enabled or an error happened. Drop indication.
+                // Fallthrough.
+            }
+        }
+            // Sometimes fallthrough.
+        case NRF_ERROR_NOT_SUPPORTED:
+            // Service changed not supported. Drop indication.
+            sc_pending_state = false;
+            gscm_db_change_notification_done(im_peer_id_get_by_conn_handle(conn_handle));
+            break;
+
+        case BLE_ERROR_GATTS_SYS_ATTR_MISSING:
+            local_db_apply_in_evt(conn_handle);
+            break;
+
+        case BLE_ERROR_INVALID_CONN_HANDLE:
+            // Do nothing.
+            break;
+
+        default:
+            NRF_LOG_ERROR("gscm_service_changed_ind_send() returned %s for conn_handle: %d",
+                          nrf_strerror_get(err_code),
+                          conn_handle);
+            send_unexpected_error(conn_handle, err_code);
+            break;
+    }
+
+    ble_conn_state_user_flag_set(conn_handle, m_flag_service_changed_pending, sc_pending_state);
+    ble_conn_state_user_flag_set(conn_handle, m_flag_service_changed_sent, sc_sent_state);
+}
+#endif
+
+static void apply_pending_handle(uint16_t conn_handle, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+    local_db_apply_in_evt(conn_handle);
+}
+
+
+static __INLINE void apply_pending_flags_check(void)
+{
+    UNUSED_RETURN_VALUE(ble_conn_state_for_each_set_user_flag(m_flag_local_db_apply_pending,
+                                                              apply_pending_handle,
+                                                              NULL));
+}
+
+
+static void db_update_pending_handle(uint16_t conn_handle, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+    if (nrf_mtx_trylock(&m_db_update_in_progress_mutex))
+    {
+        if (local_db_update_in_evt(conn_handle))
+        {
+            // Successfully started writing to flash.
+            return;
+        }
+        else
+        {
+            nrf_mtx_unlock(&m_db_update_in_progress_mutex);
+        }
+    }
+}
+
+
+#if PM_SERVICE_CHANGED_ENABLED
+static void sc_send_pending_handle(uint16_t conn_handle, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+    if (!ble_conn_state_user_flag_get(conn_handle, m_flag_service_changed_sent))
+    {
+        service_changed_send_in_evt(conn_handle);
+    }
+}
+
+
+static __INLINE void service_changed_pending_flags_check(void)
+{
+    UNUSED_RETURN_VALUE(ble_conn_state_for_each_set_user_flag(m_flag_service_changed_pending,
+                                                              sc_send_pending_handle,
+                                                              NULL));
+}
+
+
+static void service_changed_needed(uint16_t conn_handle)
+{
+    if (gscm_service_changed_ind_needed(conn_handle))
+    {
+        ble_conn_state_user_flag_set(conn_handle, m_flag_service_changed_pending, true);
+    }
+}
+#endif
+
+
+static void car_update_pending_handle(uint16_t conn_handle, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+
+    ble_uuid_t car_uuid;
+    memset(&car_uuid, 0, sizeof(ble_uuid_t));
+    car_uuid.uuid = BLE_UUID_GAP_CHARACTERISTIC_CAR;
+    car_uuid.type = BLE_UUID_TYPE_BLE;
+
+    ble_gattc_handle_range_t const car_handle_range = {1, 0xFFFF};
+
+    ret_code_t err_code = sd_ble_gattc_char_value_by_uuid_read(conn_handle, &car_uuid, &car_handle_range);
+    if (err_code == NRF_SUCCESS)
+    {
+        ble_conn_state_user_flag_set(conn_handle, m_flag_car_handle_queried, true);
+    }
+}
+
+
+static void car_update_needed(uint16_t conn_handle)
+{
+    pm_peer_data_t peer_data;
+    if (pds_peer_data_read(im_peer_id_get_by_conn_handle(conn_handle),
+                           PM_PEER_DATA_ID_CENTRAL_ADDR_RES,
+                           &peer_data,
+                           NULL) == NRF_ERROR_NOT_FOUND)
+    {
+        ble_conn_state_user_flag_set(conn_handle, m_flag_car_update_pending, true);
+    }
+}
+
+
+static __INLINE void update_pending_flags_check(void)
+{
+    uint32_t count = ble_conn_state_for_each_set_user_flag(m_flag_local_db_update_pending,
+                                                           db_update_pending_handle,
+                                                           NULL);
+    if (count == 0)
+    {
+        count = ble_conn_state_for_each_set_user_flag(m_flag_car_update_pending,
+                                                      car_update_pending_handle,
+                                                      NULL);
+        UNUSED_RETURN_VALUE(count);
+    }
+}
+
+
+/**@brief Callback function for events from the ID Manager module.
+ *        This function is registered in the ID Manager module.
+ *
+ * @param[in]  p_event  The event from the ID Manager module.
+ */
+void gcm_im_evt_handler(pm_evt_t * p_event)
+{
+    switch (p_event->evt_id)
+    {
+        case PM_EVT_BONDED_PEER_CONNECTED:
+            local_db_apply_in_evt(p_event->conn_handle);
+#if (PM_SERVICE_CHANGED_ENABLED == 1)
+            service_changed_needed(p_event->conn_handle);
+#endif
+            car_update_needed(p_event->conn_handle);
+            update_pending_flags_check();
+            break;
+        default:
+            break;
+    }
+}
+
+
+/**@brief Callback function for events from the Peer Database module.
+ *        This handler is extern in Peer Database.
+ *
+ * @param[in]  p_event  The event from the Security Dispatcher module.
+ */
+void gcm_pdb_evt_handler(pm_evt_t * p_event)
+{
+    if (   p_event->evt_id == PM_EVT_PEER_DATA_UPDATE_SUCCEEDED
+        && p_event->params.peer_data_update_succeeded.action == PM_PEER_DATA_OP_UPDATE)
+    {
+        switch (p_event->params.peer_data_update_succeeded.data_id)
+        {
+            case PM_PEER_DATA_ID_BONDING:
+            {
+                uint16_t conn_handle = im_conn_handle_get(p_event->peer_id);
+
+                if (conn_handle != BLE_CONN_HANDLE_INVALID)
+                {
+                    local_db_update(conn_handle, true);
+                    car_update_needed(conn_handle);
+                }
+                break;
+            }
+
+#if PM_SERVICE_CHANGED_ENABLED
+            case PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING:
+            {
+                ret_code_t           err_code;
+                pm_peer_data_flash_t peer_data;
+
+                err_code = pdb_peer_data_ptr_get(p_event->peer_id,
+                                                 PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING,
+                                                 &peer_data);
+
+                if (err_code == NRF_SUCCESS)
+                {
+                    if (*peer_data.p_service_changed_pending)
+                    {
+                        uint16_t conn_handle = im_conn_handle_get(p_event->peer_id);
+                        if (conn_handle != BLE_CONN_HANDLE_INVALID)
+                        {
+                            ble_conn_state_user_flag_set(conn_handle, m_flag_service_changed_pending, true);
+                            service_changed_pending_flags_check();
+                        }
+                    }
+                }
+                break;
+            }
+#endif
+
+            case PM_PEER_DATA_ID_GATT_LOCAL:
+                if (m_db_update_in_progress_mutex == NRF_MTX_LOCKED)
+                {
+                    nrf_mtx_unlock(&m_db_update_in_progress_mutex);
+                }
+
+                // Expecting a call to update_pending_flags_check() immediately.
+                break;
+
+            default:
+                /* No action */
+                break;
+        }
+    }
+
+    update_pending_flags_check();
+}
+
+
+ret_code_t gcm_init()
+{
+    NRF_PM_DEBUG_CHECK(!m_module_initialized);
+
+    internal_state_reset();
+
+    m_flag_local_db_update_pending = ble_conn_state_user_flag_acquire();
+    m_flag_local_db_apply_pending  = ble_conn_state_user_flag_acquire();
+    m_flag_service_changed_pending = ble_conn_state_user_flag_acquire();
+    m_flag_service_changed_sent    = ble_conn_state_user_flag_acquire();
+    m_flag_car_update_pending      = ble_conn_state_user_flag_acquire();
+    m_flag_car_handle_queried      = ble_conn_state_user_flag_acquire();
+    m_flag_car_value_queried       = ble_conn_state_user_flag_acquire();
+
+    if  ((m_flag_local_db_update_pending  == BLE_CONN_STATE_USER_FLAG_INVALID)
+      || (m_flag_local_db_apply_pending   == BLE_CONN_STATE_USER_FLAG_INVALID)
+      || (m_flag_service_changed_pending  == BLE_CONN_STATE_USER_FLAG_INVALID)
+      || (m_flag_service_changed_sent     == BLE_CONN_STATE_USER_FLAG_INVALID)
+      || (m_flag_car_update_pending       == BLE_CONN_STATE_USER_FLAG_INVALID)
+      || (m_flag_car_handle_queried       == BLE_CONN_STATE_USER_FLAG_INVALID)
+      || (m_flag_car_value_queried        == BLE_CONN_STATE_USER_FLAG_INVALID)
+      )
+    {
+        NRF_LOG_ERROR("Could not acquire conn_state user flags. Increase "\
+                      "BLE_CONN_STATE_USER_FLAG_COUNT in the ble_conn_state module.");
+        return NRF_ERROR_INTERNAL;
+    }
+
+    nrf_mtx_init(&m_db_update_in_progress_mutex);
+
+    m_module_initialized = true;
+
+    return NRF_SUCCESS;
+}
+
+
+void store_car_value(uint16_t conn_handle, bool car_value)
+{
+    // Use a uint32_t to enforce 4-byte alignment.
+    static const uint32_t car_value_true  = true;
+    static const uint32_t car_value_false = false;
+
+    pm_peer_data_const_t peer_data =
+    {
+        .data_id      = PM_PEER_DATA_ID_CENTRAL_ADDR_RES,
+        .length_words = 1,
+    };
+
+    ble_conn_state_user_flag_set(conn_handle, m_flag_car_update_pending, false);
+    peer_data.p_central_addr_res = car_value ? &car_value_true : &car_value_false;
+    ret_code_t err_code = pds_peer_data_store(im_peer_id_get_by_conn_handle(conn_handle), &peer_data, NULL);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_WARNING("CAR char value couldn't be stored (error: %s). Reattempt will happen on the next connection.", nrf_strerror_get(err_code));
+    }
+}
+
+
+/**@brief Callback function for BLE events from the SoftDevice.
+ *
+ * @param[in]  p_ble_evt  The BLE event from the SoftDevice.
+ */
+void gcm_ble_evt_handler(ble_evt_t const * p_ble_evt)
+{
+    uint16_t conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GATTS_EVT_SYS_ATTR_MISSING:
+            local_db_apply_in_evt(conn_handle);
+            break;
+
+#if PM_SERVICE_CHANGED_ENABLED
+        case BLE_GATTS_EVT_SC_CONFIRM:
+        {
+            pm_evt_t event =
+            {
+                .evt_id      = PM_EVT_SERVICE_CHANGED_IND_CONFIRMED,
+                .peer_id     = im_peer_id_get_by_conn_handle(conn_handle),
+                .conn_handle = conn_handle,
+            };
+
+            gscm_db_change_notification_done(event.peer_id);
+
+            ble_conn_state_user_flag_set(conn_handle, m_flag_service_changed_sent, false);
+            ble_conn_state_user_flag_set(conn_handle, m_flag_service_changed_pending, false);
+            evt_send(&event);
+            break;
+        }
+#endif
+
+        case BLE_GATTS_EVT_WRITE:
+            if (cccd_written(&p_ble_evt->evt.gatts_evt.params.write))
+            {
+                local_db_update(conn_handle, true);
+                update_pending_flags_check();
+            }
+            break;
+
+        case BLE_GATTC_EVT_CHAR_VAL_BY_UUID_READ_RSP:
+        {
+            bool handle_found = false;
+            conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;
+            const ble_gattc_evt_char_val_by_uuid_read_rsp_t * p_val = &p_ble_evt->evt.gattc_evt.params.char_val_by_uuid_read_rsp;
+
+            if (!ble_conn_state_user_flag_get(conn_handle, m_flag_car_handle_queried))
+            {
+                break;
+            }
+
+            ble_conn_state_user_flag_set(conn_handle, m_flag_car_handle_queried, false);
+
+            if (p_ble_evt->evt.gattc_evt.gatt_status == BLE_GATT_STATUS_ATTERR_ATTRIBUTE_NOT_FOUND)
+            {
+                // Store 0.
+            }
+            else if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS)
+            {
+                NRF_LOG_WARNING("Unexpected GATT status while getting CAR char value: 0x%x",
+                                p_ble_evt->evt.gattc_evt.gatt_status);
+                // Store 0.
+            }
+            else
+            {
+                if (p_val->count != 1)
+                {
+                    NRF_LOG_WARNING("Multiple (%d) CAR characteristics found, using the first.",
+                                    p_val->count);
+                }
+
+                if (p_val->value_len != 1)
+                {
+                    NRF_LOG_WARNING("Unexpected CAR characteristic value length (%d), store 0.",
+                                    p_val->value_len);
+                    // Store 0.
+                }
+                else
+                {
+                    ret_code_t err_code = sd_ble_gattc_read(conn_handle, *(uint16_t*)p_val->handle_value, 0);
+                    if (err_code == NRF_SUCCESS)
+                    {
+                        handle_found = true;
+                        ble_conn_state_user_flag_set(conn_handle, m_flag_car_value_queried, true);
+                    }
+                }
+            }
+
+            if (!handle_found)
+            {
+                store_car_value(conn_handle, false);
+            }
+            break;
+        }
+
+        case BLE_GATTC_EVT_READ_RSP:
+        {
+            bool car_value = false;
+            conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;
+            const ble_gattc_evt_read_rsp_t * p_val = &p_ble_evt->evt.gattc_evt.params.read_rsp;
+
+            if (!ble_conn_state_user_flag_get(conn_handle, m_flag_car_value_queried))
+            {
+                break;
+            }
+
+            ble_conn_state_user_flag_set(conn_handle, m_flag_car_value_queried, false);
+
+            if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS)
+            {
+                NRF_LOG_WARNING("Unexpected GATT status while getting CAR char value: 0x%x",
+                                p_ble_evt->evt.gattc_evt.gatt_status);
+                // Store 0.
+            }
+            else
+            {
+                if (p_val->len != 1)
+                {
+                    NRF_LOG_WARNING("Unexpected CAR characteristic value length (%d), store 0.",
+                                    p_val->len);
+                    // Store 0.
+                }
+                else
+                {
+                    car_value = *p_val->data;
+                }
+            }
+
+            store_car_value(conn_handle, car_value);
+        }
+    }
+
+    apply_pending_flags_check();
+#if PM_SERVICE_CHANGED_ENABLED
+    service_changed_pending_flags_check();
+#endif
+}
+
+
+ret_code_t gcm_local_db_cache_update(uint16_t conn_handle)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    local_db_update(conn_handle, true);
+    update_pending_flags_check();
+
+    return NRF_SUCCESS;
+}
+
+
+#if PM_SERVICE_CHANGED_ENABLED
+void gcm_local_database_has_changed(void)
+{
+    gscm_local_database_has_changed();
+
+    ble_conn_state_conn_handle_list_t conn_handles = ble_conn_state_conn_handles();
+
+    for (uint16_t i = 0; i < conn_handles.len; i++)
+    {
+        if (im_peer_id_get_by_conn_handle(conn_handles.conn_handles[i]) == PM_PEER_ID_INVALID)
+        {
+            ble_conn_state_user_flag_set(conn_handles.conn_handles[i], m_flag_service_changed_pending, true);
+        }
+    }
+
+    service_changed_pending_flags_check();
+}
+#endif
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/gatt_cache_manager.h

@@ -0,0 +1,113 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef GATT_CACHE_MANAGER_H__
+#define GATT_CACHE_MANAGER_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup gatt_cache_manager GATT Cache Manager
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for managing persistent storing of GATT
+ *        attributes.
+ */
+
+
+/**@brief Function for initializing the GATT Cache Manager module.
+ *
+ * @retval NRF_SUCCESS         Initialization was successful.
+ * @retval NRF_ERROR_INTERNAL  If an internal error occurred.
+ */
+ret_code_t gcm_init(void);
+
+
+/**@brief Function for dispatching SoftDevice events to the GATT Cache Manager module.
+ *
+ * @param[in]  p_ble_evt  The SoftDevice event.
+ */
+void gcm_ble_evt_handler(ble_evt_t const * p_ble_evt);
+
+
+/**@brief Function for triggering local GATT database data to be stored persistently.
+ *
+ * @details Values are retrieved from SoftDevice and written to persistent storage.
+ *
+ * @note This operation happens asynchronously, so any errors are reported as events.
+ *
+ * @note This function is only needed when you want to override the regular functionality of the
+ *       module, e.g. to immediately store to flash instead of waiting for the native logic to
+ *       perform the update.
+ *
+ * @param[in]  conn_handle  Connection handle to perform update on.
+ *
+ * @retval NRF_SUCCESS                    Store operation started.
+ */
+ret_code_t gcm_local_db_cache_update(uint16_t conn_handle);
+
+
+/**@brief Function for manually informing that the local database has changed.
+ *
+ * @details This causes a service changed notification to be sent to all bonded peers that
+ *          subscribe to it.
+ */
+void gcm_local_database_has_changed(void);
+
+/** @}
+  * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* GATT_CACHE_MANAGER_H__ */

components/ble/peer_manager/gatts_cache_manager.c

@@ -0,0 +1,457 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "gatts_cache_manager.h"
+
+#include <string.h>
+#include "ble_gap.h"
+#include "ble_err.h"
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+#include "peer_database.h"
+#include "peer_data_storage.h"
+#include "id_manager.h"
+
+#define NRF_LOG_MODULE_NAME peer_manager_gscm
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+#include "nrf_strerror.h"
+
+
+#if !defined(PM_SERVICE_CHANGED_ENABLED) || (PM_SERVICE_CHANGED_ENABLED == 1)
+// The number of registered event handlers.
+#define GSCM_EVENT_HANDLERS_CNT      (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
+
+// GATTS Cache Manager event handler in Peer Manager.
+extern void pm_gscm_evt_handler(pm_evt_t * p_gcm_evt);
+
+// GATTS Cache Manager events' handlers.
+// The number of elements in this array is GSCM_EVENT_HANDLERS_CNT.
+static pm_evt_handler_internal_t m_evt_handlers[] =
+{
+    pm_gscm_evt_handler
+};
+#endif
+
+// Syntactic sugar, two spoons.
+#define SYS_ATTR_SYS                    (BLE_GATTS_SYS_ATTR_FLAG_SYS_SRVCS)
+#define SYS_ATTR_USR                    (BLE_GATTS_SYS_ATTR_FLAG_USR_SRVCS)
+#define SYS_ATTR_BOTH                   (SYS_ATTR_SYS | SYS_ATTR_USR)
+
+static bool               m_module_initialized;
+static pm_peer_id_t       m_current_sc_store_peer_id;
+
+
+/**@brief Function for resetting the module variable(s) of the GSCM module.
+ */
+static void internal_state_reset()
+{
+    m_module_initialized       = false;
+    m_current_sc_store_peer_id = PM_PEER_ID_INVALID;
+
+    // If PM_SERVICE_CHANGED_ENABLED is 0, this variable is unused.
+    UNUSED_VARIABLE(m_current_sc_store_peer_id);
+}
+
+
+#if !defined(PM_SERVICE_CHANGED_ENABLED) || (PM_SERVICE_CHANGED_ENABLED == 1)
+static void evt_send(pm_evt_t * p_gscm_evt)
+{
+    p_gscm_evt->conn_handle = im_conn_handle_get(p_gscm_evt->peer_id);
+
+    for (uint32_t i = 0; i < GSCM_EVENT_HANDLERS_CNT; i++)
+    {
+        m_evt_handlers[i](p_gscm_evt);
+    }
+}
+
+
+//lint -save -e550
+/**@brief Function for storing service_changed_pending = true to flash for all peers, in sequence.
+ *
+ * This function aborts if it gets @ref NRF_ERROR_BUSY when trying to store. A subsequent call will
+ * continue where the last call was aborted.
+ */
+static void service_changed_pending_set(void)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    ret_code_t err_code;
+    // Use a uint32_t to enforce 4-byte alignment.
+    static const uint32_t service_changed_pending = true;
+
+    //lint -save -e65 -e64
+    pm_peer_data_const_t peer_data =
+    {
+        .data_id                   = PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING,
+        .length_words              = PM_SC_STATE_N_WORDS(),
+        .p_service_changed_pending = (bool*)&service_changed_pending,
+    };
+    //lint -restore
+
+    while (m_current_sc_store_peer_id != PM_PEER_ID_INVALID)
+    {
+        err_code = pds_peer_data_store(m_current_sc_store_peer_id, &peer_data, NULL);
+        if (err_code != NRF_SUCCESS)
+        {
+            pm_evt_t evt = {.peer_id = m_current_sc_store_peer_id};
+            if (err_code == NRF_ERROR_BUSY)
+            {
+                // Do nothing.
+            }
+            else if (err_code == NRF_ERROR_STORAGE_FULL)
+            {
+                evt.evt_id = PM_EVT_STORAGE_FULL;
+                evt_send(&evt);
+            }
+            else
+            {
+                NRF_LOG_ERROR("pds_peer_data_store() returned %s while storing service changed"\
+                              "state for peer id %d.",
+                              nrf_strerror_get(err_code),
+                              m_current_sc_store_peer_id);
+                evt.evt_id = PM_EVT_ERROR_UNEXPECTED;
+                evt.params.error_unexpected.error = err_code;
+                evt_send(&evt);
+            }
+            break;
+        }
+
+        m_current_sc_store_peer_id = pds_next_peer_id_get(m_current_sc_store_peer_id);
+    }
+}
+//lint -restore
+
+
+
+/**@brief Event handler for events from the Peer Database module.
+ *        This function is extern in Peer Database.
+ *
+ * @param[in]  p_event The event that has happened with peer id and flags.
+ */
+void gscm_pdb_evt_handler(pm_evt_t * p_event)
+{
+    if (m_current_sc_store_peer_id != PM_PEER_ID_INVALID)
+    {
+        service_changed_pending_set();
+    }
+}
+#endif
+
+
+ret_code_t gscm_init()
+{
+    NRF_PM_DEBUG_CHECK(!m_module_initialized);
+
+    internal_state_reset();
+    m_module_initialized = true;
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t gscm_local_db_cache_update(uint16_t conn_handle)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    pm_peer_id_t peer_id = im_peer_id_get_by_conn_handle(conn_handle);
+    ret_code_t   err_code;
+
+    if (peer_id == PM_PEER_ID_INVALID)
+    {
+        return BLE_ERROR_INVALID_CONN_HANDLE;
+    }
+    else
+    {
+        pm_peer_data_t peer_data;
+        uint16_t       n_bufs = 1;
+        bool           retry_with_bigger_buffer = false;
+
+        do
+        {
+            retry_with_bigger_buffer = false;
+
+            err_code = pdb_write_buf_get(peer_id, PM_PEER_DATA_ID_GATT_LOCAL, n_bufs++, &peer_data);
+            if (err_code == NRF_SUCCESS)
+            {
+                pm_peer_data_local_gatt_db_t * p_local_gatt_db = peer_data.p_local_gatt_db;
+
+                p_local_gatt_db->flags = SYS_ATTR_BOTH;
+
+                err_code = sd_ble_gatts_sys_attr_get(conn_handle, &p_local_gatt_db->data[0], &p_local_gatt_db->len, p_local_gatt_db->flags);
+
+                if (err_code == NRF_SUCCESS)
+                {
+                    pm_peer_data_flash_t curr_peer_data;
+
+                    err_code = pdb_peer_data_ptr_get(peer_id,
+                                                PM_PEER_DATA_ID_GATT_LOCAL,
+                                                &curr_peer_data);
+
+                    if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_NOT_FOUND))
+                    {
+                        NRF_LOG_ERROR("pdb_peer_data_ptr_get() returned %s for conn_handle: %d",
+                                        nrf_strerror_get(err_code),
+                                        conn_handle);
+                        return NRF_ERROR_INTERNAL;
+                    }
+
+                    if((err_code == NRF_ERROR_NOT_FOUND)
+                        || (p_local_gatt_db->len != curr_peer_data.p_local_gatt_db->len)
+                        || (memcmp(p_local_gatt_db->data, curr_peer_data.p_local_gatt_db->data,
+                                    p_local_gatt_db->len) != 0))
+                    {
+                        err_code = pdb_write_buf_store(peer_id, PM_PEER_DATA_ID_GATT_LOCAL, peer_id);
+                    }
+                    else
+                    {
+                        NRF_LOG_DEBUG("Local db is already up to date, skipping write.");
+                        ret_code_t err_code_release = pdb_write_buf_release(peer_id, PM_PEER_DATA_ID_GATT_LOCAL);
+                        if (err_code_release == NRF_SUCCESS)
+                        {
+                            err_code = NRF_ERROR_INVALID_DATA;
+                        }
+                        else
+                        {
+                            NRF_LOG_ERROR("Did another thread manipulate PM_PEER_DATA_ID_GATT_LOCAL for "\
+                                          "peer_id %d at the same time? pdb_write_buf_release() returned %s.",
+                                          peer_id,
+                                          nrf_strerror_get(err_code_release));
+                            err_code = NRF_ERROR_INTERNAL;
+                        }
+                    }
+                }
+                else
+                {
+                    if (err_code == NRF_ERROR_DATA_SIZE)
+                    {
+                        // The sys attributes are bigger than the requested write buffer.
+                        retry_with_bigger_buffer = true;
+                    }
+                    else if (err_code == NRF_ERROR_NOT_FOUND)
+                    {
+                        // There are no sys attributes in the GATT db, so nothing needs to be stored.
+                        err_code = NRF_SUCCESS;
+                    }
+
+                    ret_code_t err_code_release = pdb_write_buf_release(peer_id, PM_PEER_DATA_ID_GATT_LOCAL);
+                    if (err_code_release != NRF_SUCCESS)
+                    {
+                        NRF_LOG_ERROR("Did another thread manipulate PM_PEER_DATA_ID_GATT_LOCAL for "\
+                                      "peer_id %d at the same time? pdb_write_buf_release() returned %s.",
+                                      peer_id,
+                                      nrf_strerror_get(err_code_release));
+                        err_code = NRF_ERROR_INTERNAL;
+                    }
+                }
+            }
+            else if (err_code == NRF_ERROR_INVALID_PARAM)
+            {
+                // The sys attributes are bigger than the entire write buffer.
+                err_code = NRF_ERROR_DATA_SIZE;
+            }
+        } while (retry_with_bigger_buffer);
+    }
+
+    return err_code;
+}
+
+
+ret_code_t gscm_local_db_cache_apply(uint16_t conn_handle)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    pm_peer_id_t         peer_id = im_peer_id_get_by_conn_handle(conn_handle);
+    ret_code_t           err_code;
+    pm_peer_data_flash_t peer_data;
+    uint8_t      const * p_sys_attr_data = NULL;
+    uint16_t             sys_attr_len    = 0;
+    uint32_t             sys_attr_flags  = (SYS_ATTR_BOTH);
+    bool                 all_attributes_applied = true;
+
+    if (peer_id != PM_PEER_ID_INVALID)
+    {
+        err_code = pdb_peer_data_ptr_get(peer_id, PM_PEER_DATA_ID_GATT_LOCAL, &peer_data);
+        if (err_code == NRF_SUCCESS)
+        {
+            pm_peer_data_local_gatt_db_t const * p_local_gatt_db;
+
+            p_local_gatt_db = peer_data.p_local_gatt_db;
+            p_sys_attr_data = p_local_gatt_db->data;
+            sys_attr_len    = p_local_gatt_db->len;
+            sys_attr_flags  = p_local_gatt_db->flags;
+        }
+    }
+
+    do
+    {
+        err_code = sd_ble_gatts_sys_attr_set(conn_handle, p_sys_attr_data, sys_attr_len, sys_attr_flags);
+
+        if (err_code == NRF_ERROR_NO_MEM)
+        {
+            err_code = NRF_ERROR_BUSY;
+        }
+        else if (err_code == NRF_ERROR_INVALID_STATE)
+        {
+            err_code = NRF_SUCCESS;
+        }
+        else if (err_code == NRF_ERROR_INVALID_DATA)
+        {
+            all_attributes_applied = false;
+
+            if (sys_attr_flags & SYS_ATTR_USR)
+            {
+                // Try setting only system attributes.
+                sys_attr_flags = SYS_ATTR_SYS;
+            }
+            else if (p_sys_attr_data || sys_attr_len)
+            {
+                // Try reporting that none exist.
+                p_sys_attr_data = NULL;
+                sys_attr_len    = 0;
+                sys_attr_flags  = SYS_ATTR_BOTH;
+            }
+            else
+            {
+                NRF_LOG_ERROR("sd_ble_gatts_sys_attr_set() returned NRF_ERROR_INVALID_DATA for NULL "\
+                              "pointer which should never happen. conn_handle: %d",
+                              conn_handle);
+                err_code = NRF_ERROR_INTERNAL;
+            }
+        }
+    } while (err_code == NRF_ERROR_INVALID_DATA);
+
+    if (!all_attributes_applied)
+    {
+        err_code = NRF_ERROR_INVALID_DATA;
+    }
+
+    return err_code;
+}
+
+#if !defined(PM_SERVICE_CHANGED_ENABLED) || (PM_SERVICE_CHANGED_ENABLED == 1)
+void gscm_local_database_has_changed(void)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    m_current_sc_store_peer_id = pds_next_peer_id_get(PM_PEER_ID_INVALID);
+    service_changed_pending_set();
+}
+
+
+bool gscm_service_changed_ind_needed(uint16_t conn_handle)
+{
+    ret_code_t           err_code;
+    bool                 service_changed_state;
+    pm_peer_data_flash_t peer_data;
+
+    peer_data.p_service_changed_pending = &service_changed_state;
+    pm_peer_id_t peer_id = im_peer_id_get_by_conn_handle(conn_handle);
+
+    err_code = pdb_peer_data_ptr_get(peer_id, PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING, &peer_data);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        return false;
+    }
+
+    return *peer_data.p_service_changed_pending;
+}
+
+
+ret_code_t gscm_service_changed_ind_send(uint16_t conn_handle)
+{
+    static uint16_t start_handle;
+    const  uint16_t end_handle = 0xFFFF;
+    ret_code_t err_code;
+
+    err_code = sd_ble_gatts_initial_user_handle_get(&start_handle);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("sd_ble_gatts_initial_user_handle_get() returned %s which should not happen.",
+                      nrf_strerror_get(err_code));
+        return NRF_ERROR_INTERNAL;
+    }
+
+    do
+    {
+        err_code = sd_ble_gatts_service_changed(conn_handle, start_handle, end_handle);
+        if (err_code == BLE_ERROR_INVALID_ATTR_HANDLE)
+        {
+            start_handle += 1;
+        }
+    } while (err_code == BLE_ERROR_INVALID_ATTR_HANDLE);
+
+    return err_code;
+}
+
+
+void gscm_db_change_notification_done(pm_peer_id_t peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    // Use a uint32_t to enforce 4-byte alignment.
+    static const uint32_t service_changed_pending = false;
+
+    //lint -save -e65 -e64
+    pm_peer_data_const_t peer_data =
+    {
+        .data_id                   = PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING,
+        .length_words              = PM_SC_STATE_N_WORDS(),
+        .p_service_changed_pending = (bool*)&service_changed_pending,
+    };
+    //lint -restore
+
+    // Don't need to check return code, because all error conditions can be ignored.
+    //lint -save -e550
+    (void) pds_peer_data_store(peer_id, &peer_data, NULL);
+    //lint -restore
+}
+#endif
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/gatts_cache_manager.h

@@ -0,0 +1,165 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef GATTS_CACHE_MANAGER_H__
+#define GATTS_CACHE_MANAGER_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup gatts_cache_manager GATT Server Cache Manager
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for managing persistent storing of GATT
+ *        attributes pertaining to the GATT server role of the local device.
+ */
+
+
+/**@brief Function for initializing the GATT Server Cache Manager module.
+ *
+ * @retval NRF_SUCCESS         Initialization was successful.
+ * @retval NRF_ERROR_INTERNAL  If an internal error occurred.
+ */
+ret_code_t gscm_init(void);
+
+
+/**@brief Function for triggering local GATT database data to be stored persistently. Values are
+ *        retrieved from the SoftDevice and written to persistent storage.
+ *
+ * @param[in]  conn_handle  Connection handle to perform update on.
+ *
+ * @retval NRF_SUCCESS                    Store operation started.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  conn_handle does not refer to an active connection with a
+ *                                        bonded peer.
+ * @retval NRF_ERROR_INVALID_DATA         Refused to update because the GATT database is already up
+ *                                        to date.
+ * @retval NRF_ERROR_BUSY                 Unable to perform operation at this time. Reattempt later.
+ * @retval NRF_ERROR_DATA_SIZE            Write buffer not large enough. Call will never work with
+ *                                        this GATT database.
+ * @retval NRF_ERROR_STORAGE_FULL         No room in persistent_storage. Free up space; the
+ *                                        operation will be automatically reattempted after the
+ *                                        next FDS garbage collection procedure.
+ */
+ret_code_t gscm_local_db_cache_update(uint16_t conn_handle);
+
+
+/**@brief Function for applying stored local GATT database data to the SoftDevice. Values are
+ *        retrieved from persistent storage and given to the SoftDevice.
+ *
+ * @param[in]  conn_handle  Connection handle to apply values to.
+ *
+ * @retval NRF_SUCCESS                    Store operation started.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  conn_handle does not refer to an active connection with a
+ *                                        bonded peer.
+ * @retval NRF_ERROR_INVALID_DATA         The stored data was rejected by the SoftDevice, which
+ *                                        probably means that the local database has changed. The
+ *                                        system part of the sys_attributes was attempted applied,
+ *                                        so service changed indications can be sent to subscribers.
+ * @retval NRF_ERROR_BUSY                 Unable to perform operation at this time. Reattempt later.
+ * @retval NRF_ERROR_INTERNAL             An unexpected error happened.
+ */
+ret_code_t gscm_local_db_cache_apply(uint16_t conn_handle);
+
+
+/**@brief Function for storing the fact that the local database has changed, for all currently
+ *        bonded peers.
+ *
+ * @note This will cause a later call to @ref gscm_service_changed_ind_needed to return true for
+ *       a connection with a currently bonded peer.
+ */
+void gscm_local_database_has_changed(void);
+
+
+/**@brief Function for checking if a service changed indication should be sent.
+ *
+ * @param[in]  conn_handle  The connection to check.
+ *
+ * @return true if a service changed indication should be sent, false if not.
+ */
+bool gscm_service_changed_ind_needed(uint16_t conn_handle);
+
+
+/**@brief Function for sending a service changed indication to a connected peer.
+ *
+ * @param[in]  conn_handle  The connection to send the indication on.
+ *
+ * @retval NRF_SUCCESS                       Indication sent or not needed.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE     conn_handle does not refer to an active connection.
+ * @retval NRF_ERROR_BUSY                    Unable to send indication at this time. Reattempt later.
+ * @retval BLE_ERROR_GATTS_SYS_ATTR_MISSING  Information missing. Apply local cache, then reattempt.
+ * @retval NRF_ERROR_INVALID_PARAM           From @ref sd_ble_gatts_service_changed. Unexpected.
+ * @retval NRF_ERROR_NOT_SUPPORTED           Service changed characteristic is not present.
+ * @retval NRF_ERROR_INVALID_STATE           Service changed cannot be indicated to this peer
+ *                                           because the peer has not subscribed to it.
+ * @retval NRF_ERROR_INTERNAL                An unexpected error happened.
+ */
+ret_code_t gscm_service_changed_ind_send(uint16_t conn_handle);
+
+
+/**@brief Function for specifying that a peer has been made aware of the latest local database
+ *        change.
+ *
+ * @note After calling this, a later call to @ref gscm_service_changed_ind_needed will to return
+ *       false for this peer unless @ref gscm_local_database_has_changed is called again.
+ *
+ * @param[in]  peer_id  The connection to send the indication on.
+ */
+void gscm_db_change_notification_done(pm_peer_id_t peer_id);
+
+/** @}
+ * @endcond
+*/
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* GATTS_CACHE_MANAGER_H__ */

components/ble/peer_manager/id_manager.c

@@ -0,0 +1,758 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "id_manager.h"
+
+#include <string.h>
+#include "ble.h"
+#include "ble_gap.h"
+#include "ble_err.h"
+#include "peer_manager_types.h"
+#include "peer_database.h"
+#include "peer_data_storage.h"
+#include "nrf_soc.h"
+#include "ble_conn_state.h"
+
+#define NRF_LOG_MODULE_NAME peer_manager_im
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+
+#define IM_MAX_CONN_HANDLES             (20)
+#define IM_NO_INVALID_CONN_HANDLES      (0xFF)
+#define IM_ADDR_CLEARTEXT_LENGTH        (3)
+#define IM_ADDR_CIPHERTEXT_LENGTH       (3)
+
+// The number of registered event handlers.
+#define IM_EVENT_HANDLERS_CNT           (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
+
+
+// Identity Manager event handlers in Peer Manager and GATT Cache Manager.
+extern void pm_im_evt_handler(pm_evt_t * p_event);
+extern void gcm_im_evt_handler(pm_evt_t * p_event);
+
+// Identity Manager events' handlers.
+// The number of elements in this array is IM_EVENT_HANDLERS_CNT.
+static pm_evt_handler_internal_t const m_evt_handlers[] =
+{
+    pm_im_evt_handler,
+    gcm_im_evt_handler
+};
+
+typedef struct
+{
+    pm_peer_id_t   peer_id;
+    ble_gap_addr_t peer_address;
+} im_connection_t;
+
+static im_connection_t m_connections[IM_MAX_CONN_HANDLES];
+static uint8_t         m_wlisted_peer_cnt;
+static pm_peer_id_t    m_wlisted_peers[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
+
+
+/**@brief Function for sending an event to all registered event handlers.
+ *
+ * @param[in] p_event The event to distribute.
+ */
+static void evt_send(pm_evt_t * p_event)
+{
+    for (uint32_t i = 0; i < IM_EVENT_HANDLERS_CNT; i++)
+    {
+        m_evt_handlers[i](p_event);
+    }
+}
+
+
+/**@brief Function checking the validity of an IRK
+ *
+ * @detail An all-zero IRK is not valid. This function will check if a given IRK is valid.
+ *
+ * @param[in] p_irk The IRK for which the validity is going to be checked.
+ *
+ * @retval true  The IRK is valid.
+ * @retval false The IRK is invalid.
+ */
+bool is_valid_irk(ble_gap_irk_t const * p_irk)
+{
+    NRF_PM_DEBUG_CHECK(p_irk != NULL);
+
+    for (uint32_t i = 0; i < BLE_GAP_SEC_KEY_LEN; i++)
+    {
+        if (p_irk->irk[i] != 0)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+/**@brief Function for comparing two addresses to determine if they are identical
+ *
+ * @note The address type need to be identical, as well as every bit in the address itself.
+ *
+ * @param[in] p_addr1 The first address to be compared.
+ * @param[in] p_addr2 The second address to be compared.
+ *
+ * @retval true  The addresses are identical.
+ * @retval false The addresses are not identical.
+ */
+bool addr_compare(ble_gap_addr_t const * p_addr1, ble_gap_addr_t const * p_addr2)
+{
+    // @note emdi: use NRF_PM_DEBUG_CHECK ?
+    if ((p_addr1 == NULL) || (p_addr2 == NULL))
+    {
+        return false;
+    }
+
+    // Check that the addr type is identical, return false if it is not
+    if (p_addr1->addr_type != p_addr2->addr_type)
+    {
+        return false;
+    }
+
+    // Check if the addr bytes are is identical
+    return (memcmp(p_addr1->addr, p_addr2->addr, BLE_GAP_ADDR_LEN) == 0);
+}
+
+
+void im_ble_evt_handler(ble_evt_t const * ble_evt)
+{
+    ble_gap_evt_t gap_evt;
+    pm_peer_id_t  bonded_matching_peer_id;
+
+    if (ble_evt->header.evt_id != BLE_GAP_EVT_CONNECTED)
+    {
+        // Nothing to do.
+        return;
+    }
+
+    gap_evt                 = ble_evt->evt.gap_evt;
+    bonded_matching_peer_id = PM_PEER_ID_INVALID;
+
+    if (   gap_evt.params.connected.peer_addr.addr_type
+        != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE)
+    {
+        /* Search the database for bonding data matching the one that triggered the event.
+         * Public and static addresses can be matched on address alone, while resolvable
+         * random addresses can be resolved agains known IRKs. Non-resolvable random addresses
+         * are never matching because they are not longterm form of identification.
+         */
+
+        pm_peer_id_t         peer_id;
+        pm_peer_data_flash_t peer_data;
+
+        pds_peer_data_iterate_prepare();
+
+        switch (gap_evt.params.connected.peer_addr.addr_type)
+        {
+            case BLE_GAP_ADDR_TYPE_PUBLIC:
+            case BLE_GAP_ADDR_TYPE_RANDOM_STATIC:
+            {
+                while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data))
+                {
+                    if (addr_compare(&gap_evt.params.connected.peer_addr,
+                                     &peer_data.p_bonding_data->peer_ble_id.id_addr_info))
+                    {
+                        bonded_matching_peer_id = peer_id;
+                        break;
+                    }
+                }
+            }
+            break;
+
+            case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE:
+            {
+                while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data))
+                {
+                    if (im_address_resolve(&gap_evt.params.connected.peer_addr,
+                                           &peer_data.p_bonding_data->peer_ble_id.id_info))
+                    {
+                        bonded_matching_peer_id = peer_id;
+                        break;
+                    }
+                }
+            }
+            break;
+
+            default:
+                NRF_PM_DEBUG_CHECK(false);
+                break;
+        }
+    }
+
+    m_connections[gap_evt.conn_handle].peer_id      = bonded_matching_peer_id;
+    m_connections[gap_evt.conn_handle].peer_address = gap_evt.params.connected.peer_addr;
+
+    if (bonded_matching_peer_id != PM_PEER_ID_INVALID)
+    {
+        // Send a bonded peer event
+        pm_evt_t im_evt;
+        im_evt.conn_handle = gap_evt.conn_handle;
+        im_evt.peer_id     = bonded_matching_peer_id;
+        im_evt.evt_id      = PM_EVT_BONDED_PEER_CONNECTED;
+        evt_send(&im_evt);
+    }
+}
+
+
+/**@brief Function to compare two sets of bonding data to check if they belong to the same device.
+ * @note  Invalid irks will never match even though they are identical.
+ *
+ * @param[in]  p_bonding_data1 First bonding data for comparison
+ * @param[in]  p_bonding_data2 Second bonding data for comparison
+ *
+ * @return     True if the input matches, false if it does not.
+ */
+bool im_is_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data1,
+                                  pm_peer_data_bonding_t const * p_bonding_data2)
+{
+    NRF_PM_DEBUG_CHECK(p_bonding_data1 != NULL);
+    NRF_PM_DEBUG_CHECK(p_bonding_data2 != NULL);
+
+    ble_gap_addr_t const * p_addr1 = &p_bonding_data1->peer_ble_id.id_addr_info;
+    ble_gap_addr_t const * p_addr2 = &p_bonding_data2->peer_ble_id.id_addr_info;
+
+    bool duplicate_irk = ((memcmp(p_bonding_data1->peer_ble_id.id_info.irk,
+                                  p_bonding_data2->peer_ble_id.id_info.irk,
+                                  BLE_GAP_SEC_KEY_LEN) == 0)
+                          && is_valid_irk(&p_bonding_data1->peer_ble_id.id_info)
+                          && is_valid_irk(&p_bonding_data2->peer_ble_id.id_info));
+
+    bool duplicate_addr = addr_compare(p_addr1, p_addr2);
+
+    bool id_addrs =   ((p_addr1->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
+                    && (p_addr1->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE)
+                    && (p_addr2->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
+                    && (p_addr2->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE));
+
+    return (duplicate_addr && id_addrs) || (duplicate_irk && !id_addrs);
+}
+
+
+pm_peer_id_t im_find_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data,
+                                            pm_peer_id_t                   peer_id_skip)
+{
+    pm_peer_id_t peer_id;
+    pm_peer_data_flash_t peer_data_duplicate;
+
+    NRF_PM_DEBUG_CHECK(p_bonding_data != NULL);
+
+    pds_peer_data_iterate_prepare();
+
+    while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data_duplicate))
+    {
+        if (  (peer_id != peer_id_skip)
+            && im_is_duplicate_bonding_data(p_bonding_data,
+                                            peer_data_duplicate.p_bonding_data))
+        {
+            return peer_id;
+        }
+    }
+    return PM_PEER_ID_INVALID;
+}
+
+
+pm_peer_id_t im_peer_id_get_by_conn_handle(uint16_t conn_handle)
+{
+    if ((conn_handle >= IM_MAX_CONN_HANDLES) || !ble_conn_state_valid(conn_handle))
+    {
+        return PM_PEER_ID_INVALID;
+    }
+
+    return m_connections[conn_handle].peer_id;
+}
+
+
+ret_code_t im_ble_addr_get(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr)
+{
+    NRF_PM_DEBUG_CHECK(p_ble_addr != NULL);
+
+    if ((conn_handle >= IM_MAX_CONN_HANDLES) || !ble_conn_state_valid(conn_handle))
+    {
+        return BLE_ERROR_INVALID_CONN_HANDLE;
+    }
+
+    *p_ble_addr = m_connections[conn_handle].peer_address;
+    return NRF_SUCCESS;
+}
+
+
+bool im_master_ids_compare(ble_gap_master_id_t const * p_master_id1,
+                           ble_gap_master_id_t const * p_master_id2)
+{
+    NRF_PM_DEBUG_CHECK(p_master_id1 != NULL);
+    NRF_PM_DEBUG_CHECK(p_master_id2 != NULL);
+
+    if (!im_master_id_is_valid(p_master_id1))
+    {
+        return false;
+    }
+
+    if (p_master_id1->ediv != p_master_id2->ediv)
+    {
+        return false;
+    }
+
+    return (memcmp(p_master_id1->rand, p_master_id2->rand, BLE_GAP_SEC_RAND_LEN) == 0);
+}
+
+
+pm_peer_id_t im_peer_id_get_by_master_id(ble_gap_master_id_t const * p_master_id)
+{
+    pm_peer_id_t         peer_id;
+    pm_peer_data_flash_t peer_data;
+
+    NRF_PM_DEBUG_CHECK(p_master_id != NULL);
+
+    pds_peer_data_iterate_prepare();
+
+    // For each stored peer, check if the master_id matches p_master_id
+    while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data))
+    {
+        if (im_master_ids_compare(p_master_id, &peer_data.p_bonding_data->own_ltk.master_id) ||
+            im_master_ids_compare(p_master_id, &peer_data.p_bonding_data->peer_ltk.master_id))
+        {
+            // If a matching master ID is found then return the peer ID.
+            return peer_id;
+        }
+    }
+
+    // If no matching master ID is found return PM_PEER_ID_INVALID.
+    return PM_PEER_ID_INVALID;
+}
+
+
+uint16_t im_conn_handle_get(pm_peer_id_t peer_id)
+{
+    if (peer_id == PM_PEER_ID_INVALID)
+    {
+        return BLE_CONN_HANDLE_INVALID;
+    }
+
+    for (uint16_t conn_handle = 0; conn_handle < IM_MAX_CONN_HANDLES; conn_handle++)
+    {
+        if ((m_connections[conn_handle].peer_id == peer_id) && ble_conn_state_valid(conn_handle))
+        {
+            return conn_handle;
+        }
+    }
+    return BLE_CONN_HANDLE_INVALID;
+}
+
+
+bool im_master_id_is_valid(ble_gap_master_id_t const * p_master_id)
+{
+    if (p_master_id->ediv != 0)
+    {
+        return true;
+    }
+
+    for (uint32_t i = 0; i < BLE_GAP_SEC_RAND_LEN; i++)
+    {
+        if (p_master_id->rand[i] != 0)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+void im_new_peer_id(uint16_t conn_handle, pm_peer_id_t peer_id)
+{
+    if (conn_handle < IM_MAX_CONN_HANDLES)
+    {
+        m_connections[conn_handle].peer_id = peer_id;
+    }
+}
+
+
+ret_code_t im_peer_free(pm_peer_id_t peer_id)
+{
+    uint16_t   conn_handle;
+    ret_code_t ret;
+
+    conn_handle = im_conn_handle_get(peer_id);
+    ret         = pdb_peer_free(peer_id);
+
+    if (ret == NRF_SUCCESS && (conn_handle < IM_MAX_CONN_HANDLES))
+    {
+        m_connections[conn_handle].peer_id = PM_PEER_ID_INVALID;
+    }
+    return ret;
+}
+
+
+/**@brief Given a list of peers, loads their GAP address and IRK into the provided buffers.
+ */
+static ret_code_t peers_id_keys_get(pm_peer_id_t   const * p_peers,
+                                    uint32_t               peer_cnt,
+                                    ble_gap_addr_t       * p_gap_addrs,
+                                    uint32_t             * p_addr_cnt,
+                                    ble_gap_irk_t        * p_gap_irks,
+                                    uint32_t             * p_irk_cnt)
+{
+    ret_code_t ret;
+
+    pm_peer_data_bonding_t bond_data;
+    pm_peer_data_t         peer_data;
+
+    uint32_t const buf_size = sizeof(bond_data);
+
+    bool copy_addrs = false;
+    bool copy_irks  = false;
+
+    NRF_PM_DEBUG_CHECK(p_peers != NULL);
+
+    // One of these two has to be provided.
+    NRF_PM_DEBUG_CHECK((p_gap_addrs != NULL) || (p_gap_irks != NULL));
+
+    if ((p_gap_addrs != NULL) && (p_addr_cnt != NULL))
+    {
+        NRF_PM_DEBUG_CHECK((*p_addr_cnt) >= peer_cnt);
+
+        copy_addrs  = true;
+        *p_addr_cnt = 0;
+    }
+
+    if ((p_gap_irks != NULL) && (p_irk_cnt != NULL))
+    {
+        NRF_PM_DEBUG_CHECK((*p_irk_cnt) >= peer_cnt);
+
+        copy_irks  = true;
+        *p_irk_cnt = 0;
+    }
+
+    memset(&peer_data, 0x00, sizeof(peer_data));
+    peer_data.p_bonding_data = &bond_data;
+
+    // Read through flash memory and look for peers ID keys.
+
+    for (uint32_t i = 0; i < peer_cnt; i++)
+    {
+        memset(&bond_data, 0x00, sizeof(bond_data));
+
+        // Read peer data from flash.
+        ret = pds_peer_data_read(p_peers[i], PM_PEER_DATA_ID_BONDING,
+                                 &peer_data, &buf_size);
+
+        if ((ret == NRF_ERROR_NOT_FOUND) || (ret == NRF_ERROR_INVALID_PARAM))
+        {
+            // Peer data coulnd't be found in flash or peer ID is not valid.
+            return NRF_ERROR_NOT_FOUND;
+        }
+
+        uint8_t const addr_type = bond_data.peer_ble_id.id_addr_info.addr_type;
+
+        if ((addr_type != BLE_GAP_ADDR_TYPE_PUBLIC) &&
+            (addr_type != BLE_GAP_ADDR_TYPE_RANDOM_STATIC))
+        {
+            // The address shared by the peer during bonding can't be used for whitelisting.
+            return BLE_ERROR_GAP_INVALID_BLE_ADDR;
+        }
+
+        // Copy the GAP address.
+        if (copy_addrs)
+        {
+            memcpy(&p_gap_addrs[i], &bond_data.peer_ble_id.id_addr_info, sizeof(ble_gap_addr_t));
+            (*p_addr_cnt)++;
+        }
+
+        // Copy the IRK.
+        if (copy_irks)
+        {
+            memcpy(&p_gap_irks[i], bond_data.peer_ble_id.id_info.irk, BLE_GAP_SEC_KEY_LEN);
+            (*p_irk_cnt)++;
+        }
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t im_device_identities_list_set(pm_peer_id_t const * p_peers,
+                                         uint32_t             peer_cnt)
+{
+    ret_code_t             ret;
+    pm_peer_data_t         peer_data;
+    pm_peer_data_bonding_t bond_data;
+
+    ble_gap_id_key_t         keys[BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT];
+    ble_gap_id_key_t const * key_ptrs[BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT];
+
+    if (peer_cnt > BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    if ((p_peers == NULL) || (peer_cnt == 0))
+    {
+        // Clear the device identities list.
+        return sd_ble_gap_device_identities_set(NULL, NULL, 0);
+    }
+
+    peer_data.p_bonding_data = &bond_data;
+    uint32_t const buf_size  = sizeof(bond_data);
+
+    memset(keys, 0x00, sizeof(keys));
+    for (uint32_t i = 0; i < BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT; i++)
+    {
+        key_ptrs[i] = &keys[i];
+    }
+
+    for (uint32_t i = 0; i < peer_cnt; i++)
+    {
+        memset(&bond_data, 0x00, sizeof(bond_data));
+
+        // Read peer data from flash.
+        ret = pds_peer_data_read(p_peers[i], PM_PEER_DATA_ID_BONDING,
+                                 &peer_data, &buf_size);
+
+        if ((ret == NRF_ERROR_NOT_FOUND) || (ret == NRF_ERROR_INVALID_PARAM))
+        {
+            NRF_LOG_WARNING("peer id %d: Peer data could not be found in flash. Remove the peer ID "
+                            "from the peer list and try again.",
+                            p_peers[i]);
+            return NRF_ERROR_NOT_FOUND;
+        }
+
+        uint8_t const addr_type = bond_data.peer_ble_id.id_addr_info.addr_type;
+
+        if ((addr_type != BLE_GAP_ADDR_TYPE_PUBLIC) &&
+            (addr_type != BLE_GAP_ADDR_TYPE_RANDOM_STATIC))
+        {
+            NRF_LOG_WARNING("peer id %d: The address shared by the peer during bonding cannot be "
+                            "whitelisted. Remove the peer ID from the peer list and try again.",
+                            p_peers[i]);
+            return BLE_ERROR_GAP_INVALID_BLE_ADDR;
+        }
+
+        // Copy data to the buffer.
+        memcpy(&keys[i], &bond_data.peer_ble_id, sizeof(ble_gap_id_key_t));
+    }
+
+    return sd_ble_gap_device_identities_set(key_ptrs, NULL, peer_cnt);
+}
+
+
+ret_code_t im_id_addr_set(ble_gap_addr_t const * p_addr)
+{
+    return sd_ble_gap_addr_set(p_addr);
+}
+
+
+ret_code_t im_id_addr_get(ble_gap_addr_t * p_addr)
+{
+    NRF_PM_DEBUG_CHECK(p_addr != NULL);
+
+    return sd_ble_gap_addr_get(p_addr);
+}
+
+
+ret_code_t im_privacy_set(pm_privacy_params_t const * p_privacy_params)
+{
+    return sd_ble_gap_privacy_set(p_privacy_params);
+}
+
+
+ret_code_t im_privacy_get(pm_privacy_params_t * p_privacy_params)
+{
+    return sd_ble_gap_privacy_get(p_privacy_params);
+}
+
+
+/* Create a whitelist for the user using the cached list of peers.
+ * This whitelist is meant to be provided by the application to the Advertising module.
+ */
+ret_code_t im_whitelist_get(ble_gap_addr_t * p_addrs,
+                            uint32_t       * p_addr_cnt,
+                            ble_gap_irk_t  * p_irks,
+                            uint32_t       * p_irk_cnt)
+{
+    // One of the two buffers has to be provided.
+    NRF_PM_DEBUG_CHECK((p_addrs    != NULL) || (p_irks    != NULL));
+    NRF_PM_DEBUG_CHECK((p_addr_cnt != NULL) || (p_irk_cnt != NULL));
+
+    if (((p_addr_cnt != NULL) && (m_wlisted_peer_cnt > *p_addr_cnt)) ||
+        ((p_irk_cnt  != NULL) && (m_wlisted_peer_cnt > *p_irk_cnt)))
+    {
+        // The size of the cached list of peers is larger than the provided buffers.
+        return NRF_ERROR_NO_MEM;
+    }
+
+    // NRF_SUCCESS or
+    // NRF_ERROR_NOT_FOUND,            if a peer or its data were not found.
+    // BLE_ERROR_GAP_INVALID_BLE_ADDR, if a peer address can not be used for whitelisting.
+    return peers_id_keys_get(m_wlisted_peers, m_wlisted_peer_cnt,
+                             p_addrs, p_addr_cnt,
+                             p_irks,  p_irk_cnt);
+}
+
+
+/* Copies the peers to whitelist into a local cache.
+ * The cached list will be used by im_whitelist_get() to retrieve the active whitelist.
+ * For SoftDevices 3x, also loads the peers' GAP addresses and whitelists them using
+ * sd_ble_gap_whitelist_set().
+ */
+ret_code_t im_whitelist_set(pm_peer_id_t const * p_peers,
+                            uint32_t             peer_cnt)
+{
+    // Clear the cache of whitelisted peers.
+    memset(m_wlisted_peers, 0x00, sizeof(m_wlisted_peers));
+
+    if ((p_peers == NULL) || (peer_cnt == 0))
+    {
+        // Clear the current whitelist.
+        m_wlisted_peer_cnt = 0;
+
+        // NRF_SUCCESS, or
+        // BLE_GAP_ERROR_WHITELIST_IN_USE
+        return sd_ble_gap_whitelist_set(NULL, 0);
+    }
+
+    // Copy the new whitelisted peers.
+    m_wlisted_peer_cnt = peer_cnt;
+    memcpy(m_wlisted_peers, p_peers, sizeof(pm_peer_id_t) * peer_cnt);
+
+    ret_code_t ret;
+    uint32_t   wlist_addr_cnt = 0;
+
+    ble_gap_addr_t const * addr_ptrs[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
+    ble_gap_addr_t         addrs[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
+
+    memset(addrs, 0x00, sizeof(addrs));
+
+    // Fetch GAP addresses for these peers, but don't fetch IRKs.
+    ret = peers_id_keys_get(p_peers, peer_cnt, addrs, &wlist_addr_cnt, NULL, NULL);
+
+    if (ret != NRF_SUCCESS)
+    {
+        // NRF_ERROR_NOT_FOUND,            if a peer or its data were not found.
+        // BLE_ERROR_GAP_INVALID_BLE_ADDR, if a peer address can not be used for whitelisting.
+        return ret;
+    }
+
+    for (uint32_t i = 0; i < BLE_GAP_WHITELIST_ADDR_MAX_COUNT; i++)
+    {
+        addr_ptrs[i] = &addrs[i];
+    }
+
+    // NRF_ERROR_DATA_SIZE,             if peer_cnt > BLE_GAP_WHITELIST_ADDR_MAX_COUNT.
+    // BLE_ERROR_GAP_WHITELIST_IN_USE,  if a whitelist is in use.
+    return sd_ble_gap_whitelist_set(addr_ptrs, peer_cnt);
+}
+
+
+/**@brief Function for calculating the ah() hash function described in Bluetooth core specification
+ *        4.2 section 3.H.2.2.2.
+ *
+ * @detail  BLE uses a hash function to calculate the first half of a resolvable address
+ *          from the second half of the address and an irk. This function will use the ECB
+ *          periferal to hash these data acording to the Bluetooth core specification.
+ *
+ * @note The ECB expect little endian input and output.
+ *       This function expect big endian and will reverse the data as necessary.
+ *
+ * @param[in]  p_k          The key used in the hash function.
+ *                          For address resolution this is should be the irk.
+ *                          The array must have a length of 16.
+ * @param[in]  p_r          The rand used in the hash function. For generating a new address
+ *                          this would be a random number. For resolving a resolvable address
+ *                          this would be the last half of the address being resolved.
+ *                          The array must have a length of 3.
+ * @param[out] p_local_hash The result of the hash operation. For address resolution this
+ *                          will match the first half of the address being resolved if and only
+ *                          if the irk used in the hash function is the same one used to generate
+ *                          the address.
+ *                          The array must have a length of 16.
+ */
+void ah(uint8_t const * p_k, uint8_t const * p_r, uint8_t * p_local_hash)
+{
+    nrf_ecb_hal_data_t ecb_hal_data;
+
+    for (uint32_t i = 0; i < SOC_ECB_KEY_LENGTH; i++)
+    {
+        ecb_hal_data.key[i] = p_k[SOC_ECB_KEY_LENGTH - 1 - i];
+    }
+
+    memset(ecb_hal_data.cleartext, 0, SOC_ECB_KEY_LENGTH - IM_ADDR_CLEARTEXT_LENGTH);
+
+    for (uint32_t i = 0; i < IM_ADDR_CLEARTEXT_LENGTH; i++)
+    {
+        ecb_hal_data.cleartext[SOC_ECB_KEY_LENGTH - 1 - i] = p_r[i];
+    }
+
+    // Can only return NRF_SUCCESS.
+    (void) sd_ecb_block_encrypt(&ecb_hal_data);
+
+    for (uint32_t i = 0; i < IM_ADDR_CIPHERTEXT_LENGTH; i++)
+    {
+        p_local_hash[i] = ecb_hal_data.ciphertext[SOC_ECB_KEY_LENGTH - 1 - i];
+    }
+}
+
+
+bool im_address_resolve(ble_gap_addr_t const * p_addr, ble_gap_irk_t const * p_irk)
+{
+    uint8_t hash[IM_ADDR_CIPHERTEXT_LENGTH];
+    uint8_t local_hash[IM_ADDR_CIPHERTEXT_LENGTH];
+    uint8_t prand[IM_ADDR_CLEARTEXT_LENGTH];
+
+    if (p_addr->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
+    {
+        return false;
+    }
+
+    memcpy(hash, p_addr->addr, IM_ADDR_CIPHERTEXT_LENGTH);
+    memcpy(prand, &p_addr->addr[IM_ADDR_CIPHERTEXT_LENGTH], IM_ADDR_CLEARTEXT_LENGTH);
+    ah(p_irk->irk, prand, local_hash);
+
+    return (memcmp(hash, local_hash, IM_ADDR_CIPHERTEXT_LENGTH) == 0);
+}
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/id_manager.h

@@ -0,0 +1,318 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef PEER_ID_MANAGER_H__
+#define PEER_ID_MANAGER_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup id_manager ID Manager
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for keeping track of peer identities
+ *       (IRK and peer address).
+ */
+
+
+/**@brief Function for dispatching SoftDevice events to the ID Manager module.
+ *
+ * @param[in]  p_ble_evt  The SoftDevice event.
+ */
+void im_ble_evt_handler(ble_evt_t const * p_ble_evt);
+
+
+/**@brief Function for getting the corresponding peer ID from a connection handle.
+ *
+ * @param[in]  conn_handle  The connection handle.
+ *
+ * @return The corresponding peer ID, or @ref PM_PEER_ID_INVALID if none could be resolved.
+ */
+pm_peer_id_t im_peer_id_get_by_conn_handle(uint16_t conn_handle);
+
+
+/**@brief Function for getting the corresponding peer ID from a master ID (EDIV and rand).
+ *
+ * @param[in]  p_master_id  The master ID.
+ *
+ * @return The corresponding peer ID, or @ref PM_PEER_ID_INVALID if none could be resolved.
+ */
+pm_peer_id_t im_peer_id_get_by_master_id(ble_gap_master_id_t const * p_master_id);
+
+
+/**@brief Function for getting the corresponding connection handle from a peer ID.
+ *
+ * @param[in] peer_id  The peer ID.
+ *
+ * @return The corresponding connection handle, or @ref BLE_CONN_HANDLE_INVALID if none could be
+ *         resolved. The conn_handle can refer to a recently disconnected connection.
+ */
+uint16_t im_conn_handle_get(pm_peer_id_t peer_id);
+
+
+/**@brief Function for comparing two master ids
+ * @note  Two invalid master IDs will not match.
+ *
+ * @param[in]  p_master_id1 First master id for comparison
+ * @param[in]  p_master_id2 Second master id for comparison
+ *
+ * @return     True if the input matches, false if it does not.
+ */
+bool im_master_ids_compare(ble_gap_master_id_t const * p_master_id1,
+                           ble_gap_master_id_t const * p_master_id2);
+
+
+/**@brief Function for getting the BLE address used by the peer when connecting.
+ *
+ * @param[in]  conn_handle  The connection handle.
+ * @param[out] p_ble_addr   The BLE address used by the peer when the connection specified by
+ *                          conn_handle was established. Cannot be NULL.
+ *
+ * @retval NRF_SUCCESS                   The address was found and copied.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE conn_handle does not refer to an active connection.
+ */
+ret_code_t im_ble_addr_get(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr);
+
+
+/**@brief Function for checking if a master ID is valid or invalid
+ *
+ * @param[in]  p_master_id  The master ID.
+ *
+ * @retval true   The master id is valid.
+ * @retval false  The master id is invalid (i.e. all zeros).
+ */
+bool im_master_id_is_valid(ble_gap_master_id_t const * p_master_id);
+
+
+/**@brief Function for checking if two pieces of bonding data correspond to the same peer.
+ *
+ * @param[in]  p_bonding_data1  The first piece of bonding data to check.
+ * @param[in]  p_bonding_data2  The second piece of bonding data to check.
+ *
+ * @retval true   The bonding data correspond to the same peer.
+ * @retval false  The bonding data do not correspond to the same peer.
+ */
+bool im_is_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data1,
+                                  pm_peer_data_bonding_t const * p_bonding_data2);
+
+
+/**@brief Function for finding if we are already bonded to a peer.
+ *
+ * @param[in]  p_bonding_data  The bonding data to check.
+ * @param[in]  peer_id_skip    Optional peer to ignore when searching for duplicates.
+ *
+ * @return  An existing peer ID for the peer, or PM_PEER_ID_INVALID if none was found.
+ */
+pm_peer_id_t im_find_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data,
+                                            pm_peer_id_t                   peer_id_skip);
+
+
+/**@brief Function for reporting that a new peer ID has been allocated for a specified connection.
+ *
+ * @param[in]  conn_handle  The connection.
+ * @param[in]  peer_id      The new peer ID.
+ */
+void im_new_peer_id(uint16_t conn_handle, pm_peer_id_t peer_id);
+
+
+/**@brief Function for deleting all of a peer's data from flash and disassociating it from any
+ *        connection handles it is associated with.
+ *
+ * @param[in]  peer_id  The peer to free.
+ *
+ * @return Any error code returned by @ref pdb_peer_free.
+ */
+ret_code_t im_peer_free(pm_peer_id_t peer_id);
+
+
+/**@brief Function to set the local Bluetooth identity address.
+ *
+ * @details The local Bluetooth identity address is the address that identifies this device to other
+ *          peers. The address type must be either @ref BLE_GAP_ADDR_TYPE_PUBLIC or @ref
+ *          BLE_GAP_ADDR_TYPE_RANDOM_STATIC. The identity address cannot be changed while roles are
+ *          running.
+ *
+ * @note This address will be distributed to the peer during bonding.
+ *       If the address changes, the address stored in the peer device will not be valid and the
+ *       ability to reconnect using the old address will be lost.
+ *
+ * @note By default the SoftDevice will set an address of type @ref BLE_GAP_ADDR_TYPE_RANDOM_STATIC
+ *       upon being enabled. The address is a random number populated during the IC manufacturing
+ *       process and remains unchanged for the lifetime of each IC.
+ *
+ * @param[in] p_addr Pointer to address structure.
+ *
+ * @retval NRF_SUCCESS                     Address successfully set.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR  If the GAP address is invalid.
+ * @retval NRF_ERROR_BUSY                  Could not process at this time. Process SoftDevice events
+ *                                         and retry.
+ * @retval NRF_ERROR_INVALID_STATE         The identity address cannot be changed while advertising,
+ *                                         scanning, or while in a connection.
+ * @retval NRF_ERROR_INTERNAL              If an internal error occurred.
+ */
+ret_code_t im_id_addr_set(ble_gap_addr_t const * p_addr);
+
+
+/**@brief Function to get the local Bluetooth identity address.
+ *
+ * @note This will always return the identity address irrespective of the privacy settings,
+ *       i.e. the address type will always be either @ref BLE_GAP_ADDR_TYPE_PUBLIC or @ref
+ *       BLE_GAP_ADDR_TYPE_RANDOM_STATIC.
+ *
+ * @param[out] p_addr Pointer to address structure to be filled in.
+ *
+ * @retval NRF_SUCCESS  If the address was successfully retrieved.
+ */
+ret_code_t im_id_addr_get(ble_gap_addr_t * p_addr);
+
+
+/**@brief Function to set privacy settings.
+ *
+ * @details Privacy settings cannot be set while advertising, scanning, or while in a connection.
+ *
+ * @param[in] p_privacy_params Privacy settings.
+ *
+ * @retval NRF_SUCCESS              If privacy options were set successfully.
+ * @retval NRF_ERROR_NULL           If @p p_privacy_params is NULL.
+ * @retval NRF_ERROR_INVALID_PARAM  If the address type is not valid.
+ * @retval NRF_ERROR_BUSY           If the request could not be processed at this time.
+ *                                  Process SoftDevice events and retry.
+ * @retval NRF_ERROR_INVALID_STATE  Privacy settings cannot be changed while BLE roles using
+ *                                  privacy are enabled.
+ */
+ret_code_t im_privacy_set(pm_privacy_params_t const * p_privacy_params);
+
+
+/**@brief Function to retrieve the current privacy settings.
+ *
+ * @details The privacy settings returned include the current device irk as well.
+ *
+ * @param[in] p_privacy_params Privacy settings.
+ *
+ * @retval NRF_SUCCESS            Successfully retrieved privacy settings.
+ * @retval NRF_ERROR_NULL         @c p_privacy_params is NULL.
+ * @retval NRF_ERROR_INTERNAL     If an internal error occurred.
+ */
+ret_code_t im_privacy_get(pm_privacy_params_t * p_privacy_params);
+
+
+/**@brief Function for resolving a resolvable address with an identity resolution key (IRK).
+ *
+ * @details This function will use the ECB peripheral to resolve a resolvable address.
+ *          This can be used to resolve the identity of a device distributing a random
+ *          resolvable address based on any IRKs you have received earlier. If an address is
+ *          resolved by an IRK, the device distributing the address must also know the IRK.
+ *
+ * @param[in] p_addr  A random resolvable address.
+ * @param[in] p_irk   An identity resolution key (IRK).
+ *
+ * @retval true   The irk used matched the one used to create the address.
+ * @retval false  The irk used did not match the one used to create the address, or an argument was
+ *                NULL.
+ */
+bool im_address_resolve(ble_gap_addr_t const * p_addr, ble_gap_irk_t const * p_irk);
+
+
+/**@brief Function for setting / clearing the whitelist.
+ *
+ * @param p_peers   The peers to whitelist. Pass NULL to clear the whitelist.
+ * @param peer_cnt  The number of peers to whitelist. Pass zero to clear the whitelist.
+ *
+ * @retval NRF_SUCCESS                      If the whitelist was successfully set or cleared.
+ * @retval BLE_GAP_ERROR_WHITELIST_IN_USE   If a whitelist is in use.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR   If any peer has an address which can not be used
+ *                                          for whitelisting.
+ * @retval NRF_ERROR_NOT_FOUND              If any peer or its data could not be found.
+ * @retval NRF_ERROR_DATA_SIZE              If @p peer_cnt is greater than
+ *                                          @ref BLE_GAP_WHITELIST_ADDR_MAX_COUNT.
+ */
+ret_code_t im_whitelist_set(pm_peer_id_t const * p_peers,
+                            uint32_t     const   peer_cnt);
+
+
+/**@brief Retrieves the current whitelist, set by a previous call to @ref im_whitelist_set.
+ *
+ * @param[out]   A buffer where to copy the GAP addresses.
+ * @param[inout] In: the size of the @p p_addrs buffer.
+ *               Out: the number of address copied into the buffer.
+ * @param[out]   A buffer where to copy the IRKs.
+ * @param[inout] In: the size of the @p p_irks buffer.
+ *               Out: the number of IRKs copied into the buffer.
+ *
+ * @retval NRF_SUCCESS                      If the whitelist was successfully retrieved.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR   If any peer has an address which can not be used for
+ *                                          whitelisting.
+ * @retval NRF_ERROR_NOT_FOUND              If the data for any of the cached whitelisted peers
+ *                                          can not be found anymore. It might have been deleted in
+ *                                          the meanwhile.
+ * @retval NRF_ERROR_NO_MEM                 If the provided buffers are too small.
+ */
+ret_code_t im_whitelist_get(ble_gap_addr_t * p_addrs,
+                            uint32_t       * p_addr_cnt,
+                            ble_gap_irk_t  * p_irks,
+                            uint32_t       * p_irk_cnt);
+
+
+/**@brief Set the device identities list.
+ */
+ret_code_t im_device_identities_list_set(pm_peer_id_t const * p_peers,
+                                         uint32_t             peer_cnt);
+
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PEER_ID_MANAGER_H__ */

components/ble/peer_manager/nrf_ble_lesc.c

@@ -0,0 +1,519 @@
+/**
+ * Copyright (c) 2018 - 2021, 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.
+ *
+ */
+
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(NRF_BLE_LESC)
+
+#include "nrf_ble_lesc.h"
+#include "nrf_crypto.h"
+
+#define NRF_LOG_MODULE_NAME nrf_ble_lesc
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+/**@brief Descriptor of the peer public key. */
+typedef struct
+{
+    nrf_crypto_ecc_public_key_t value;             /**< Peer public key. */
+    bool                        is_requested;      /**< Flag indicating that the public key has been requested to compute DH key. */
+    bool                        is_valid;          /**< Flag indicating that the public key is valid. */
+    bool                        passkey_requested; /**< Flag indicating that the passkey key has been requested. */
+    bool                        passkey_displayed; /**< Flag indicating that the passkey display event has been received. */
+} nrf_ble_lesc_peer_pub_key_t;
+
+/**@brief   The maximum number of peripheral and central connections combined.
+ *          This value is based on what is configured in the SoftDevice handler sdk_config.
+ */
+#define NRF_BLE_LESC_LINK_COUNT (NRF_SDH_BLE_PERIPHERAL_LINK_COUNT + NRF_SDH_BLE_CENTRAL_LINK_COUNT)
+
+__ALIGN(4) static ble_gap_lesc_p256_pk_t m_lesc_public_key;                             /**< LESC ECC Public Key. */
+__ALIGN(4) static ble_gap_lesc_dhkey_t   m_lesc_dh_key;                                 /**< LESC ECC DH Key. */
+
+static nrf_crypto_ecdh_context_t                  m_ecdh_context;                       /**< Context to do the LESC ECDH calculation */
+
+static bool                                       m_ble_lesc_internal_error;            /**< Flag indicating that the module encountered an internal error. */
+static bool                                       m_keypair_generated;                  /**< Flag indicating that the local ECDH key pair was generated. */
+static nrf_crypto_ecc_key_pair_generate_context_t m_keygen_context;                     /**< Context to generate private/public key pair. */
+static nrf_crypto_ecc_private_key_t               m_private_key;                        /**< Allocated private key type to use for LESC DH generation. */
+static nrf_crypto_ecc_public_key_t                m_public_key;                         /**< Allocated public key type to use for LESC DH generation. */
+static nrf_ble_lesc_peer_pub_key_t                m_peer_keys[NRF_BLE_LESC_LINK_COUNT]; /**< Array of pointers to peer public keys, used for LESC DH generation. */
+
+static bool                                       m_lesc_oobd_own_generated;
+static ble_gap_lesc_oob_data_t                    m_ble_lesc_oobd_own;                  /**< LESC OOB data used in LESC OOB pairing mode. */
+static nrf_ble_lesc_peer_oob_data_handler         m_lesc_oobd_peer_handler;
+
+ret_code_t nrf_ble_lesc_init(void)
+{
+    ret_code_t err_code;
+
+    memset((void *) m_peer_keys, 0, sizeof(m_peer_keys));
+
+#if NRF_CRYPTO_ALLOCATOR == NRF_CRYPTO_ALLOCATOR_NRF_MALLOC
+    // Initialize mem_manager if used by nrf_crypto.
+    err_code = nrf_mem_init();
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_mem_init() returned error 0x%x.", err_code);
+        return err_code;
+    }
+#endif
+
+    // Ensure that nrf_crypto has been initialized.
+    err_code = nrf_crypto_init();
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_crypto_init() returned error 0x%x.", err_code);
+        return err_code;
+    }
+    NRF_LOG_DEBUG("Initialized nrf_crypto.");
+
+#if defined(NRF_CRYPTO_RNG_AUTO_INIT_ENABLED) && (NRF_CRYPTO_RNG_AUTO_INIT_ENABLED == 1)
+    // Do nothing. RNG is initialized with nrf_crypto_init call.
+#elif defined(NRF_CRYPTO_RNG_AUTO_INIT_ENABLED) && (NRF_CRYPTO_RNG_AUTO_INIT_ENABLED == 0)
+    // Initialize the RNG.
+    err_code = nrf_crypto_rng_init(NULL, NULL);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_crypto_rng_init() returned error 0x%x.", err_code);
+        return err_code;
+    }
+#else
+    #error Invalid sdk_config.h (does not contain NRF_CRYPTO_RNG_AUTO_INIT_ENABLED)
+#endif // defined(NRF_CRYPTO_RNG_AUTO_INIT_ENABLED) && (NRF_CRYPTO_RNG_AUTO_INIT_ENABLED == 1)
+    NRF_LOG_DEBUG("Initialized nrf_ble_lesc.");
+
+    // Reset module state.
+    m_ble_lesc_internal_error = false;
+    m_keypair_generated       = false;
+
+    // Generate ECC key pair. Only one key pair is automatically generated by this module.
+    err_code = nrf_ble_lesc_keypair_generate();
+    return err_code;
+}
+
+
+ret_code_t nrf_ble_lesc_keypair_generate(void)
+{
+    ret_code_t err_code;
+    size_t     public_len = NRF_CRYPTO_ECC_SECP256R1_RAW_PUBLIC_KEY_SIZE;
+
+    // Check if any DH computation is pending
+    for (uint32_t i = 0; i < ARRAY_SIZE(m_peer_keys); i++)
+    {
+        if (m_peer_keys[i].is_valid)
+        {
+            return NRF_ERROR_BUSY;
+        }
+    }
+
+    // Update flag to indicate that there is no valid private key.
+    m_keypair_generated       = false;
+    m_lesc_oobd_own_generated = false;
+
+    NRF_LOG_DEBUG("Generating ECC key pair");
+    err_code = nrf_crypto_ecc_key_pair_generate(&m_keygen_context,
+                                                &g_nrf_crypto_ecc_secp256r1_curve_info,
+                                                &m_private_key,
+                                                &m_public_key);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_crypto_ecc_key_pair_generate() returned error 0x%x.", err_code);
+        return err_code;
+    }
+
+    // Convert to a raw type.
+    err_code = nrf_crypto_ecc_public_key_to_raw(&m_public_key,
+                                                m_lesc_public_key.pk,
+                                                &public_len);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_crypto_ecc_public_key_to_raw() returned error 0x%x.", err_code);
+        return err_code;
+    }
+
+    // Invert the raw type to little-endian (required for BLE).
+    err_code = nrf_crypto_ecc_byte_order_invert(&g_nrf_crypto_ecc_secp256r1_curve_info,
+                                                m_lesc_public_key.pk,
+                                                m_lesc_public_key.pk,
+                                                NRF_CRYPTO_ECC_SECP256R1_RAW_PUBLIC_KEY_SIZE);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_crypto_ecc_byte_order_invert() returned error 0x%x.", err_code);
+    }
+    else
+    {
+        // Set the flag to indicate that there is a valid ECDH key pair generated.
+        m_keypair_generated = true;
+    }
+
+    return err_code;
+}
+
+
+ret_code_t nrf_ble_lesc_own_oob_data_generate(void)
+{
+    ret_code_t err_code = NRF_ERROR_INVALID_STATE;
+
+    m_lesc_oobd_own_generated = false;
+
+    if (m_keypair_generated)
+    {
+        err_code = sd_ble_gap_lesc_oob_data_get(BLE_CONN_HANDLE_INVALID,
+                                                &m_lesc_public_key,
+                                                &m_ble_lesc_oobd_own);
+        if (err_code == NRF_SUCCESS)
+        {
+            m_lesc_oobd_own_generated = true;
+        }
+    }
+
+    return err_code;
+}
+
+
+ble_gap_lesc_p256_pk_t * nrf_ble_lesc_public_key_get(void)
+{
+    ble_gap_lesc_p256_pk_t * p_lesc_pk = NULL;
+
+    if (m_keypair_generated)
+    {
+        p_lesc_pk = &m_lesc_public_key;
+    }
+    else
+    {
+        NRF_LOG_ERROR("Trying to access LESC public key that has not been generated yet.");
+    }
+
+    return p_lesc_pk;
+}
+
+
+ble_gap_lesc_oob_data_t * nrf_ble_lesc_own_oob_data_get(void)
+{
+    ble_gap_lesc_oob_data_t * p_lesc_oobd_own = NULL;
+
+    if (m_lesc_oobd_own_generated)
+    {
+        p_lesc_oobd_own = &m_ble_lesc_oobd_own;
+    }
+    else
+    {
+        NRF_LOG_ERROR("Trying to access LESC OOB data that have not been generated yet.");
+    }
+
+    return p_lesc_oobd_own;
+}
+
+
+void nrf_ble_lesc_peer_oob_data_handler_set(nrf_ble_lesc_peer_oob_data_handler handler)
+{
+    m_lesc_oobd_peer_handler = handler;
+}
+
+
+/**@brief Function for calculating a DH key and responding to the DH key request on a given
+ *        connection handle.
+ *
+ * @param[in]  p_peer_public_key  ECC peer public key, used to compute shared secret.
+ * @param[in]  conn_handle        Connection handle.
+ *
+ * @retval NRF_SUCCESS If the operation was successful.
+ * @retval Other       Other error codes might be returned by the @ref nrf_crypto_ecdh_compute, @ref
+ *                     nrf_crypto_ecc_byte_order_invert, and @ref sd_ble_gap_lesc_dhkey_reply functions.
+ */
+static ret_code_t compute_and_give_dhkey(nrf_ble_lesc_peer_pub_key_t * p_peer_public_key,
+                                         uint16_t                      conn_handle)
+{
+    ret_code_t err_code           = NRF_ERROR_INTERNAL;
+    size_t     shared_secret_size = BLE_GAP_LESC_DHKEY_LEN;
+    uint8_t  * p_shared_secret    = m_lesc_dh_key.key;
+
+    // Check if there is a valid generated and set a local ECDH public key.
+    if (!m_keypair_generated)
+    {
+        return NRF_ERROR_INTERNAL;
+    }
+
+    // Check if the public_key is valid
+    if (p_peer_public_key->is_valid)
+    {
+        err_code = nrf_crypto_ecdh_compute(&m_ecdh_context,
+                                           &m_private_key,
+                                           &p_peer_public_key->value,
+                                           p_shared_secret,
+                                           &shared_secret_size);
+    }
+
+    if (err_code == NRF_SUCCESS)
+    {
+        // Invert the shared secret for little endian format.
+        err_code = nrf_crypto_ecc_byte_order_invert(&g_nrf_crypto_ecc_secp256r1_curve_info,
+                                                    p_shared_secret,
+                                                    p_shared_secret,
+                                                    BLE_GAP_LESC_DHKEY_LEN);
+        VERIFY_SUCCESS(err_code);
+    }
+    else
+    {
+        NRF_LOG_WARNING("Creating invalid shared secret to make LESC fail.");
+        err_code = nrf_crypto_rng_vector_generate(p_shared_secret, BLE_GAP_LESC_DHKEY_LEN);
+        VERIFY_SUCCESS(err_code);
+    }
+
+    NRF_LOG_INFO("Calling sd_ble_gap_lesc_dhkey_reply on conn_handle: %d", conn_handle);
+    err_code = sd_ble_gap_lesc_dhkey_reply(conn_handle, &m_lesc_dh_key);
+
+    return err_code;
+}
+
+
+ret_code_t nrf_ble_lesc_request_handler(void)
+{
+    ret_code_t err_code = NRF_SUCCESS;
+
+    // If the LESC module is in an invalid state, a restart is required.
+    if (m_ble_lesc_internal_error)
+    {
+        return NRF_ERROR_INTERNAL;
+    }
+
+    for (uint16_t i = 0; i < NRF_BLE_LESC_LINK_COUNT; i++)
+    {
+        if (m_peer_keys[i].is_requested)
+        {
+            err_code                         = compute_and_give_dhkey(&m_peer_keys[i], i);
+            m_peer_keys[i].is_requested      = false;
+            m_peer_keys[i].is_valid          = false;
+            m_peer_keys[i].passkey_requested = false;
+            m_peer_keys[i].passkey_displayed = false;
+
+            VERIFY_SUCCESS(err_code);
+
+        }
+    }
+
+    return err_code;
+}
+
+
+/**@brief Function for handling a DH key request event.
+ *
+ * @param[in]  conn_handle      Connection handle.
+ * @param[in]  p_dhkey_request  DH key request descriptor.
+ *
+ * @retval NRF_SUCCESS      If the operation was successful.
+ * @retval Other            Other error codes might be returned by the @ref nrf_crypto_ecc_byte_order_invert
+ *                          and @ref nrf_crypto_ecc_public_key_from_raw functions.
+ */
+static ret_code_t on_dhkey_request(uint16_t                                 conn_handle,
+                                   ble_gap_evt_lesc_dhkey_request_t const * p_dhkey_request)
+{
+    ret_code_t err_code = NRF_SUCCESS;
+    uint8_t    public_raw[BLE_GAP_LESC_P256_PK_LEN];
+    uint8_t  * p_public_raw;
+    size_t     public_raw_len;
+
+    p_public_raw   = p_dhkey_request->p_pk_peer->pk;
+    public_raw_len = BLE_GAP_LESC_P256_PK_LEN;
+
+    // Don't allow to pair with remote peer which uses the same public key.
+    // Compare only X cordinate of the public key, bytes from 0 to 31.
+    if (memcmp(m_lesc_public_key.pk, p_public_raw, BLE_GAP_LESC_P256_PK_LEN / 2) == 0)
+    {
+        NRF_LOG_WARNING("Remote peer is using identical public key.");
+        m_peer_keys[conn_handle].is_valid = false;
+
+        // In case when we have gotten passkey requested then we will respond to it with the
+        // "NONE" key type to prevent us from going through Authentication Stage 1.
+        if (m_peer_keys[conn_handle].passkey_requested)
+        {
+            m_peer_keys[conn_handle].passkey_requested = false;
+
+            err_code = sd_ble_gap_auth_key_reply(conn_handle, BLE_GAP_AUTH_KEY_TYPE_NONE, NULL);
+
+            return err_code;
+
+        }
+        // In case we have gotten passkey display event then we need to disconnect a link
+        // to prevent us from going through Authentication Stage 1.
+        else if (m_peer_keys[conn_handle].passkey_displayed)
+        {
+            m_peer_keys[conn_handle].passkey_displayed = false;
+
+            err_code = sd_ble_gap_disconnect(conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
+            if (err_code != NRF_SUCCESS && err_code != NRF_ERROR_INVALID_STATE)
+            {
+                return err_code;
+            }
+
+            return NRF_SUCCESS;
+        }
+        else
+        {
+            // Do nothing
+        }
+    } else {
+        // Convert the received public key from little endian to big-endian.
+        err_code = nrf_crypto_ecc_byte_order_invert(&g_nrf_crypto_ecc_secp256r1_curve_info,
+                                                    p_public_raw,
+                                                    public_raw,
+                                                    public_raw_len);
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_ERROR("nrf_crypto_ecc_byte_order_invert() returned error 0x%x.", err_code);
+            return err_code;
+        }
+
+        // Copy peer public key to the allocated context. The dhkey calculation will be performed in
+        // @ref nrf_ble_lesc_request_handler, so it does not block normal operation.
+        err_code = nrf_crypto_ecc_public_key_from_raw(&g_nrf_crypto_ecc_secp256r1_curve_info,
+                                                   &m_peer_keys[conn_handle].value,
+                                                   public_raw,
+                                                   public_raw_len);
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_ERROR("nrf_crypto_ecc_public_key_from_raw() returned error 0x%x.", err_code);
+            m_peer_keys[conn_handle].is_valid = false;
+        }
+        else
+        {
+            m_peer_keys[conn_handle].is_valid = true;
+        }
+    }
+
+    m_peer_keys[conn_handle].is_requested = true;
+
+    return NRF_SUCCESS;
+}
+
+
+/**@brief Function for setting LESC OOB data.
+ *
+ * @param[in]  conn_handle      Connection handle.
+ *
+ * @retval NRF_SUCCESS      If the operation was successful.
+ * @retval Other            Other error codes might be returned by the @ref sd_ble_gap_lesc_oob_data_set.
+ */
+static ret_code_t lesc_oob_data_set(uint16_t conn_handle)
+{
+    ret_code_t                err_code;
+    ble_gap_lesc_oob_data_t * p_lesc_oobd_own;
+    ble_gap_lesc_oob_data_t * p_lesc_oobd_peer;
+
+    p_lesc_oobd_own  = (m_lesc_oobd_own_generated) ? &m_ble_lesc_oobd_own : NULL;
+    p_lesc_oobd_peer = (m_lesc_oobd_peer_handler != NULL) ?
+                        m_lesc_oobd_peer_handler(conn_handle) : NULL;
+
+    err_code = sd_ble_gap_lesc_oob_data_set(conn_handle,
+                                            p_lesc_oobd_own,
+                                            p_lesc_oobd_peer);
+    return err_code;
+}
+
+
+void nrf_ble_lesc_on_ble_evt(ble_evt_t const * p_ble_evt)
+{
+    ret_code_t err_code    = NRF_SUCCESS;
+    uint16_t   conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
+
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_DISCONNECTED:
+            m_peer_keys[conn_handle].is_valid          = false;
+            m_peer_keys[conn_handle].is_requested      = false;
+            m_peer_keys[conn_handle].passkey_requested = false;
+            m_peer_keys[conn_handle].passkey_displayed = false;
+
+            break;
+        
+        case BLE_GAP_EVT_AUTH_KEY_REQUEST:
+            if (p_ble_evt->evt.gap_evt.params.auth_key_request.key_type ==
+                BLE_GAP_AUTH_KEY_TYPE_PASSKEY)
+            {
+                m_peer_keys[conn_handle].passkey_requested = true;
+            }
+
+            break;
+        
+        case BLE_GAP_EVT_PASSKEY_DISPLAY:
+            m_peer_keys[conn_handle].passkey_displayed = true;
+
+            break;
+
+        case BLE_GAP_EVT_LESC_DHKEY_REQUEST:
+            NRF_LOG_DEBUG("BLE_GAP_EVT_LESC_DHKEY_REQUEST");
+
+            if (p_ble_evt->evt.gap_evt.params.lesc_dhkey_request.oobd_req)
+            {
+                err_code = lesc_oob_data_set(conn_handle);
+                if (err_code != NRF_SUCCESS)
+                {
+                    NRF_LOG_ERROR("sd_ble_gap_lesc_oob_data_set() returned error 0x%x.", err_code);
+                    m_ble_lesc_internal_error = true;
+                }
+            }
+
+            err_code = on_dhkey_request(conn_handle,
+                                        &p_ble_evt->evt.gap_evt.params.lesc_dhkey_request);
+            if (err_code != NRF_SUCCESS)
+            {
+                m_ble_lesc_internal_error = true;
+            }
+            break;
+
+#if NRF_BLE_LESC_GENERATE_NEW_KEYS
+        case BLE_GAP_EVT_AUTH_STATUS:
+            // Generate new pairing keys.
+             err_code = nrf_ble_lesc_keypair_generate();
+             if (err_code != NRF_SUCCESS)
+             {
+                m_ble_lesc_internal_error = true;
+             }
+
+             break;
+#endif // NRF_BLE_LESC_GENERATE_NEW_KEYS
+
+        default:
+            break;
+    }
+}
+
+
+#endif // NRF_BLE_LESC_ENABLED

components/ble/peer_manager/nrf_ble_lesc.h

@@ -0,0 +1,164 @@
+/**
+ * Copyright (c) 2018 - 2021, 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 nrf_ble_lesc LESC module
+ * @{
+ * @ingroup peer_manager
+ * @brief Module for handling LESC related events.
+ */
+
+#ifndef NRF_BLE_LESC_H__
+#define NRF_BLE_LESC_H__
+
+#include <stdint.h>
+#include <string.h>
+#include "ble.h"
+#include "sdk_errors.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief Peer OOB Data handler prototype. */
+typedef ble_gap_lesc_oob_data_t * (* nrf_ble_lesc_peer_oob_data_handler)(uint16_t conn_handle);
+
+/**@brief   Function for initializing the LESC module.
+ *
+ * @details This function initializes the nrf_crypto for ECC and ECDH calculations, which are
+ *          required to handle LESC authentication procedures.
+ *
+ * @retval NRF_SUCCESS   If the operation was successful.
+ * @retval Other         Other error codes might be returned by the @ref nrf_crypto_init or
+ *                       @ref nrf_ble_lesc_keypair_generate functions.
+ */
+ret_code_t nrf_ble_lesc_init(void);
+
+
+/**@brief   Function for generating ECC keypair used for the LESC procedure.
+ *
+ * @details This function generates an ECC key pair, which consists of a private and public key. Keys are
+ *          generated using ECC and are used to create LESC DH key during authentication procedures.
+ *
+ * @retval NRF_SUCCESS    If the operation was successful.
+ * @retval NRF_ERROR_BUSY If any pending request needs to be processed by @ref nrf_ble_lesc_request_handler.
+ * @retval Other          Other error codes might be returned by the @ref nrf_crypto_ecc_key_pair_generate,
+ *                        @ref nrf_crypto_ecc_public_key_to_raw and @ref nrf_crypto_ecc_byte_order_invert
+ *                        functions.
+ */
+ret_code_t nrf_ble_lesc_keypair_generate(void);
+
+
+/**@brief   Function for generating LESC OOB data.
+ *
+ * @details This function generates LESC OOB data, which can be transmitted Out-Of-Band to the peer
+ *          device and used during LESC procedure. It is required to generate ECC keypair with @ref
+ *          nrf_ble_lesc_keypair_generate before calling this function.
+ *
+ * @retval NRF_SUCCESS               If the operation was successful.
+ * @retval NRF_ERROR_INVALID_STATE   If the ECC keypair hasn't been generated or is currently
+ *                                   being generated.
+ */
+ret_code_t nrf_ble_lesc_own_oob_data_generate(void);
+
+
+/**@brief   Function for accessing the ECC public key used for LESC DH key generation.
+ *
+ * @details This function can be used to access the ECC public key, which is required to generate a LESC DH key
+ *          at the peer side.
+ *
+ * @return Pointer to the generated public key or NULL if the key has not been generated yet.
+ */
+ble_gap_lesc_p256_pk_t * nrf_ble_lesc_public_key_get(void);
+
+
+/**@brief   Function for accessing LESC OOB data.
+ *
+ * @details This function can be used to access LESC OOB data that is associated with this device.
+ *          It is required to regenerate LESC OOB data with @ref nrf_ble_lesc_own_oob_data_generate,
+ *          after each change of ECC keypair with @ref nrf_ble_lesc_keypair_generate.
+ *
+ * @return Pointer to the LESC OOB data or NULL if the data has not been generated yet or is no
+ *         no longer valid.
+ */
+ble_gap_lesc_oob_data_t * nrf_ble_lesc_own_oob_data_get(void);
+
+
+/**@brief   Function for setting the handler used to retrieve peer OOB data.
+ *
+ * @param[in]   handler   Function to retrieve peer OOB data.
+ */
+void nrf_ble_lesc_peer_oob_data_handler_set(nrf_ble_lesc_peer_oob_data_handler handler);
+
+
+/**@brief   Function for responding to a DH key requests.
+ *
+ * @details This function calculates DH keys and supplies them to the SoftDevice if there are any
+ *          pending requests for keys.
+ *
+ * @note This function should be called systematically (e.g. in the main application loop) to handle
+ *       any pending DH key requests.
+ *
+ * @retval NRF_SUCCESS        If the operation was successful.
+ * @retval NRF_ERROR_INTERNAL If the LESC module encountered an internal error. The only way to recover from
+ *                            this type of error is to reset the application.
+ * @retval Other              Other error codes might be returned by the @ref nrf_crypto_ecdh_compute,
+ *                            @ref nrf_crypto_ecc_byte_order_invert, and @ref sd_ble_gap_lesc_dhkey_reply
+ *                            functions.
+ */
+ret_code_t nrf_ble_lesc_request_handler(void);
+
+
+/**@brief   Function for handling BLE stack events.
+ *
+ * @details This function handles events from the BLE stack that are of interest to the module.
+ *
+ * @param[in]   p_ble_evt   Event received from the BLE stack.
+ */
+void nrf_ble_lesc_on_ble_evt(ble_evt_t const * p_ble_evt);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_BLE_LESC_H__
+
+/** @} */

components/ble/peer_manager/peer_data_storage.c

@@ -0,0 +1,657 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "peer_data_storage.h"
+
+#include <stdint.h>
+#include <string.h>
+#include "sdk_errors.h"
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+#include "peer_id.h"
+#include "fds.h"
+
+#define NRF_LOG_MODULE_NAME peer_manager_pds
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+
+// Macro for verifying that the peer id is within a valid range.
+#define VERIFY_PEER_ID_IN_RANGE(id)         VERIFY_FALSE((id >= PM_PEER_ID_N_AVAILABLE_IDS), \
+                                            NRF_ERROR_INVALID_PARAM)
+
+// Macro for verifying that the peer data id is withing a valid range.
+#define VERIFY_PEER_DATA_ID_IN_RANGE(id)    VERIFY_TRUE(peer_data_id_is_valid(id), \
+                                            NRF_ERROR_INVALID_PARAM)
+
+// The number of registered event handlers.
+#define PDS_EVENT_HANDLERS_CNT              (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
+
+
+// Peer Data Storage event handler in Peer Database.
+extern void pdb_pds_evt_handler(pm_evt_t *);
+
+// Peer Data Storage events' handlers.
+// The number of elements in this array is PDS_EVENT_HANDLERS_CNT.
+static pm_evt_handler_internal_t const m_evt_handlers[] =
+{
+    pdb_pds_evt_handler,
+};
+
+static bool          m_module_initialized   = false;
+static volatile bool m_peer_delete_deferred = false;
+
+// A token used for Flash Data Storage searches.
+static fds_find_token_t m_fds_ftok;
+
+
+// Function for dispatching events to all registered event handlers.
+static void pds_evt_send(pm_evt_t * p_event)
+{
+    p_event->conn_handle = BLE_CONN_HANDLE_INVALID;
+
+    for (uint32_t i = 0; i < PDS_EVENT_HANDLERS_CNT; i++)
+    {
+        m_evt_handlers[i](p_event);
+    }
+}
+
+
+// Function to convert peer IDs to file IDs.
+static uint16_t peer_id_to_file_id(pm_peer_id_t peer_id)
+{
+    return (uint16_t)(peer_id + PEER_ID_TO_FILE_ID);
+}
+
+
+// Function to convert peer data id to type id.
+static pm_peer_id_t file_id_to_peer_id(uint16_t file_id)
+{
+    return (pm_peer_id_t)(file_id + FILE_ID_TO_PEER_ID);
+}
+
+
+// Function to convert peer data IDs to record keys.
+static uint16_t peer_data_id_to_record_key(pm_peer_data_id_t peer_data_id)
+{
+    return (uint16_t)(peer_data_id + DATA_ID_TO_RECORD_KEY);
+}
+
+
+// Function to convert record keys to peer data IDs.
+static pm_peer_data_id_t record_key_to_peer_data_id(uint16_t record_key)
+{
+    return (pm_peer_data_id_t)(record_key + RECORD_KEY_TO_DATA_ID);
+}
+
+
+// Function for checking whether a file ID is relevant for the Peer Manager.
+static bool file_id_within_pm_range(uint16_t file_id)
+{
+    return ((PDS_FIRST_RESERVED_FILE_ID <= file_id)
+         && (file_id <= PDS_LAST_RESERVED_FILE_ID));
+}
+
+
+// Function for checking whether a record key is relevant for the Peer Manager.
+static bool record_key_within_pm_range(uint16_t record_key)
+{
+    return ((PDS_FIRST_RESERVED_RECORD_KEY <= record_key)
+         && (record_key <= PDS_LAST_RESERVED_RECORD_KEY));
+}
+
+
+static bool peer_data_id_is_valid(pm_peer_data_id_t data_id)
+{
+    return ((data_id == PM_PEER_DATA_ID_BONDING)                 ||
+            (data_id == PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING) ||
+            (data_id == PM_PEER_DATA_ID_GATT_LOCAL)              ||
+            (data_id == PM_PEER_DATA_ID_GATT_REMOTE)             ||
+            (data_id == PM_PEER_DATA_ID_PEER_RANK)               ||
+            (data_id == PM_PEER_DATA_ID_CENTRAL_ADDR_RES)        ||
+            (data_id == PM_PEER_DATA_ID_APPLICATION));
+}
+
+
+/**@brief Function for sending a PM_EVT_ERROR_UNEXPECTED event.
+ *
+ * @param[in]  peer_id    The peer the event pertains to.
+ * @param[in]  err_code   The unexpected error that occurred.
+ */
+static void send_unexpected_error(pm_peer_id_t peer_id, ret_code_t err_code)
+{
+    pm_evt_t error_evt =
+    {
+        .evt_id = PM_EVT_ERROR_UNEXPECTED,
+        .peer_id = peer_id,
+        .params =
+        {
+            .error_unexpected =
+            {
+                .error = err_code,
+            }
+        }
+    };
+    pds_evt_send(&error_evt);
+}
+
+
+// Function for deleting all data beloning to a peer.
+// These operations will be sent to FDS one at a time.
+static void peer_data_delete_process()
+{
+    ret_code_t        ret;
+    pm_peer_id_t      peer_id;
+    uint16_t          file_id;
+    fds_record_desc_t desc;
+    fds_find_token_t  ftok;
+
+    m_peer_delete_deferred = false;
+
+    memset(&ftok, 0x00, sizeof(fds_find_token_t));
+    peer_id = peer_id_get_next_deleted(PM_PEER_ID_INVALID);
+
+    while (    (peer_id != PM_PEER_ID_INVALID)
+            && (fds_record_find_in_file(peer_id_to_file_id(peer_id), &desc, &ftok)
+            == FDS_ERR_NOT_FOUND))
+    {
+        peer_id_free(peer_id);
+        peer_id = peer_id_get_next_deleted(peer_id);
+    }
+
+    if (peer_id != PM_PEER_ID_INVALID)
+    {
+        file_id = peer_id_to_file_id(peer_id);
+        ret     = fds_file_delete(file_id);
+
+        if (ret == FDS_ERR_NO_SPACE_IN_QUEUES)
+        {
+            m_peer_delete_deferred = true;
+        }
+        else if (ret != NRF_SUCCESS)
+        {
+            NRF_LOG_ERROR("Could not delete peer data. fds_file_delete() returned 0x%x for peer_id: %d",
+                          ret,
+                          peer_id);
+            send_unexpected_error(peer_id, ret);
+        }
+    }
+}
+
+
+static ret_code_t peer_data_find(pm_peer_id_t              peer_id,
+                                 pm_peer_data_id_t         data_id,
+                                 fds_record_desc_t * const p_desc)
+{
+    ret_code_t       ret;
+    fds_find_token_t ftok;
+
+    NRF_PM_DEBUG_CHECK(peer_id < PM_PEER_ID_N_AVAILABLE_IDS);
+    NRF_PM_DEBUG_CHECK(peer_data_id_is_valid(data_id));
+    NRF_PM_DEBUG_CHECK(p_desc != NULL);
+
+    memset(&ftok, 0x00, sizeof(fds_find_token_t));
+
+    uint16_t file_id    = peer_id_to_file_id(peer_id);
+    uint16_t record_key = peer_data_id_to_record_key(data_id);
+
+    ret = fds_record_find(file_id, record_key, p_desc, &ftok);
+
+    if (ret != NRF_SUCCESS)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+static void peer_ids_load()
+{
+    fds_record_desc_t  record_desc;
+    fds_flash_record_t record;
+    fds_find_token_t   ftok;
+
+    memset(&ftok, 0x00, sizeof(fds_find_token_t));
+
+    uint16_t const record_key = peer_data_id_to_record_key(PM_PEER_DATA_ID_BONDING);
+
+    while (fds_record_find_by_key(record_key, &record_desc, &ftok) == NRF_SUCCESS)
+    {
+        pm_peer_id_t peer_id;
+
+        // It is safe to ignore the return value since the descriptor was
+        // just obtained and also 'record' is different from NULL.
+        (void)fds_record_open(&record_desc, &record);
+        peer_id = file_id_to_peer_id(record.p_header->file_id);
+        (void)fds_record_close(&record_desc);
+
+        (void)peer_id_allocate(peer_id);
+    }
+}
+
+
+static void fds_evt_handler(fds_evt_t const * const p_fds_evt)
+{
+    pm_evt_t pds_evt =
+    {
+        .peer_id = file_id_to_peer_id(p_fds_evt->write.file_id)
+    };
+
+    switch (p_fds_evt->id)
+    {
+        case FDS_EVT_WRITE:
+        case FDS_EVT_UPDATE:
+        case FDS_EVT_DEL_RECORD:
+            if (   file_id_within_pm_range(p_fds_evt->write.file_id)
+                || record_key_within_pm_range(p_fds_evt->write.record_key))
+            {
+                pds_evt.params.peer_data_update_succeeded.data_id
+                                = record_key_to_peer_data_id(p_fds_evt->write.record_key);
+                pds_evt.params.peer_data_update_succeeded.action
+                                = (p_fds_evt->id == FDS_EVT_DEL_RECORD) ? PM_PEER_DATA_OP_DELETE
+                                                                        : PM_PEER_DATA_OP_UPDATE;
+                pds_evt.params.peer_data_update_succeeded.token = p_fds_evt->write.record_id;
+
+                if (p_fds_evt->result == NRF_SUCCESS)
+                {
+                    pds_evt.evt_id = PM_EVT_PEER_DATA_UPDATE_SUCCEEDED;
+                    pds_evt.params.peer_data_update_succeeded.flash_changed = true;
+                }
+                else
+                {
+                    pds_evt.evt_id = PM_EVT_PEER_DATA_UPDATE_FAILED;
+                    pds_evt.params.peer_data_update_failed.error = p_fds_evt->result;
+                }
+
+                pds_evt_send(&pds_evt);
+            }
+            break;
+
+        case FDS_EVT_DEL_FILE:
+            if (    file_id_within_pm_range(p_fds_evt->del.file_id)
+                && (p_fds_evt->del.record_key == FDS_RECORD_KEY_DIRTY))
+            {
+                if (p_fds_evt->result == NRF_SUCCESS)
+                {
+                    pds_evt.evt_id = PM_EVT_PEER_DELETE_SUCCEEDED;
+                    peer_id_free(pds_evt.peer_id);
+                }
+                else
+                {
+                    pds_evt.evt_id = PM_EVT_PEER_DELETE_FAILED;
+                    pds_evt.params.peer_delete_failed.error = p_fds_evt->result;
+                }
+
+                m_peer_delete_deferred = true; // Trigger remaining deletes.
+
+                pds_evt_send(&pds_evt);
+            }
+            break;
+
+        case FDS_EVT_GC:
+            if (p_fds_evt->result == NRF_SUCCESS)
+            {
+                pds_evt.evt_id = PM_EVT_FLASH_GARBAGE_COLLECTED;
+            }
+            else
+            {
+                pds_evt.evt_id = PM_EVT_FLASH_GARBAGE_COLLECTION_FAILED;
+                pds_evt.params.garbage_collection_failed.error = p_fds_evt->result;
+            }
+            pds_evt.peer_id = PM_PEER_ID_INVALID;
+            pds_evt_send(&pds_evt);
+            break;
+
+        default:
+            // No action.
+            break;
+    }
+
+    if (m_peer_delete_deferred)
+    {
+        peer_data_delete_process();
+    }
+}
+
+
+ret_code_t pds_init()
+{
+    ret_code_t ret;
+
+    // Check for re-initialization if debugging.
+    NRF_PM_DEBUG_CHECK(!m_module_initialized);
+
+    ret = fds_register(fds_evt_handler);
+    if (ret != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Could not initialize flash storage. fds_register() returned 0x%x.", ret);
+        return NRF_ERROR_INTERNAL;
+    }
+
+    ret = fds_init();
+    if (ret != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Could not initialize flash storage. fds_init() returned 0x%x.", ret);
+        return NRF_ERROR_STORAGE_FULL;
+    }
+
+    peer_id_init();
+    peer_ids_load();
+
+    m_module_initialized = true;
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t pds_peer_data_read(pm_peer_id_t                    peer_id,
+                              pm_peer_data_id_t               data_id,
+                              pm_peer_data_t          * const p_data,
+                              uint32_t          const * const p_buf_len)
+{
+    ret_code_t         ret;
+    fds_record_desc_t  rec_desc;
+    fds_flash_record_t rec_flash;
+
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    NRF_PM_DEBUG_CHECK(p_data != NULL);
+
+    VERIFY_PEER_ID_IN_RANGE(peer_id);
+    VERIFY_PEER_DATA_ID_IN_RANGE(data_id);
+
+    ret = peer_data_find(peer_id, data_id, &rec_desc);
+
+    if (ret != NRF_SUCCESS)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    // Shouldn't fail, unless the record was deleted in the meanwhile or the CRC check has failed.
+    ret = fds_record_open(&rec_desc, &rec_flash);
+
+    if (ret != NRF_SUCCESS)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    p_data->data_id      = data_id;
+    p_data->length_words = rec_flash.p_header->length_words;
+
+    // If p_buf_len is NULL, provide a pointer to data in flash, otherwise,
+    // check that the buffer is large enough and copy the data in flash into the buffer.
+    if (p_buf_len == NULL)
+    {
+        // The cast is necessary because if no buffer is provided, we just copy the pointer,
+        // but in that case it should be considered a pointer to const data by the caller,
+        // since it is a pointer to data in flash.
+        p_data->p_all_data = (void*)rec_flash.p_data;
+    }
+    else
+    {
+        uint32_t const data_len_bytes = (p_data->length_words * sizeof(uint32_t));
+        uint32_t const copy_len_bytes = MIN((*p_buf_len), (p_data->length_words * sizeof(uint32_t)));
+
+        memcpy(p_data->p_all_data, rec_flash.p_data, copy_len_bytes);
+
+        if (copy_len_bytes < data_len_bytes)
+        {
+            return NRF_ERROR_DATA_SIZE;
+        }
+    }
+
+    // Shouldn't fail unless the record was already closed, in which case it can be ignored.
+    (void)fds_record_close(&rec_desc);
+
+    return NRF_SUCCESS;
+}
+
+
+void pds_peer_data_iterate_prepare(void)
+{
+    memset(&m_fds_ftok, 0x00, sizeof(fds_find_token_t));
+}
+
+
+bool pds_peer_data_iterate(pm_peer_data_id_t            data_id,
+                           pm_peer_id_t         * const p_peer_id,
+                           pm_peer_data_flash_t * const p_data)
+{
+    ret_code_t         ret;
+    uint16_t           rec_key;
+    fds_record_desc_t  rec_desc;
+    fds_flash_record_t rec_flash;
+
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    NRF_PM_DEBUG_CHECK(p_peer_id != NULL);
+    NRF_PM_DEBUG_CHECK(p_data    != NULL);
+
+    VERIFY_PEER_DATA_ID_IN_RANGE(data_id);
+
+    rec_key = peer_data_id_to_record_key(data_id);
+
+    if (fds_record_find_by_key(rec_key, &rec_desc, &m_fds_ftok) != NRF_SUCCESS)
+    {
+        return false;
+    }
+
+    ret = fds_record_open(&rec_desc, &rec_flash);
+
+    if (ret != NRF_SUCCESS)
+    {
+        // It can only happen if the record was deleted after the call to fds_record_find_by_key(),
+        // before we could open it, or if CRC support was enabled in Flash Data Storage at compile
+        // time and the CRC check failed.
+        return false;
+    }
+
+    p_data->data_id      = data_id;
+    p_data->length_words = rec_flash.p_header->length_words;
+    p_data->p_all_data   = rec_flash.p_data;
+
+    *p_peer_id           = file_id_to_peer_id(rec_flash.p_header->file_id);
+
+    (void)fds_record_close(&rec_desc);
+
+    return true;
+}
+
+
+ret_code_t pds_peer_data_store(pm_peer_id_t                 peer_id,
+                               pm_peer_data_const_t const * p_peer_data,
+                               pm_store_token_t           * p_store_token)
+{
+    ret_code_t         ret;
+    fds_record_t       rec;
+    fds_record_desc_t  rec_desc;
+
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    NRF_PM_DEBUG_CHECK(p_peer_data != NULL);
+
+    VERIFY_PEER_ID_IN_RANGE(peer_id);
+    VERIFY_PEER_DATA_ID_IN_RANGE(p_peer_data->data_id);
+
+    // Prepare the record to be stored in flash.
+    rec.file_id           = peer_id_to_file_id(peer_id);
+    rec.key               = peer_data_id_to_record_key(p_peer_data->data_id);
+    rec.data.p_data       = (void*)p_peer_data->p_all_data;
+    rec.data.length_words = p_peer_data->length_words;
+
+    ret = peer_data_find(peer_id, p_peer_data->data_id, &rec_desc);
+
+    if (ret == NRF_ERROR_NOT_FOUND)
+    {
+        ret = fds_record_write(&rec_desc, &rec);
+    }
+    else // NRF_SUCCESS
+    {
+        // Update existing record.
+        ret = fds_record_update(&rec_desc, &rec);
+    }
+
+    switch (ret)
+    {
+        case NRF_SUCCESS:
+            if (p_store_token != NULL)
+            {
+                // Update the store token.
+                (void)fds_record_id_from_desc(&rec_desc, (uint32_t*)p_store_token);
+            }
+            return NRF_SUCCESS;
+
+        case FDS_ERR_BUSY:
+        case FDS_ERR_NO_SPACE_IN_QUEUES:
+            return NRF_ERROR_BUSY;
+
+        case FDS_ERR_NO_SPACE_IN_FLASH:
+            return NRF_ERROR_STORAGE_FULL;
+
+        case FDS_ERR_UNALIGNED_ADDR:
+            return NRF_ERROR_INVALID_ADDR;
+
+        default:
+            NRF_LOG_ERROR("Could not write data to flash. fds_record_{write|update}() returned 0x%x. "\
+                          "peer_id: %d",
+                          ret,
+                          peer_id);
+            return NRF_ERROR_INTERNAL;
+    }
+}
+
+
+ret_code_t pds_peer_data_delete(pm_peer_id_t peer_id, pm_peer_data_id_t data_id)
+{
+    ret_code_t        ret;
+    fds_record_desc_t record_desc;
+
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    VERIFY_PEER_ID_IN_RANGE(peer_id);
+    VERIFY_PEER_DATA_ID_IN_RANGE(data_id);
+
+    ret = peer_data_find(peer_id, data_id, &record_desc);
+
+    if (ret != NRF_SUCCESS)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    ret = fds_record_delete(&record_desc);
+
+    switch (ret)
+    {
+        case NRF_SUCCESS:
+            return NRF_SUCCESS;
+
+        case FDS_ERR_NO_SPACE_IN_QUEUES:
+            return NRF_ERROR_BUSY;
+
+        default:
+            NRF_LOG_ERROR("Could not delete peer. fds_record_delete() returned 0x%x. peer_id: %d, "\
+                          "data_id: %d.",
+                          ret,
+                          peer_id,
+                          data_id);
+            return NRF_ERROR_INTERNAL;
+    }
+}
+
+
+pm_peer_id_t pds_peer_id_allocate(void)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    return peer_id_allocate(PM_PEER_ID_INVALID);
+}
+
+
+ret_code_t pds_peer_id_free(pm_peer_id_t peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    VERIFY_PEER_ID_IN_RANGE(peer_id);
+
+    (void)peer_id_delete(peer_id);
+    peer_data_delete_process();
+
+    return NRF_SUCCESS;
+}
+
+
+bool pds_peer_id_is_allocated(pm_peer_id_t peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    return peer_id_is_allocated(peer_id);
+}
+
+
+bool pds_peer_id_is_deleted(pm_peer_id_t peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    return peer_id_is_deleted(peer_id);
+}
+
+
+pm_peer_id_t pds_next_peer_id_get(pm_peer_id_t prev_peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    return peer_id_get_next_used(prev_peer_id);
+}
+
+
+pm_peer_id_t pds_next_deleted_peer_id_get(pm_peer_id_t prev_peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    return peer_id_get_next_deleted(prev_peer_id);
+}
+
+
+uint32_t pds_peer_count_get(void)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    return peer_id_n_ids();
+}
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/peer_data_storage.h

@@ -0,0 +1,251 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef PEER_DATA_STORAGE_H__
+#define PEER_DATA_STORAGE_H__
+
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup peer_data_storage Peer Data Storage
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. This module provides a Peer Manager-specific API
+ *        to the persistent storage.
+ *
+ * @details This module uses Flash Data Storage (FDS) to interface with persistent storage.
+ */
+
+#define PDS_FIRST_RESERVED_FILE_ID    (0xC000)  /**< The beginning of the range of file IDs reserved for Peer Manager. */
+#define PDS_LAST_RESERVED_FILE_ID     (0xFFFE)  /**< The end of the range of file IDs reserved for Peer Manager. */
+#define PDS_FIRST_RESERVED_RECORD_KEY (0xC000)  /**< The beginning of the range of record keys reserved for Peer Manager. */
+#define PDS_LAST_RESERVED_RECORD_KEY  (0xFFFE)  /**< The end of the range of record keys reserved for Peer Manager. */
+
+#define PEER_ID_TO_FILE_ID            ( PDS_FIRST_RESERVED_FILE_ID)    //!< Macro for converting a @ref pm_peer_id_t to an FDS file ID.
+#define FILE_ID_TO_PEER_ID            (-PDS_FIRST_RESERVED_FILE_ID)    //!< Macro for converting an FDS file ID to a @ref pm_peer_id_t.
+#define DATA_ID_TO_RECORD_KEY         ( PDS_FIRST_RESERVED_RECORD_KEY) //!< Macro for converting a @ref pm_peer_data_id_t to an FDS record ID.
+#define RECORD_KEY_TO_DATA_ID         (-PDS_FIRST_RESERVED_RECORD_KEY) //!< Macro for converting an FDS record ID to a @ref pm_peer_data_id_t.
+
+
+/**@brief Function for initializing the module.
+ *
+ * @retval NRF_SUCCESS             If initialization was successful.
+ * @retval NRF_ERROR_STORAGE_FULL  If no flash pages were available for use.
+ * @retval NRF_ERROR_INTERNAL      If the module couldn't register with the flash filesystem.
+ */
+ret_code_t pds_init(void);
+
+
+/**@brief Function for reading peer data in flash.
+ *
+ * @param[in]  peer_id     The peer the data belongs to.
+ * @param[in]  data_id     The data to retrieve.
+ * @param[out] p_data      The peer data. May not be @c NULL. p_data.length_words and p_data.data_id
+ *                         are ignored. p_data.p_all_data is ignored if @p p_buf_len is @c NULL.
+ * @param[in]  p_buf_len   Length of the provided buffer, in bytes. Pass @c NULL to only copy
+ *                         a pointer to the data in flash.
+ *
+ * @retval NRF_SUCCESS              If the operation was successful.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p peer_id or @p data_id are invalid.
+ * @retval NRF_ERROR_NOT_FOUND      If the data was not found in flash.
+ * @retval NRF_ERROR_DATA_SIZE      If the provided buffer is too small. The data is still copied,
+ *                                  filling the provided buffer.
+ */
+ret_code_t pds_peer_data_read(pm_peer_id_t                    peer_id,
+                              pm_peer_data_id_t               data_id,
+                              pm_peer_data_t          * const p_data,
+                              uint32_t          const * const p_buf_len);
+
+
+/**@brief Function to prepare iterating over peer data in flash using @ref pds_peer_data_iterate.
+ *        Call this function once each time before iterating using @ref pds_peer_data_iterate.
+ */
+void pds_peer_data_iterate_prepare(void);
+
+
+/**@brief Function for iterating peers' data in flash.
+ *        Always call @ref pds_peer_data_iterate_prepare before starting iterating.
+ *
+ * @param[in]  data_id    The peer data to iterate over.
+ * @param[out] p_peer_id  The peer the data belongs to.
+ * @param[out] p_data     The peer data in flash.
+ *
+ * @retval true   If the operation was successful.
+ * @retval false  If the data was not found in flash, or another error occurred.
+ */
+bool pds_peer_data_iterate(pm_peer_data_id_t            data_id,
+                           pm_peer_id_t         * const p_peer_id,
+                           pm_peer_data_flash_t * const p_data);
+
+
+/**@brief Function for storing peer data in flash. If the same piece of data already exists for the
+ *        given peer, it will be updated. This operation is asynchronous.
+ *        Expect a @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED or @ref PM_EVT_PEER_DATA_UPDATE_FAILED
+ *        event.
+ *
+ * @param[in]  peer_id        The peer the data belongs to.
+ * @param[in]  p_peer_data    The peer data. May not be @c NULL.
+ * @param[out] p_store_token  A token identifying this particular store operation. The token can be
+ *                            used to identify events pertaining to this operation. Pass @p NULL
+ *                            if not used.
+ *
+ * @retval NRF_SUCCESS              If the operation was initiated successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p peer_id or the data ID in @p_peer_data are invalid.
+ * @retval NRF_ERROR_INVALID_ADDR   If @p p_peer_data is not word-aligned.
+ * @retval NRF_ERROR_STORAGE_FULL   If no space is available in flash.
+ * @retval NRF_ERROR_BUSY           If the flash filesystem was busy.
+ * @retval NRF_ERROR_INTERNAL       If an unexpected error occurred.
+ */
+ret_code_t pds_peer_data_store(pm_peer_id_t                 peer_id,
+                               pm_peer_data_const_t const * p_peer_data,
+                               pm_store_token_t           * p_store_token);
+
+
+/**@brief Function for deleting peer data in flash. This operation is asynchronous.
+ *        Expect a @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED or @ref PM_EVT_PEER_DATA_UPDATE_FAILED
+ *        event.
+ *
+ * @param[in]  peer_id  The peer the data belongs to
+ * @param[in]  data_id  The data to delete.
+ *
+ * @retval NRF_SUCCESS              If the operation was initiated successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p peer_id or @p data_id are invalid.
+ * @retval NRF_ERROR_NOT_FOUND      If data was not found in flash.
+ * @retval NRF_ERROR_BUSY           If the flash filesystem was busy.
+ * @retval NRF_ERROR_INTERNAL       If an unexpected error occurred.
+ */
+ret_code_t pds_peer_data_delete(pm_peer_id_t peer_id, pm_peer_data_id_t data_id);
+
+
+/**@brief Function for claiming an unused peer ID.
+ *
+ * @retval  PM_PEER_ID_INVALID  If no peer ID was available.
+ */
+pm_peer_id_t pds_peer_id_allocate(void);
+
+
+/**@brief Function for freeing a peer ID and deleting all data associated with it in flash.
+ *
+ * @param[in]  peer_id  The ID of the peer to free.
+ *
+ * @retval NRF_SUCCESS             The operation was initiated successfully.
+ * @retval NRF_ERROR_INVALID_PARAM If @p peer_id is invalid.
+ */
+ret_code_t pds_peer_id_free(pm_peer_id_t peer_id);
+
+
+/**@brief Function for finding out whether a peer ID is in use.
+ *
+ * @param[in]  peer_id  The peer ID to inquire about.
+ *
+ * @retval  true   @p peer_id is in use.
+ * @retval  false  @p peer_id is free.
+ */
+bool pds_peer_id_is_allocated(pm_peer_id_t peer_id);
+
+
+/**@brief Function for finding out whether a peer ID is marked for deletion.
+ *
+ * @param[in]  peer_id  The peer ID to inquire about.
+ *
+ * @retval  true   @p peer_id is marked for deletion.
+ * @retval  false  @p peer_id is not marked for deletion.
+ */
+bool pds_peer_id_is_deleted(pm_peer_id_t peer_id);
+
+/**@brief Function for getting the next peer ID in the sequence of all used peer IDs. Can be
+ *        used to loop through all used peer IDs.
+ *
+ * @note @ref PM_PEER_ID_INVALID is considered to be before the first and after the last ordinary
+ *       peer ID.
+ *
+ * @param[in]  prev_peer_id  The previous peer ID.
+ *
+ * @return  The first ordinary peer ID  If @p prev_peer_id is @ref PM_PEER_ID_INVALID.
+ * @retval  PM_PEER_ID_INVALID          If @p prev_peer_id is the last ordinary peer ID or the module
+ *                                      is not initialized.
+ */
+pm_peer_id_t pds_next_peer_id_get(pm_peer_id_t prev_peer_id);
+
+
+/**@brief Function for getting the next peer ID in the sequence of all peer IDs pending deletion.
+ *        Can be used to loop through all used peer IDs.
+ *
+ * @note @ref PM_PEER_ID_INVALID is considered to be before the first and after the last ordinary
+ *       peer ID.
+ *
+ * @param[in]  prev_peer_id  The previous peer ID.
+ *
+ * @return  The next peer ID pending deletion.
+ * @return  The first ordinary peer ID  if prev_peer_id was @ref PM_PEER_ID_INVALID.
+ * @retval  PM_PEER_ID_INVALID          if prev_peer_id was the last ordinary peer ID or the module
+ *                                      is not initialized.
+ */
+pm_peer_id_t pds_next_deleted_peer_id_get(pm_peer_id_t prev_peer_id);
+
+
+/**@brief Function for querying the number of valid peer IDs available. I.E the number of peers
+ *        in persistent storage.
+ *
+ * @return  The number of valid peer IDs.
+ */
+uint32_t pds_peer_count_get(void);
+
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PEER_DATA_STORAGE_H__ */

components/ble/peer_manager/peer_database.c

@@ -0,0 +1,726 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "peer_database.h"
+
+#include <string.h>
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+#include "peer_data_storage.h"
+#include "pm_buffer.h"
+
+#define NRF_LOG_MODULE_NAME peer_manager_pdb
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+#include "nrf_strerror.h"
+
+/**@brief Macro for verifying that the data ID is among the values eligible for using the write buffer.
+ *
+ * @param[in] data_id  The data ID to verify.
+ */
+// @note emdi: could this maybe be a function?
+#define VERIFY_DATA_ID_WRITE_BUF(data_id)                                                    \
+do                                                                                           \
+{                                                                                            \
+    if (((data_id) != PM_PEER_DATA_ID_BONDING) && ((data_id) != PM_PEER_DATA_ID_GATT_LOCAL)) \
+    {                                                                                        \
+        return NRF_ERROR_INVALID_PARAM;                                                      \
+    }                                                                                        \
+} while (0)
+
+
+// The number of registered event handlers.
+#define PDB_EVENT_HANDLERS_CNT      (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
+
+
+// Peer Database event handlers in other Peer Manager submodules.
+extern void pm_pdb_evt_handler(pm_evt_t * p_event);
+extern void sm_pdb_evt_handler(pm_evt_t * p_event);
+#if !defined(PM_SERVICE_CHANGED_ENABLED) || (PM_SERVICE_CHANGED_ENABLED == 1)
+extern void gscm_pdb_evt_handler(pm_evt_t * p_event);
+#endif
+extern void gcm_pdb_evt_handler(pm_evt_t * p_event);
+
+// Peer Database events' handlers.
+// The number of elements in this array is PDB_EVENT_HANDLERS_CNT.
+static pm_evt_handler_internal_t const m_evt_handlers[] =
+{
+    pm_pdb_evt_handler,
+    sm_pdb_evt_handler,
+#if !defined(PM_SERVICE_CHANGED_ENABLED) || (PM_SERVICE_CHANGED_ENABLED == 1)
+    gscm_pdb_evt_handler,
+#endif
+    gcm_pdb_evt_handler,
+};
+
+
+/**@brief Struct for keeping track of one write buffer, from allocation, until it is fully written
+ *        or cancelled.
+ */
+typedef struct
+{
+    pm_peer_id_t        peer_id;               /**< The peer ID this buffer belongs to. */
+    pm_peer_data_id_t   data_id;               /**< The data ID this buffer belongs to. */
+    pm_store_token_t    store_token;           /**< Token given by Peer Data Storage when a flash write has been successfully requested. This is used as the check for whether such an operation has been successfully requested. */
+    uint8_t             n_bufs;                /**< The number of buffer blocks containing peer data. */
+    uint8_t             buffer_block_id;       /**< The index of the first (or only) buffer block containing peer data. */
+    uint8_t             store_flash_full : 1;  /**< Flag indicating that the buffer was attempted written to flash, but a flash full error was returned and the operation should be retried after room has been made. */
+    uint8_t             store_busy       : 1;  /**< Flag indicating that the buffer was attempted written to flash, but a busy error was returned and the operation should be retried. */
+} pdb_buffer_record_t;
+
+
+static bool                m_module_initialized;
+static pm_buffer_t         m_write_buffer;                                 /**< The internal states of the write buffer. */
+static pdb_buffer_record_t m_write_buffer_records[PM_FLASH_BUFFERS];       /**< The available write buffer records. */
+static bool                m_pending_store = false;                        /**< Whether there are any pending (Not yet successfully requested in Peer Data Storage) store operations. This flag is for convenience only. The real bookkeeping is in the records (@ref m_write_buffer_records). */
+
+
+
+/**@brief Function for invalidating a record of a write buffer allocation.
+ *
+ * @param[in]  p_record  The record to invalidate.
+ */
+static void write_buffer_record_invalidate(pdb_buffer_record_t * p_record)
+{
+    p_record->peer_id          = PM_PEER_ID_INVALID;
+    p_record->data_id          = PM_PEER_DATA_ID_INVALID;
+    p_record->buffer_block_id  = PM_BUFFER_INVALID_ID;
+    p_record->store_busy       = false;
+    p_record->store_flash_full = false;
+    p_record->n_bufs           = 0;
+    p_record->store_token      = PM_STORE_TOKEN_INVALID;
+}
+
+
+/**@brief Function for finding a record of a write buffer allocation.
+ *
+ * @param[in]    peer_id  The peer ID in the record.
+ * @param[inout] p_index  In: The starting index, out: The index of the record
+ *
+ * @return  A pointer to the matching record, or NULL if none was found.
+ */
+static pdb_buffer_record_t * write_buffer_record_find_next(pm_peer_id_t peer_id, uint32_t * p_index)
+{
+    for (uint32_t i = *p_index; i < PM_FLASH_BUFFERS; i++)
+    {
+        if ((m_write_buffer_records[i].peer_id == peer_id))
+        {
+            *p_index = i;
+            return &m_write_buffer_records[i];
+        }
+    }
+    return NULL;
+}
+
+
+/**@brief Function for finding a record of a write buffer allocation.
+ *
+ * @param[in]  peer_id  The peer ID in the record.
+ * @param[in]  data_id  The data ID in the record.
+ *
+ * @return  A pointer to the matching record, or NULL if none was found.
+ */
+static pdb_buffer_record_t * write_buffer_record_find(pm_peer_id_t      peer_id,
+                                                      pm_peer_data_id_t data_id)
+{
+    uint32_t index = 0;
+    pdb_buffer_record_t * p_record = write_buffer_record_find_next(peer_id, &index);
+
+    while ((p_record != NULL) && ( (p_record->data_id != data_id)
+                                || (p_record->store_busy)
+                                || (p_record->store_flash_full)
+                                || (p_record->store_token != PM_STORE_TOKEN_INVALID)))
+    {
+        index++;
+        p_record = write_buffer_record_find_next(peer_id, &index);
+    }
+
+    return p_record;
+}
+
+
+/**@brief Function for finding a record of a write buffer allocation that has been sent to be stored.
+ *
+ * @param[in]  store_token  The store token received when store was called for the record.
+ *
+ * @return  A pointer to the matching record, or NULL if none was found.
+ */
+static pdb_buffer_record_t * write_buffer_record_find_stored(pm_store_token_t store_token)
+{
+    for (int i = 0; i < PM_FLASH_BUFFERS; i++)
+    {
+        if (m_write_buffer_records[i].store_token == store_token)
+        {
+            return &m_write_buffer_records[i];
+        }
+    }
+    return NULL;
+}
+
+
+/**@brief Function for finding an available record for write buffer allocation.
+ *
+ * @return  A pointer to the available record, or NULL if none was found.
+ */
+static pdb_buffer_record_t * write_buffer_record_find_unused(void)
+{
+    return write_buffer_record_find(PM_PEER_ID_INVALID, PM_PEER_DATA_ID_INVALID);
+}
+
+
+/**@brief Function for gracefully deactivating a write buffer record.
+ *
+ * @details This function will first release any buffers, then invalidate the record.
+ *
+ * @param[inout] p_write_buffer_record  The record to release.
+ *
+ * @return  A pointer to the matching record, or NULL if none was found.
+ */
+static void write_buffer_record_release(pdb_buffer_record_t * p_write_buffer_record)
+{
+    for (uint32_t i = 0; i < p_write_buffer_record->n_bufs; i++)
+    {
+        pm_buffer_release(&m_write_buffer, p_write_buffer_record->buffer_block_id + i);
+    }
+
+    write_buffer_record_invalidate(p_write_buffer_record);
+}
+
+
+/**@brief Function for claiming and activating a write buffer record.
+ *
+ * @param[out] pp_write_buffer_record  The claimed record.
+ * @param[in]  peer_id                 The peer ID this record should have.
+ * @param[in]  data_id                 The data ID this record should have.
+ */
+static void write_buffer_record_acquire(pdb_buffer_record_t ** pp_write_buffer_record,
+                                        pm_peer_id_t           peer_id,
+                                        pm_peer_data_id_t      data_id)
+{
+    if (pp_write_buffer_record == NULL)
+    {
+        return;
+    }
+    *pp_write_buffer_record = write_buffer_record_find_unused();
+    if (*pp_write_buffer_record == NULL)
+    {
+        // This also means the buffer is full.
+        return;
+    }
+    (*pp_write_buffer_record)->peer_id = peer_id;
+    (*pp_write_buffer_record)->data_id = data_id;
+}
+
+
+/**@brief Function for dispatching outbound events to all registered event handlers.
+ *
+ * @param[in]  p_event  The event to dispatch.
+ */
+static void pdb_evt_send(pm_evt_t * p_event)
+{
+    for (uint32_t i = 0; i < PDB_EVENT_HANDLERS_CNT; i++)
+    {
+        m_evt_handlers[i](p_event);
+    }
+}
+
+
+/**@brief Function for resetting the internal state of the Peer Database module.
+ *
+ * @param[out] p_event  The event to dispatch.
+ */
+static void internal_state_reset()
+{
+    for (uint32_t i = 0; i < PM_FLASH_BUFFERS; i++)
+    {
+        write_buffer_record_invalidate(&m_write_buffer_records[i]);
+    }
+}
+
+
+static void peer_data_point_to_buffer(pm_peer_data_t * p_peer_data, pm_peer_data_id_t data_id, uint8_t * p_buffer_memory, uint16_t n_bufs)
+{
+    uint16_t n_bytes = n_bufs * PDB_WRITE_BUF_SIZE;
+    p_peer_data->data_id    = data_id;
+
+    p_peer_data->p_all_data   = (pm_peer_data_bonding_t *)p_buffer_memory;
+    p_peer_data->length_words = BYTES_TO_WORDS(n_bytes);
+}
+
+
+static void peer_data_const_point_to_buffer(pm_peer_data_const_t * p_peer_data, pm_peer_data_id_t data_id,  uint8_t * p_buffer_memory, uint32_t n_bufs)
+{
+    peer_data_point_to_buffer((pm_peer_data_t*)p_peer_data, data_id, p_buffer_memory, n_bufs);
+}
+
+
+static void write_buf_length_words_set(pm_peer_data_const_t * p_peer_data)
+{
+    switch (p_peer_data->data_id)
+    {
+        case PM_PEER_DATA_ID_BONDING:
+            p_peer_data->length_words = PM_BONDING_DATA_N_WORDS();
+            break;
+        case PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING:
+            p_peer_data->length_words = PM_SC_STATE_N_WORDS();
+            break;
+        case PM_PEER_DATA_ID_PEER_RANK:
+            p_peer_data->length_words = PM_USAGE_INDEX_N_WORDS();
+            break;
+        case PM_PEER_DATA_ID_GATT_LOCAL:
+            p_peer_data->length_words = PM_LOCAL_DB_N_WORDS(p_peer_data->p_local_gatt_db->len);
+            break;
+        default:
+            // No action needed.
+            break;
+    }
+}
+
+
+/**@brief Function for writing data into persistent storage. Writing happens asynchronously.
+ *
+ * @note This will unlock the data after it has been written.
+ *
+ * @param[in]  p_write_buffer_record  The write buffer record to write into persistent storage.
+ *
+ * @retval NRF_SUCCESS              Data storing was successfully started.
+ * @retval NRF_ERROR_STORAGE_FULL   No space available in persistent storage. Please clear some
+ *                                  space, the operation will be reattempted after the next compress
+ *                                  procedure.
+ * @retval NRF_ERROR_INVALID_PARAM  Data ID was invalid.
+ * @retval NRF_ERROR_INVALID_STATE  Module is not initialized.
+ * @retval NRF_ERROR_INTERNAL       Unexpected internal error.
+ */
+ret_code_t write_buf_store(pdb_buffer_record_t * p_write_buffer_record)
+{
+    ret_code_t            err_code = NRF_SUCCESS;
+    pm_peer_data_const_t  peer_data = {.data_id = p_write_buffer_record->data_id};
+    uint8_t             * p_buffer_memory;
+
+    p_buffer_memory = pm_buffer_ptr_get(&m_write_buffer, p_write_buffer_record->buffer_block_id);
+    if (p_buffer_memory == NULL)
+    {
+        NRF_LOG_ERROR("pm_buffer_ptr_get() could not retrieve RAM buffer. block_id: %d",
+                      p_write_buffer_record->buffer_block_id);
+        return NRF_ERROR_INTERNAL;
+    }
+
+    peer_data_const_point_to_buffer(&peer_data,
+                                     p_write_buffer_record->data_id,
+                                     p_buffer_memory,
+                                     p_write_buffer_record->n_bufs);
+    write_buf_length_words_set(&peer_data);
+
+    err_code = pds_peer_data_store(p_write_buffer_record->peer_id,
+                                   &peer_data,
+                                   &p_write_buffer_record->store_token);
+
+
+    switch (err_code)
+    {
+        case NRF_SUCCESS:
+            p_write_buffer_record->store_busy       = false;
+            p_write_buffer_record->store_flash_full = false;
+            break;
+
+        case NRF_ERROR_BUSY:
+            p_write_buffer_record->store_busy       = true;
+            p_write_buffer_record->store_flash_full = false;
+            m_pending_store                         = true;
+
+            err_code = NRF_SUCCESS;
+            break;
+
+        case NRF_ERROR_STORAGE_FULL:
+            p_write_buffer_record->store_busy       = false;
+            p_write_buffer_record->store_flash_full = true;
+            m_pending_store                         = true;
+            break;
+
+        case NRF_ERROR_INVALID_PARAM:
+            // No action.
+            break;
+
+        default:
+            NRF_LOG_ERROR("pds_peer_data_store() returned %s. peer_id: %d",
+                          nrf_strerror_get(err_code),
+                          p_write_buffer_record->peer_id);
+            err_code = NRF_ERROR_INTERNAL;
+            break;
+    }
+
+    return err_code;
+}
+
+
+/**@brief This calls @ref write_buf_store and sends events based on the return value.
+ *
+ * See @ref write_buf_store for more info.
+ *
+ * @return  Whether or not the store operation succeeded.
+ */
+static bool write_buf_store_in_event(pdb_buffer_record_t * p_write_buffer_record)
+{
+    ret_code_t err_code;
+    pm_evt_t   event;
+
+    err_code = write_buf_store(p_write_buffer_record);
+    if (err_code != NRF_SUCCESS)
+    {
+        event.conn_handle = BLE_CONN_HANDLE_INVALID;
+        event.peer_id     = p_write_buffer_record->peer_id;
+
+        if (err_code == NRF_ERROR_STORAGE_FULL)
+        {
+            event.evt_id = PM_EVT_STORAGE_FULL;
+        }
+        else
+        {
+            event.evt_id                        = PM_EVT_ERROR_UNEXPECTED;
+            event.params.error_unexpected.error = err_code;
+
+            NRF_LOG_ERROR("Some peer data was not properly written to flash. write_buf_store() "\
+                          "returned %s for peer_id: %d",
+                          nrf_strerror_get(err_code),
+                          p_write_buffer_record->peer_id);
+        }
+
+        pdb_evt_send(&event);
+
+        return false;
+    }
+
+    return true;
+}
+
+
+/**@brief This reattempts store operations on write buffers, that previously failed because of @ref
+ *        NRF_ERROR_BUSY or @ref NRF_ERROR_STORAGE_FULL errors.
+ *
+ * param[in]  retry_flash_full  Whether to retry operations that failed because of an
+ *                              @ref NRF_ERROR_STORAGE_FULL error.
+ */
+static void reattempt_previous_operations(bool retry_flash_full)
+{
+    if (!m_pending_store)
+    {
+        return;
+    }
+
+    m_pending_store = false;
+
+    for (uint32_t i = 0; i < PM_FLASH_BUFFERS; i++)
+    {
+        if  ((m_write_buffer_records[i].store_busy)
+          || (m_write_buffer_records[i].store_flash_full))
+        {
+            m_pending_store = true;
+
+            if (m_write_buffer_records[i].store_busy || retry_flash_full)
+            {
+                if (!write_buf_store_in_event(&m_write_buffer_records[i]))
+                {
+                    return;
+                }
+            }
+        }
+    }
+}
+
+
+/**@brief Function for handling events from the Peer Data Storage module.
+ *        This function is extern in Peer Data Storage.
+ *
+ * @param[in]  p_event  The event to handle.
+ */
+void pdb_pds_evt_handler(pm_evt_t * p_event)
+{
+    pdb_buffer_record_t * p_write_buffer_record;
+    bool                  evt_send         = true;
+    bool                  retry_flash_full = false;
+
+    p_write_buffer_record = write_buffer_record_find_stored(p_event->params.peer_data_update_succeeded.token);
+
+    switch (p_event->evt_id)
+    {
+        case PM_EVT_PEER_DATA_UPDATE_SUCCEEDED:
+            if (   (p_event->params.peer_data_update_succeeded.action == PM_PEER_DATA_OP_UPDATE)
+                && (p_write_buffer_record != NULL))
+            {
+                // The write came from PDB.
+                write_buffer_record_release(p_write_buffer_record);
+            }
+            break;
+
+        case PM_EVT_PEER_DATA_UPDATE_FAILED:
+            if (   (p_event->params.peer_data_update_succeeded.action == PM_PEER_DATA_OP_UPDATE)
+                && (p_write_buffer_record != NULL))
+            {
+                // The write came from PDB, retry.
+                p_write_buffer_record->store_token = PM_STORE_TOKEN_INVALID;
+                p_write_buffer_record->store_busy  = true;
+                m_pending_store                    = true;
+                evt_send                           = false;
+            }
+            break;
+
+        case PM_EVT_FLASH_GARBAGE_COLLECTED:
+            retry_flash_full = true;
+            break;
+
+        default:
+            break;
+    }
+
+    if (evt_send)
+    {
+        // Forward the event to all registered Peer Database event handlers.
+        pdb_evt_send(p_event);
+    }
+
+    reattempt_previous_operations(retry_flash_full);
+}
+
+
+ret_code_t pdb_init()
+{
+    ret_code_t err_code;
+
+    NRF_PM_DEBUG_CHECK(!m_module_initialized);
+
+    internal_state_reset();
+
+    PM_BUFFER_INIT(&m_write_buffer, PM_FLASH_BUFFERS, PDB_WRITE_BUF_SIZE, err_code);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("PM_BUFFER_INIT() returned %s.", nrf_strerror_get(err_code));
+        return NRF_ERROR_INTERNAL;
+    }
+
+    m_module_initialized = true;
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t pdb_peer_free(pm_peer_id_t peer_id)
+{
+    ret_code_t err_code;
+
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    uint32_t index = 0;
+    pdb_buffer_record_t * p_record = write_buffer_record_find_next(peer_id, &index);
+
+    while (p_record != NULL)
+    {
+        err_code = pdb_write_buf_release(peer_id, p_record->data_id);
+        UNUSED_VARIABLE(err_code); // All return values are acceptable.
+
+        if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_NOT_FOUND))
+        {
+            NRF_LOG_ERROR("pdb_write_buf_release() returned %s which should not happen. peer_id: %d, "\
+                          "data_id: %d",
+                          nrf_strerror_get(err_code),
+                          peer_id,
+                          p_record->data_id);
+            return NRF_ERROR_INTERNAL;
+        }
+
+        index++;
+        p_record = write_buffer_record_find_next(peer_id, &index);
+    }
+
+    err_code = pds_peer_id_free(peer_id);
+
+    if ((err_code == NRF_SUCCESS) || (err_code == NRF_ERROR_INVALID_PARAM))
+    {
+        return err_code;
+    }
+    else
+    {
+        NRF_LOG_ERROR("Peer ID %d was not properly released. pds_peer_id_free() returned %s. "\
+                      "peer_id: %d",
+                      peer_id,
+                      nrf_strerror_get(err_code));
+        return NRF_ERROR_INTERNAL;
+    }
+}
+
+
+ret_code_t pdb_peer_data_ptr_get(pm_peer_id_t                 peer_id,
+                                 pm_peer_data_id_t            data_id,
+                                 pm_peer_data_flash_t * const p_peer_data)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    NRF_PM_DEBUG_CHECK(p_peer_data != NULL);
+
+    // Pass NULL to only retrieve a pointer.
+    return pds_peer_data_read(peer_id, data_id, (pm_peer_data_t*)p_peer_data, NULL);
+}
+
+
+
+ret_code_t pdb_write_buf_get(pm_peer_id_t      peer_id,
+                             pm_peer_data_id_t data_id,
+                             uint32_t          n_bufs,
+                             pm_peer_data_t  * p_peer_data)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    VERIFY_PARAM_NOT_NULL(p_peer_data);
+    VERIFY_DATA_ID_WRITE_BUF(data_id);
+
+    if ((n_bufs == 0) || (n_bufs > PM_FLASH_BUFFERS))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    pdb_buffer_record_t * p_write_buffer_record;
+    uint8_t             * p_buffer_memory;
+    bool                  new_record = false;
+
+    p_write_buffer_record = write_buffer_record_find(peer_id, data_id);
+
+    if (p_write_buffer_record == NULL)
+    {
+        // No buffer exists.
+        write_buffer_record_acquire(&p_write_buffer_record, peer_id, data_id);
+        if (p_write_buffer_record == NULL)
+        {
+            return NRF_ERROR_BUSY;
+        }
+    }
+    else if (p_write_buffer_record->n_bufs != n_bufs)
+    {
+        // Buffer exists with a different n_bufs from what was requested.
+        return NRF_ERROR_FORBIDDEN;
+    }
+
+    if (p_write_buffer_record->buffer_block_id == PM_BUFFER_INVALID_ID)
+    {
+        p_write_buffer_record->buffer_block_id = pm_buffer_block_acquire(&m_write_buffer, n_bufs);
+
+        if (p_write_buffer_record->buffer_block_id == PM_BUFFER_INVALID_ID)
+        {
+            write_buffer_record_invalidate(p_write_buffer_record);
+            return NRF_ERROR_BUSY;
+        }
+
+        new_record = true;
+    }
+
+    p_write_buffer_record->n_bufs = n_bufs;
+
+    p_buffer_memory = pm_buffer_ptr_get(&m_write_buffer, p_write_buffer_record->buffer_block_id);
+
+    if (p_buffer_memory == NULL)
+    {
+        NRF_LOG_ERROR("Cannot store data to flash because pm_buffer_ptr_get() could not retrieve "\
+                      "RAM buffer. Is block_id %d not allocated?",
+                      p_write_buffer_record->buffer_block_id);
+        return NRF_ERROR_INTERNAL;
+    }
+
+    peer_data_point_to_buffer(p_peer_data, data_id, p_buffer_memory, n_bufs);
+    if (new_record && (data_id == PM_PEER_DATA_ID_GATT_LOCAL))
+    {
+        p_peer_data->p_local_gatt_db->len = PM_LOCAL_DB_LEN(p_peer_data->length_words);
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t pdb_write_buf_release(pm_peer_id_t peer_id, pm_peer_data_id_t data_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    pdb_buffer_record_t * p_write_buffer_record;
+    p_write_buffer_record = write_buffer_record_find(peer_id, data_id);
+
+    if (p_write_buffer_record == NULL)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    write_buffer_record_release(p_write_buffer_record);
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t pdb_write_buf_store(pm_peer_id_t      peer_id,
+                               pm_peer_data_id_t data_id,
+                               pm_peer_id_t      new_peer_id)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    VERIFY_DATA_ID_WRITE_BUF(data_id);
+
+    if (!pds_peer_id_is_allocated(new_peer_id))
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    pdb_buffer_record_t * p_write_buffer_record = write_buffer_record_find(peer_id, data_id);
+
+    if (p_write_buffer_record == NULL)
+    {
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    p_write_buffer_record->peer_id = new_peer_id;
+    p_write_buffer_record->data_id = data_id;
+    return write_buf_store(p_write_buffer_record);
+}
+
+
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/peer_database.h

@@ -0,0 +1,194 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef PEER_DATABASE_H__
+#define PEER_DATABASE_H__
+
+#include <stdint.h>
+#include "peer_manager_types.h"
+#include "peer_manager_internal.h"
+#include "sdk_errors.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup peer_database Peer Database
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for simple management of reading and
+ *        writing of peer data into persistent storage.
+ *
+ */
+
+#define PDB_WRITE_BUF_SIZE (sizeof(pm_peer_data_bonding_t)) //!< The size (in bytes) of each block in the internal buffer accessible via @ref pdb_write_buf_get.
+
+/**@brief Macro for creating a peer ID value from a connection handle.
+ *
+ * Use this macro with pdb_write_buf_get() or pdb_write_buf_store(). It allows to create a write buffer
+ * even if you do not yet know the proper peer ID the data will be stored for.
+ *
+ * @return @p conn_handle + @ref PM_PEER_ID_N_AVAILABLE_IDS.
+ */
+#define PDB_TEMP_PEER_ID(conn_handle) (PM_PEER_ID_N_AVAILABLE_IDS + conn_handle)
+
+
+/**@brief Function for initializing the module.
+ *
+ * @retval NRF_SUCCESS          If initialization was successful.
+ * @retval NRF_ERROR_INTERNAL   An unexpected error happened.
+ */
+ret_code_t pdb_init(void);
+
+/**@brief Function for freeing a peer's persistent bond storage.
+ *
+ * @note This function will call @ref pdb_write_buf_release on the data for this peer.
+ *
+ * @param[in] peer_id  ID to be freed.
+ *
+ * @retval NRF_SUCCESS              Peer ID was released and clear operation was initiated successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  Peer ID was invalid.
+ */
+ret_code_t pdb_peer_free(pm_peer_id_t peer_id);
+
+/**@brief Function for retrieving a pointer to peer data in flash (read-only).
+ *
+ * @note  Dereferencing this pointer is not the safest thing to do if interrupts are enabled,
+ *        because Flash Data Storage garbage collection might move the data around. Either disable
+ *        interrupts while using the data, or use @ref pdb_peer_data_load.
+ *
+ * @param[in]  peer_id      The peer the data belongs to.
+ * @param[in]  data_id      The data to read.
+ * @param[out] p_peer_data  The peer data, read-only.
+ *
+ * @retval NRF_SUCCESS              If the pointer to the data was retrieved successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If either @p peer_id or @p data_id are invalid.
+ * @retval NRF_ERROR_NOT_FOUND      If data was not found in flash.
+ */
+ret_code_t pdb_peer_data_ptr_get(pm_peer_id_t                 peer_id,
+                                 pm_peer_data_id_t            data_id,
+                                 pm_peer_data_flash_t * const p_peer_data);
+
+/**@brief Function for retrieving pointers to a write buffer for peer data.
+ *
+ * @details This function will provide pointers to a buffer of the data. The data buffer will not be
+ *          written to persistent storage until @ref pdb_write_buf_store is called. The buffer is
+ *          released by calling either @ref pdb_write_buf_release, @ref pdb_write_buf_store, or
+ *          @ref pdb_peer_free.
+ *
+ *          When the data_id refers to a variable length data type, the available size is written
+ *          to the data, both the top-level, and any internal length fields.
+ *
+ * @note Calling this function on a peer_id/data_id pair that already has a buffer created will
+ *       give the same buffer, not create a new one. If n_bufs was increased since last time, the
+ *       buffer might be relocated to be able to provide additional room. In this case, the data
+ *       will be copied. If n_bufs was increased since last time, this function might return @ref
+ *       NRF_ERROR_BUSY. In that case, the buffer is automatically released.
+ *
+ * @param[in]  peer_id      ID of the peer to get a write buffer for. If @p peer_id is larger than
+ *                          @ref PM_PEER_ID_N_AVAILABLE_IDS, it is interpreted as pertaining to
+ *                          the connection with connection handle peer_id - PM_PEER_ID_N_AVAILABLE_IDS.
+ *                          See @ref PDB_TEMP_PEER_ID.
+ * @param[in]  data_id      The piece of data to get.
+ * @param[in]  n_bufs       Number of contiguous buffers needed.
+ * @param[out] p_peer_data  Pointers to mutable peer data.
+ *
+ * @retval NRF_SUCCESS              Data retrieved successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  @p data_id was invalid, or @p n_bufs was 0 or more than the total
+ *                                  available buffers.
+ * @retval NRF_ERROR_FORBIDDEN      n_bufs was higher or lower than the existing buffer. If needed,
+ *                                  release the existing buffer with @ref pdb_write_buf_release, and
+ *                                  call this function again.
+ * @retval NRF_ERROR_NULL           p_peer_data was NULL.
+ * @retval NRF_ERROR_BUSY           Not enough buffer(s) available.
+ * @retval NRF_ERROR_INTERNAL       Unexpected internal error.
+ */
+ret_code_t pdb_write_buf_get(pm_peer_id_t      peer_id,
+                             pm_peer_data_id_t data_id,
+                             uint32_t          n_bufs,
+                             pm_peer_data_t  * p_peer_data);
+
+/**@brief Function for freeing a write buffer allocated with @ref pdb_write_buf_get.
+ *
+ * @note This function will not write peer data to persistent memory. Data in released buffer will
+ *       be lost.
+ *
+ * @param[in]  peer_id  ID of peer to release buffer for.
+ * @param[in]  data_id  Which piece of data to release buffer for.
+ *
+ * @retval NRF_SUCCESS              Successfully released buffer.
+ * @retval NRF_ERROR_NOT_FOUND      No buffer was allocated for this peer ID/data ID pair.
+ */
+ret_code_t pdb_write_buf_release(pm_peer_id_t peer_id, pm_peer_data_id_t data_id);
+
+/**@brief Function for writing data into persistent storage. Writing happens asynchronously.
+ *
+ * @note This will unlock the data after it has been written.
+ *
+ * @param[in]  peer_id      The ID used to address the write buffer.
+ * @param[in]  data_id      The piece of data to store.
+ * @param[in]  new_peer_id  The ID to put in flash. This is usually the same as peer_id, but
+ *                          must be valid, i.e. allocated (and smaller than @ref PM_PEER_ID_N_AVAILABLE_IDS).
+ *
+ * @retval NRF_SUCCESS              Data storing was successfully started.
+ * @retval NRF_ERROR_STORAGE_FULL   No space available in persistent storage. Please clear some
+ *                                  space, the operation will be reattempted after the next compress
+ *                                  procedure.
+ * @retval NRF_ERROR_INVALID_PARAM  @p data_id or @p new_peer_id was invalid.
+ * @retval NRF_ERROR_NOT_FOUND      No buffer has been allocated for this peer ID/data ID pair.
+ * @retval NRF_ERROR_INTERNAL       Unexpected internal error.
+ */
+ret_code_t pdb_write_buf_store(pm_peer_id_t      peer_id,
+                               pm_peer_data_id_t data_id,
+                               pm_peer_id_t      new_peer_id);
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PEER_DATABASE_H__ */
+
+

components/ble/peer_manager/peer_id.c

@@ -0,0 +1,200 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "peer_id.h"
+
+#include <stdint.h>
+#include <string.h>
+#include "sdk_errors.h"
+#include "peer_manager_types.h"
+#include "nrf_atflags.h"
+
+
+typedef struct
+{
+    NRF_ATFLAGS_DEF_MEMBER(used_peer_ids,    PM_PEER_ID_N_AVAILABLE_IDS); /**< Bitmap designating which peer IDs are in use. */
+    NRF_ATFLAGS_DEF_MEMBER(deleted_peer_ids, PM_PEER_ID_N_AVAILABLE_IDS); /**< Bitmap designating which peer IDs are marked for deletion. */
+} pi_t;
+
+
+static pi_t m_pi = {{0}, {0}};
+
+
+static void internal_state_reset(pi_t * p_pi)
+{
+    memset(p_pi, 0, sizeof(pi_t));
+}
+
+
+void peer_id_init(void)
+{
+    internal_state_reset(&m_pi);
+}
+
+
+static pm_peer_id_t claim(pm_peer_id_t peer_id, nrf_atflags_t * p_peer_id_flags)
+{
+    pm_peer_id_t allocated_peer_id = PM_PEER_ID_INVALID;
+    if (peer_id == PM_PEER_ID_INVALID)
+    {
+        allocated_peer_id = nrf_atflags_find_and_set_flag(p_peer_id_flags, PM_PEER_ID_N_AVAILABLE_IDS);
+        if (allocated_peer_id == PM_PEER_ID_N_AVAILABLE_IDS)
+        {
+            allocated_peer_id = PM_PEER_ID_INVALID;
+        }
+    }
+    else if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS)
+    {
+        bool lock_success = !nrf_atflags_fetch_set(p_peer_id_flags, peer_id);
+        allocated_peer_id = lock_success ? peer_id : PM_PEER_ID_INVALID;
+    }
+    return allocated_peer_id;
+}
+
+
+static void release(pm_peer_id_t peer_id, nrf_atflags_t * p_peer_id_flags)
+{
+    if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS)
+    {
+        nrf_atflags_clear(p_peer_id_flags, peer_id);
+    }
+}
+
+
+pm_peer_id_t peer_id_allocate(pm_peer_id_t peer_id)
+{
+    return claim(peer_id, m_pi.used_peer_ids);
+}
+
+
+bool peer_id_delete(pm_peer_id_t peer_id)
+{
+    pm_peer_id_t deleted_peer_id;
+
+    if (peer_id == PM_PEER_ID_INVALID)
+    {
+        return false;
+    }
+
+    deleted_peer_id = claim(peer_id, m_pi.deleted_peer_ids);
+
+    return (deleted_peer_id == peer_id);
+}
+
+
+void peer_id_free(pm_peer_id_t peer_id)
+{
+    release(peer_id, m_pi.used_peer_ids);
+    release(peer_id, m_pi.deleted_peer_ids);
+}
+
+
+bool peer_id_is_allocated(pm_peer_id_t peer_id)
+{
+    if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS)
+    {
+        return nrf_atflags_get(m_pi.used_peer_ids, peer_id);
+    }
+    return false;
+}
+
+
+bool peer_id_is_deleted(pm_peer_id_t peer_id)
+{
+    if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS)
+    {
+        return nrf_atflags_get(m_pi.deleted_peer_ids, peer_id);
+    }
+    return false;
+}
+
+
+pm_peer_id_t next_id_get(pm_peer_id_t prev_peer_id, nrf_atflags_t * p_peer_id_flags)
+{
+    pm_peer_id_t i = (prev_peer_id == PM_PEER_ID_INVALID) ? 0 : (prev_peer_id + 1);
+    for (; i < PM_PEER_ID_N_AVAILABLE_IDS; i++)
+    {
+        if (nrf_atflags_get(p_peer_id_flags, i))
+        {
+            return i;
+        }
+    }
+
+    return PM_PEER_ID_INVALID;
+}
+
+
+pm_peer_id_t peer_id_get_next_used(pm_peer_id_t peer_id)
+{
+    peer_id = next_id_get(peer_id, m_pi.used_peer_ids);
+
+    while (peer_id != PM_PEER_ID_INVALID)
+    {
+        if (!peer_id_is_deleted(peer_id))
+        {
+            return peer_id;
+        }
+
+        peer_id = next_id_get(peer_id, m_pi.used_peer_ids);
+    }
+
+    return peer_id;
+}
+
+
+pm_peer_id_t peer_id_get_next_deleted(pm_peer_id_t prev_peer_id)
+{
+    return next_id_get(prev_peer_id, m_pi.deleted_peer_ids);
+}
+
+
+uint32_t peer_id_n_ids(void)
+{
+    uint32_t n_ids = 0;
+
+    for (pm_peer_id_t i = 0; i < PM_PEER_ID_N_AVAILABLE_IDS; i++)
+    {
+        n_ids += nrf_atflags_get(m_pi.used_peer_ids, i);
+    }
+
+    return n_ids;
+}
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/peer_id.h

@@ -0,0 +1,167 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef PEER_ID_H__
+#define PEER_ID_H__
+
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup peer_id Peer IDs
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. This module keeps track of which peer IDs are in
+ *        use and which are free.
+ */
+
+
+/**@brief Function for initializing the module.
+ */
+void peer_id_init(void);
+
+
+/**@brief Function for claiming an unused peer ID.
+ *
+ * @param peer_id  The peer ID to allocate. If this is @ref PM_PEER_ID_INVALID, the first available
+ *                 will be allocated.
+ *
+ * @return  The allocated peer ID.
+ * @retval  PM_PEER_ID_INVALID  If no peer ID could be allocated or module is not initialized.
+ */
+pm_peer_id_t peer_id_allocate(pm_peer_id_t peer_id);
+
+
+/**@brief Function for marking a peer ID for deletion.
+ *
+ * @param peer_id  The peer ID to delete.
+ *
+ * @retval  true   Deletion was successful.
+ * @retval  false  Peer ID already marked for deletion, peer_id was PM_PEER_ID_INVALID, or module is
+ *                 not initialized.
+ */
+bool peer_id_delete(pm_peer_id_t peer_id);
+
+
+/**@brief Function for freeing a peer ID and clearing all data associated with it in persistent
+ *        storage.
+ *
+ * @param[in]  peer_id  Peer ID to free.
+ */
+void peer_id_free(pm_peer_id_t peer_id);
+
+
+/**@brief Function for finding out whether a peer ID is marked for deletion.
+ *
+ * @param[in]  peer_id  The peer ID to inquire about.
+ *
+ * @retval  true   peer_id is in marked for deletion.
+ * @retval  false  peer_id is not marked for deletion, or the module is not initialized.
+ */
+bool peer_id_is_deleted(pm_peer_id_t peer_id);
+
+
+/**@brief Function for finding out whether a peer ID is in use.
+ *
+ * @param[in]  peer_id  The peer ID to inquire about.
+ *
+ * @retval  true   peer_id is in use.
+ * @retval  false  peer_id is free, or the module is not initialized.
+ */
+bool peer_id_is_allocated(pm_peer_id_t peer_id);
+
+
+/**@brief Function for getting the next peer ID in the sequence of all used peer IDs. Can be
+ *        used to loop through all used peer IDs.
+ *
+ * @note @ref PM_PEER_ID_INVALID is considered to be before the first and after the last ordinary
+ *       peer ID.
+ *
+ * @param[in]  prev_peer_id  The previous peer ID.
+ *
+ * @return  The next peer ID.
+ * @return  The first used peer ID  if prev_peer_id was @ref PM_PEER_ID_INVALID.
+ * @retval  PM_PEER_ID_INVALID      if prev_peer_id was the last ordinary peer ID or the module is
+ *                                  not initialized.
+ */
+pm_peer_id_t peer_id_get_next_used(pm_peer_id_t prev_peer_id);
+
+
+/**@brief Function for getting the next peer ID in the sequence of all peer IDs marked for deletion.
+ *        Can be used to loop through all peer IDs marked for deletion.
+ *
+ * @note @ref PM_PEER_ID_INVALID is considered to be before the first and after the last ordinary
+ *       peer ID.
+ *
+ * @param[in]  prev_peer_id  The previous peer ID.
+ *
+ * @return  The next peer ID.
+ * @return  The first used peer ID  if prev_peer_id was @ref PM_PEER_ID_INVALID.
+ * @retval  PM_PEER_ID_INVALID      if prev_peer_id was the last ordinary peer ID or the module is
+ *                                  not initialized.
+ */
+pm_peer_id_t peer_id_get_next_deleted(pm_peer_id_t prev_peer_id);
+
+
+/**@brief Function for querying the number of valid peer IDs available. I.E the number of peers
+ *        in persistent storage.
+ *
+ * @return  The number of valid peer IDs, or 0 if module is not initialized.
+ */
+uint32_t peer_id_n_ids(void);
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PEER_ID_H__ */

components/ble/peer_manager/peer_manager.h

@@ -0,0 +1,856 @@
+/**
+ * Copyright (c) 2015 - 2021, 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 peer_manager.h
+ *
+ * @defgroup peer_manager Peer Manager
+ * @ingroup ble_sdk_lib
+ * @{
+ * @brief Module for managing BLE bonding, which includes controlling encryption and pairing
+ *        procedures as well as persistently storing different pieces of data that must be stored
+ *        when bonded.
+ *
+ * @details The API consists of functions for configuring the pairing and encryption behavior of the
+ *          device and functions for manipulating the stored data.
+ *
+ *          This module uses Flash Data Storage (FDS) to interface with persistent storage. The
+ *          Peer Manager needs exclusive use of certain FDS file IDs and record keys. See
+ *          @ref lib_fds_functionality_keys for more information.
+ */
+
+
+#ifndef PEER_MANAGER_H__
+#define PEER_MANAGER_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "sdk_common.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+#include "peer_database.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**@brief Peer list filtrations. They determine which peer ID will be added to list.
+ */
+typedef enum
+{
+    PM_PEER_ID_LIST_ALL_ID,                                          /**< Add all peers. */
+    PM_PEER_ID_LIST_SKIP_NO_ID_ADDR = BIT_0,                         /**< Add only peers with an ID address (static address). */
+    PM_PEER_ID_LIST_SKIP_NO_IRK     = BIT_1,                         /**< Add only peers with a valid IRK. This implies @ref PM_PEER_ID_LIST_SKIP_NO_ID_ADDR, since all peers with IRKs have ID addresses. */
+    PM_PEER_ID_LIST_SKIP_NO_CAR     = BIT_2,                         /**< Add only peers with Central Address Resolution characteristic set to 0. */
+    PM_PEER_ID_LIST_SKIP_ALL        = PM_PEER_ID_LIST_SKIP_NO_IRK |  /**< All above filters applied. */
+                                      PM_PEER_ID_LIST_SKIP_NO_CAR
+} pm_peer_id_list_skip_t;
+
+/**@brief Function for initializing the Peer Manager.
+ *
+ * @details You must initialize the Peer Manager before you can call any other Peer Manager
+ *          functions.
+ *
+ * @retval NRF_SUCCESS              If initialization was successful.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ */
+ret_code_t pm_init(void);
+
+
+/**@brief Function for registering an event handler with the Peer Manager.
+ *
+ * @param[in] event_handler  Callback for events from the @ref peer_manager module. @p event_handler
+ *                           is called for every event that the Peer Manager sends after this
+ *                           function is called.
+ *
+ * @retval NRF_SUCCESS              If initialization was successful.
+ * @retval NRF_ERROR_NULL           If @p event_handler was NULL.
+ * @retval NRF_ERROR_NO_MEM         If no more registrations can happen.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_register(pm_evt_handler_t event_handler);
+
+
+/**@brief Function for providing pairing and bonding parameters to use for pairing procedures.
+ *
+ * @details Until this function is called, all bonding procedures that are initiated by the
+ *          peer are rejected.
+ *
+ *          This function can be called multiple times with different parameters, even with NULL as
+ *          @p p_sec_params, in which case the Peer Manager starts rejecting all procedures again.
+ *
+ * @param[in]  p_sec_params  Security parameters to be used for subsequent security procedures.
+ *
+ * @retval NRF_SUCCESS              If the parameters were set successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If the combination of parameters is invalid.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ */
+ret_code_t pm_sec_params_set(ble_gap_sec_params_t * p_sec_params);
+
+
+/**@brief Function for establishing encryption on a connection, and optionally establishing a bond.
+ *
+ * @details This function attempts to secure the link that is specified by @p conn_handle. It uses
+ *          the parameters that were previously provided in a call to @ref pm_sec_params_set.
+ *
+ *          If the connection is a master connection, calling this function starts a security
+ *          procedure on the link. If we have keys from a previous bonding procedure with this peer
+ *          and the keys meet the security requirements in the currently active security parameters,
+ *          the function attempts to establish encryption with the existing keys. If no key exists,
+ *          the function attempts to perform pairing and bonding according to the currently active
+ *          security parameters.
+ *
+ *          If the function completes successfully, a @ref PM_EVT_CONN_SEC_START event is sent.
+ *          The procedure might be queued, in which case the @ref PM_EVT_CONN_SEC_START event is
+ *          delayed until the procedure is initiated in the SoftDevice.
+ *
+ *          If the connection is a slave connection, the function sends a security request to
+ *          the peer (master). It is up to the peer then to initiate pairing or encryption.
+ *          If the peer ignores the request, a @ref BLE_GAP_EVT_AUTH_STATUS event occurs
+ *          with the status @ref BLE_GAP_SEC_STATUS_TIMEOUT. Otherwise, the peer initiates
+ *          security, in which case things happen as if the peer had initiated security itself.
+ *          See @ref PM_EVT_CONN_SEC_START for information about peer-initiated security.
+ *
+ * @param[in]  conn_handle      Connection handle of the link as provided by the SoftDevice.
+ * @param[in]  force_repairing  Whether to force a pairing procedure even if there is an existing
+ *                              encryption key. This argument is relevant only for
+ *                              the central role. Recommended value: false.
+ *
+ * @retval NRF_SUCCESS                    If the operation completed successfully.
+ * @retval NRF_ERROR_BUSY                 If a security procedure is already in progress on the link,
+ *                                        or if the link is disconnecting or disconnected.
+ * @retval NRF_ERROR_TIMEOUT              If there was an SMP time-out, so that no more security
+ *                                        operations can be performed on this link.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  If the connection handle is invalid.
+ * @retval NRF_ERROR_NOT_FOUND            If the security parameters have not been set, either by
+ *                                        @ref pm_sec_params_set or by @ref pm_conn_sec_params_reply.
+ * @retval NRF_ERROR_INVALID_DATA         If the peer is bonded, but no LTK was found in the stored
+ *                                        bonding data. Repairing was not requested.
+ * @retval NRF_ERROR_INVALID_STATE        If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_INTERNAL             If an internal error occurred.
+ */
+ret_code_t pm_conn_secure(uint16_t conn_handle, bool force_repairing);
+
+
+/**@brief Function for excluding a connection from the BLE event flow that is handled inside
+ *        the Peer Manager.
+ *
+ * @details This function is optional, and must be called in reply to a @ref PM_EVT_CONN_CONFIG_REQ
+ *          event, before the Peer Manager event handler returns. If it is not called in time,
+ *          BLE events for a connection handle passed in the @ref PM_EVT_CONN_CONFIG_REQ event will
+ *          be normally handled by the Peer Manager.
+ *
+ * @param[in]  conn_handle   The connection to be excluded.
+ * @param[in]  p_context     The context found in the request event that this function replies to.
+ *
+ * @retval NRF_SUCCESS              Successful reply.
+ * @retval NRF_ERROR_NULL           p_context was null.
+ */
+ret_code_t pm_conn_exclude(uint16_t conn_handle, void const * p_context);
+
+
+/**@brief Function for providing security configuration for a link.
+ *
+ * @details This function is optional, and must be called in reply to a @ref
+ *          PM_EVT_CONN_SEC_CONFIG_REQ event, before the Peer Manager event handler returns. If it
+ *          is not called in time, a default configuration is used. See @ref pm_conn_sec_config_t
+ *          for the value of the default.
+ *
+ * @param[in]  conn_handle        The connection to set the configuration for.
+ * @param[in]  p_conn_sec_config  The configuration.
+ */
+void pm_conn_sec_config_reply(uint16_t conn_handle, pm_conn_sec_config_t * p_conn_sec_config);
+
+
+/**@brief Function for providing security parameters for a link.
+ *
+ * @details This function is optional, and must be called in reply to a @ref
+ *          PM_EVT_CONN_SEC_PARAMS_REQ event, before the Peer Manager event handler returns. If it
+ *          is not called in time, the parameters given in @ref pm_sec_params_set are used.
+ *
+ * @param[in]  conn_handle   The connection to set the parameters for.
+ * @param[in]  p_sec_params  The parameters. If NULL, the security procedure is rejected.
+ * @param[in]  p_context     The context found in the request event that this function replies to.
+ *
+ * @retval NRF_SUCCESS              Successful reply.
+ * @retval NRF_ERROR_NULL           p_sec_params or p_context was null.
+ * @retval NRF_ERROR_INVALID_PARAM  Value of p_sec_params was invalid.
+ * @retval NRF_ERROR_INVALID_STATE  This module is not initialized.
+ */
+ret_code_t pm_conn_sec_params_reply(uint16_t               conn_handle,
+                                    ble_gap_sec_params_t * p_sec_params,
+                                    void           const * p_context);
+
+
+/**@brief Function for manually informing that the local database has changed.
+ *
+ * @details This function sends a service changed indication to all bonded and/or connected peers
+ *          that subscribe to this indication. If a bonded peer is not connected, the indication is
+ *          sent when it reconnects. Every time an indication is sent, a @ref
+ *          PM_EVT_SERVICE_CHANGED_IND_SENT event occurs, followed by a @ref
+ *          PM_EVT_SERVICE_CHANGED_IND_CONFIRMED when the peer sends its confirmation. Peers that
+ *          are not subscribed to the service changed indication when this function is called do not
+ *          receive an indication, and no events are sent to the user. Likewise, if the service
+ *          changed characteristic is not present in the local database, or if the @ref
+ *          PM_SERVICE_CHANGED_ENABLED is set to 0, no indications are sent peers, and no events are
+ *          sent to the user.
+ */
+void pm_local_database_has_changed(void);
+
+
+/**@brief Function for getting the security status of a connection.
+ *
+ * @param[in]  conn_handle        Connection handle of the link as provided by the SoftDevice.
+ * @param[out] p_conn_sec_status  Security status of the link.
+ *
+ * @retval NRF_SUCCESS                    If pairing was initiated successfully.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  If the connection handle is invalid.
+ * @retval NRF_ERROR_NULL                 If @p p_conn_sec_status was NULL.
+ * @retval NRF_ERROR_INVALID_STATE        If the Peer Manager is not initialized.
+ */
+ret_code_t pm_conn_sec_status_get(uint16_t conn_handle, pm_conn_sec_status_t * p_conn_sec_status);
+
+
+/**@brief Function for comparing the security status of a connection against a baseline.
+ *
+ * @param[in]  conn_handle       Connection handle of the link as provided by the SoftDevice.
+ * @param[out] p_sec_status_req  Target baseline security status to compare against.
+ *
+ * @retval true   If the security status of the connection matches or exceeds the baseline on all
+ *                points.
+ * @retval false  If the security status of the connection does not fulfil the baseline, or could
+ *                not be retrieved.
+ */
+bool pm_sec_is_sufficient(uint16_t conn_handle, pm_conn_sec_status_t * p_sec_status_req);
+
+
+/**@brief Experimental function for specifying the public key to use for LESC operations.
+ *
+ * @details This function can be called multiple times. The specified public key will be used for
+ *          all subsequent LESC (LE Secure Connections) operations until the next time this function
+ *          is called.
+ *
+ * @note The key must continue to reside in application memory as it is not copied by Peer Manager.
+ *
+ * @note This function is deprecated. LESC keys are now handled internally if @ref PM_LESC_ENABLED
+ *       is true. If @ref PM_LESC_ENABLED is false, this function works as before.
+ *
+ * @param[in]  p_public_key  The public key to use for all subsequent LESC operations.
+ *
+ * @retval NRF_SUCCESS                    If pairing was initiated successfully.
+ * @retval NRF_ERROR_INVALID_STATE        If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_FORBIDDEN            If LESC module support is enabled (see @ref PM_LESC_ENABLED).
+ */
+ret_code_t pm_lesc_public_key_set(ble_gap_lesc_p256_pk_t * p_public_key);
+
+
+/**@brief Function for setting or clearing the whitelist.
+ *
+ * When using the S13x SoftDevice v3.x, this function sets or clears the whitelist.
+ * When using the S13x SoftDevice v2.x, this function caches a list of
+ * peers that can be retrieved later by @ref pm_whitelist_get to pass to the @ref lib_ble_advertising.
+ *
+ * To clear the current whitelist, pass either NULL as @p p_peers or zero as @p peer_cnt.
+ *
+ * @param[in] p_peers   The peers to add to the whitelist. Pass NULL to clear the current whitelist.
+ * @param[in] peer_cnt  The number of peers to add to the whitelist. The number must not be greater than
+ *                      @ref BLE_GAP_WHITELIST_ADDR_MAX_COUNT. Pass zero to clear the current
+ *                      whitelist.
+ *
+ * @retval NRF_SUCCESS                      If the whitelist was successfully set or cleared.
+ * @retval BLE_GAP_ERROR_WHITELIST_IN_USE   If a whitelist is already in use and cannot be set.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR   If a peer in @p p_peers has an address that cannot
+ *                                          be used for whitelisting.
+ * @retval NRF_ERROR_NOT_FOUND              If any of the peers in @p p_peers cannot be found.
+ * @retval NRF_ERROR_DATA_SIZE              If @p peer_cnt is greater than
+ *                                          @ref BLE_GAP_WHITELIST_ADDR_MAX_COUNT.
+ * @retval NRF_ERROR_INVALID_STATE          If the Peer Manager is not initialized.
+ */
+ret_code_t pm_whitelist_set(pm_peer_id_t const * p_peers,
+                            uint32_t             peer_cnt);
+
+
+/**@brief Function for retrieving the previously set whitelist.
+ *
+ * The function retrieves the whitelist of GAP addresses and IRKs that was
+ * previously set by @ref pm_whitelist_set.
+ *
+ * To retrieve only GAP addresses or only IRKs, provide only one of the
+ * buffers. If a buffer is provided, its size must be specified.
+ *
+ * @param[out]   p_addrs    The buffer where to store GAP addresses. Pass NULL to retrieve
+ *                          only IRKs (in that case, @p p_irks must not be NULL).
+ * @param[in,out] p_addr_cnt In: The size of the @p p_addrs buffer.
+ *                          May be NULL if and only if @p p_addrs is NULL.
+ *                          Out: The number of GAP addresses copied into the buffer.
+ *                          If @p p_addrs is NULL, this parameter remains unchanged.
+ * @param[out]   p_irks     The buffer where to store IRKs. Pass NULL to retrieve
+ *                          only GAP addresses (in that case, @p p_addrs must not NULL).
+ * @param[in,out] p_irk_cnt  In: The size of the @p p_irks buffer.
+ *                          May be NULL if and only if @p p_irks is NULL.
+ *                          Out: The number of IRKs copied into the buffer.
+ *                          If @p p_irks is NULL, this paramater remains unchanged.
+ *
+ * @retval NRF_SUCCESS                      If the whitelist was successfully retrieved.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR   If a peer has an address that cannot be used for
+ *                                          whitelisting (this error can occur only
+ *                                          when using the S13x SoftDevice v2.x).
+ * @retval NRF_ERROR_NULL                   If a required parameter is NULL.
+ * @retval NRF_ERROR_NO_MEM                 If the provided buffers are too small.
+ * @retval NRF_ERROR_NOT_FOUND              If the data for any of the cached whitelisted peers
+ *                                          cannot be found. It might have been deleted.
+ * @retval NRF_ERROR_INVALID_STATE          If the Peer Manager is not initialized.
+ */
+ret_code_t pm_whitelist_get(ble_gap_addr_t * p_addrs,
+                            uint32_t       * p_addr_cnt,
+                            ble_gap_irk_t  * p_irks,
+                            uint32_t       * p_irk_cnt);
+
+
+/**@brief Function for setting and clearing the device identities list.
+ *
+ * @note When entering directed advertising, make sure the active identities list does not contain
+ *       peers that have no Central Address Resolution. See @ref pm_peer_id_list with skip_id
+ *       @ref PM_PEER_ID_LIST_SKIP_NO_CAR.
+ *
+ * @param[in]   p_peers     The peers to add to the device identities list. Pass NULL to clear
+ *                          the device identities list.
+ * @param[in]   peer_cnt    The number of peers. Pass zero to clear the device identities list.
+ *
+ * @retval NRF_SUCCESS                              If the device identities list was successfully
+ *                                                  set or cleared.
+ * @retval NRF_ERROR_NOT_FOUND                      If a peer is invalid or its data could not
+ *                                                  be found in flash.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR           If a peer has an address that cannot be
+ *                                                  used for whitelisting.
+ * @retval BLE_ERROR_GAP_DEVICE_IDENTITIES_IN_USE   If the device identities list is in use and
+ *                                                  cannot be set.
+ * @retval NRF_ERROR_INVALID_STATE                  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_NOT_SUPPORTED                  If using a SoftDevice that does not support
+ *                                                  device identities, e.g. S130 v2.0.
+ */
+ret_code_t pm_device_identities_list_set(pm_peer_id_t const * p_peers,
+                                         uint32_t             peer_cnt);
+
+
+/**@brief Function for setting the local <em>Bluetooth</em> identity address.
+ *
+ * @details The local <em>Bluetooth</em> identity address is the address that identifies the device
+ * to other peers. The address type must be either @ref BLE_GAP_ADDR_TYPE_PUBLIC or @ref
+ * BLE_GAP_ADDR_TYPE_RANDOM_STATIC. The identity address cannot be changed while roles are running.
+ *
+ * The SoftDevice sets a default address of type @ref BLE_GAP_ADDR_TYPE_RANDOM_STATIC when it is
+ * enabled. This default address is a random number that is populated during the IC manufacturing
+ * process. It remains unchanged for the lifetime of each IC, but the application can use this
+ * function to assign a different identity address.
+ *
+ * The identity address is distributed to the peer during bonding. Changing the identity address
+ * means bonded devices might not recognize us.
+ *
+ * @note The SoftDevice functions @ref sd_ble_gap_addr_set and @ref sd_ble_gap_privacy_set must not
+ *       be called when using the Peer Manager. Use the Peer Manager equivalents instead.
+ *
+ * @param[in] p_addr The GAP address to be set.
+ *
+ * @retval NRF_SUCCESS                     If the identity address was set successfully.
+ * @retval NRF_ERROR_NULL                  If @p p_addr is NULL.
+ * @retval NRF_ERROR_INVALID_ADDR          If the @p p_addr pointer is invalid.
+ * @retval BLE_ERROR_GAP_INVALID_BLE_ADDR  If the BLE address is invalid.
+ * @retval NRF_ERROR_BUSY                  If the SoftDevice was busy. Process SoftDevice events
+ *                                         and retry.
+ * @retval NRF_ERROR_INVALID_STATE         If the Peer Manager is not initialized or if this function
+ *                                         was called while advertising, scanning, or while connected.
+ * @retval NRF_ERROR_INTERNAL              If an internal error occurred.
+ */
+ret_code_t pm_id_addr_set(ble_gap_addr_t const * p_addr);
+
+
+/**@brief Function for retrieving the local <em>Bluetooth</em> identity address.
+ *
+ * This function always returns the identity address, irrespective of the privacy settings.
+ * This means that the address type will always be either @ref BLE_GAP_ADDR_TYPE_PUBLIC or @ref
+ * BLE_GAP_ADDR_TYPE_RANDOM_STATIC.
+ *
+ * @param[out] p_addr Pointer to the address structure to be filled in.
+ *
+ * @retval NRF_SUCCESS             If the address was retrieved successfully.
+ * @retval NRF_ERROR_NULL          If @p p_addr is NULL.
+ * @retval NRF_ERROR_INVALID_STATE If the Peer Manager is not initialized.
+ */
+ret_code_t pm_id_addr_get(ble_gap_addr_t * p_addr);
+
+
+/**@brief Function for configuring privacy settings.
+ *
+ * The privacy settings cannot be configured while advertising, scanning, or while in a connection.
+ *
+ * @note The SoftDevice functions @ref sd_ble_gap_addr_set
+ *       and @ref sd_ble_gap_privacy_set must not be called when using the Peer Manager.
+ *       Use this function instead.
+ *
+ * @param[in] p_privacy_params Privacy settings.
+ *
+ * @retval NRF_SUCCESS                     If the privacy settings were configured successfully.
+ * @retval NRF_ERROR_NULL                  If @p p_privacy_params is NULL.
+ * @retval NRF_ERROR_BUSY                  If the operation could not be performed at this time.
+ *                                         Process SoftDevice events and retry.
+ * @retval NRF_ERROR_INVALID_PARAM         If the address type is invalid.
+ * @retval NRF_ERROR_INVALID_STATE         If this function is called while BLE roles using
+ *                                         privacy are enabled.
+ * @retval NRF_ERROR_INVALID_STATE         If the Peer Manager is not initialized.
+ */
+ret_code_t pm_privacy_set(pm_privacy_params_t const * p_privacy_params);
+
+
+/**@brief Function for retrieving privacy settings.
+ *
+ * The privacy settings that are returned include the current IRK as well.
+ *
+ * @param[out] p_privacy_params Privacy settings.
+ *
+ * @retval NRF_SUCCESS              If the privacy settings were retrieved successfully.
+ * @retval NRF_ERROR_NULL           If @p p_privacy_params or @p p_privacy_params->p_device_irk is
+ *                                  NULL.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_privacy_get(pm_privacy_params_t * p_privacy_params);
+
+
+/**@brief Function for resolving a resolvable address with an identity resolution key (IRK).
+ *
+ * @param[in] p_addr  A private random resolvable address.
+ * @param[in] p_irk   An identity resolution key (IRK).
+ *
+ * @retval true   The IRK used matched the one used to create the address.
+ * @retval false  The IRK used did not match the one used to create the address, or an argument was
+ *                NULL or invalid.
+ */
+bool pm_address_resolve(ble_gap_addr_t const * p_addr, ble_gap_irk_t const * p_irk);
+
+
+/**@brief Function for getting the connection handle of the connection with a bonded peer.
+ *
+ * @param[in]  peer_id        The peer ID of the bonded peer.
+ * @param[out] p_conn_handle  Connection handle, or @ref BLE_CONN_HANDLE_INVALID if none could be
+ *                            resolved. The conn_handle can refer to a recently disconnected connection.
+ *
+ * @retval NRF_SUCCESS              If the connection handle was retrieved successfully.
+ * @retval NRF_ERROR_NULL           If @p p_conn_handle was NULL.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_conn_handle_get(pm_peer_id_t peer_id, uint16_t * p_conn_handle);
+
+
+/**@brief Function for retrieving the ID of a peer, given its connection handle.
+ *
+ * @param[in]  conn_handle  The connection handle of the peer.
+ * @param[out] p_peer_id    The peer ID, or @ref PM_PEER_ID_INVALID if the peer is not bonded or
+ *                          @p conn_handle does not refer to a valid connection.
+ *
+ * @retval NRF_SUCCESS              If the peer ID was retrieved successfully.
+ * @retval NRF_ERROR_NULL           If @p p_peer_id was NULL.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_peer_id_get(uint16_t conn_handle, pm_peer_id_t * p_peer_id);
+
+
+/**@brief Function for retrieving a filtered list of peer IDs.
+ *
+ * @details This function starts searching from @p first_peer_id. IDs ordering
+ *          is the same as for @ref pm_next_peer_id_get(). If the first_peer_id
+ *          is @ref PM_PEER_ID_INVALID, the function starts searching from the first ID.
+ *          The function looks for the ID's number specified by @p p_list_size. Only those IDs that
+ *          match @p skip_id are added to the list. The number of returned elements is determined
+ *          by @p p_list_size.
+ *
+ * @warning The size of the @p p_peer_list buffer must be equal or greater than @p p_list_size.
+ *
+ * @param[out]    p_peer_list   Pointer to peer IDs list buffer.
+ * @param[in,out] p_list_size   The amount of IDs to return / The number of returned IDs.
+ * @param[in]     first_peer_id The first ID from which the search begins. IDs ordering
+ *                              is the same as for @ref pm_next_peer_id_get()
+ * @param[in]     skip_id       It determines which peer ID will be added to list.
+ *
+ * @retval NRF_SUCCESS              If the ID list has been filled out.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p skip_id was invalid.
+ * @retval NRF_ERROR_NULL           If peer_list or list_size was NULL.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_peer_id_list(pm_peer_id_t         * p_peer_list,
+                           uint32_t       * const p_list_size,
+                           pm_peer_id_t           first_peer_id,
+                           pm_peer_id_list_skip_t skip_id);
+
+
+/**@brief Function for getting the next peer ID in the sequence of all used peer IDs.
+ *
+ * @details This function can be used to loop through all used peer IDs. The order in which
+ *          peer IDs are returned should be considered unpredictable. @ref PM_PEER_ID_INVALID
+ *          is considered to be before the first and after the last used peer ID.
+ *
+ * @details To loop through all peer IDs exactly once, use the following constuct:
+ *          @code{c}
+ *              pm_peer_id_t current_peer_id = pm_next_peer_id_get(PM_PEER_ID_INVALID);
+ *              while (current_peer_id != PM_PEER_ID_INVALID)
+ *              {
+ *                  // Do something with current_peer_id.
+ *                  current_peer_id = pm_next_peer_id_get(current_peer_id)
+ *              }
+ *          @endcode
+ *
+ * @note This function does not report peer IDs that are pending deletion.
+ *
+ * @param[in]  prev_peer_id  The previous peer ID.
+ *
+ * @return  The next peer ID. If @p prev_peer_id was @ref PM_PEER_ID_INVALID, the
+ *          next peer ID is the first used peer ID. If @p prev_peer_id was the last
+ *          used peer ID, the function returns @ref PM_PEER_ID_INVALID.
+ */
+pm_peer_id_t pm_next_peer_id_get(pm_peer_id_t prev_peer_id);
+
+
+/**@brief Function for querying the number of valid peer IDs that are available.
+ *
+ * @details This function returns the number of peers for which there is data in persistent storage.
+ *
+ * @return  The number of valid peer IDs.
+ */
+uint32_t pm_peer_count(void);
+
+
+
+
+/**@anchor PM_PEER_DATA_FUNCTIONS
+ * @name Functions (Peer Data)
+ * Functions for manipulating peer data.
+ * @{
+ */
+
+/**
+ * @{
+ */
+
+/**@brief Function for retrieving stored data of a peer.
+ *
+ * @note The length of the provided buffer must be a multiple of 4.
+ *
+ * @param[in]    peer_id   Peer ID to get data for.
+ * @param[in]    data_id   Which type of data to read.
+ * @param[out]   p_data    Where to put the retrieved data. The documentation for
+ *                         @ref pm_peer_data_id_t specifies what data type each data ID is stored as.
+ * @param[in,out] p_len    In: The length in bytes of @p p_data.
+ *                         Out: The length in bytes of the read data, if the read was successful.
+ *
+ * @retval NRF_SUCCESS              If the data was read successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If the data type or the peer ID was invalid or unallocated.
+ * @retval NRF_ERROR_NULL           If a pointer parameter was NULL.
+ * @retval NRF_ERROR_NOT_FOUND      If no stored data was found for this peer ID/data ID combination.
+ * @retval NRF_ERROR_DATA_SIZE      If the provided buffer was not large enough. The data is still
+ *                                  copied, filling the provided buffer. Note that this error can
+ *                                  occur even if loading the same size as was stored, because the
+ *                                  underlying layers round the length up to the nearest word (4 bytes)
+ *                                  when storing.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_peer_data_load(pm_peer_id_t      peer_id,
+                             pm_peer_data_id_t data_id,
+                             void            * p_data,
+                             uint32_t        * p_len);
+
+/**@brief Function for reading a peer's bonding data (@ref PM_PEER_DATA_ID_BONDING).
+ * @details See @ref pm_peer_data_load for parameters and return values. */
+ret_code_t pm_peer_data_bonding_load(pm_peer_id_t             peer_id,
+                                     pm_peer_data_bonding_t * p_data);
+
+/**@brief Function for reading a peer's remote DB values. (@ref PM_PEER_DATA_ID_GATT_REMOTE).
+ * @details See @ref pm_peer_data_load for parameters and return values. */
+ret_code_t pm_peer_data_remote_db_load(pm_peer_id_t        peer_id,
+                                       ble_gatt_db_srv_t * p_data,
+                                       uint32_t          * p_len);
+
+/**@brief Function for reading a peer's application data. (@ref PM_PEER_DATA_ID_APPLICATION).
+ * @details See @ref pm_peer_data_load for parameters and return values. */
+ret_code_t pm_peer_data_app_data_load(pm_peer_id_t peer_id,
+                                      void       * p_data,
+                                      uint32_t   * p_len);
+/** @}*/
+
+
+/**
+ * @{
+ */
+
+/**@brief Function for setting or updating stored data of a peer.
+ *
+ * @note Writing the data to persistent storage happens asynchronously. Therefore, the buffer
+ *       that contains the data must be kept alive until the operation has completed.
+ *
+ * @note The data written using this function might later be overwritten as a result of internal
+ *       operations in the Peer Manager. A Peer Manager event is sent each time data is updated,
+ *       regardless of whether the operation originated internally or from action by the user.
+ *       Data with @p data_id @ref PM_PEER_DATA_ID_GATT_REMOTE @ref PM_PEER_DATA_ID_APPLICATION is
+ *       never (over)written internally.
+ *
+ * @param[in]  peer_id  Peer ID to set data for.
+ * @param[in]  data_id  Which type of data to set.
+ * @param[in]  p_data   New value to set. The documentation for @ref pm_peer_data_id_t specifies
+ *                      what data type each data ID should be stored as.
+ * @param[in]  len      The length in bytes of @p p_data.
+ * @param[out] p_token  A token that identifies this particular store operation. The token can be
+ *                      used to identify events that pertain to this operation. This parameter can
+ *                      be NULL.
+ *
+ * @retval NRF_SUCCESS              If the data is scheduled to be written to persistent storage.
+ * @retval NRF_ERROR_NULL           If @p p_data is NULL.
+ * @retval NRF_ERROR_NOT_FOUND      If no peer was found for the peer ID.
+ * @retval NRF_ERROR_INVALID_ADDR   If @p p_data is not word-aligned (4 bytes).
+ * @retval NRF_ERROR_BUSY           If the underlying flash handler is busy with other flash
+ *                                  operations. Try again after receiving a Peer Manager event.
+ * @retval NRF_ERROR_STORAGE_FULL   If there is not enough space in persistent storage.
+ * @retval NRF_ERROR_FORBIDDEN      If data ID is @ref PM_PEER_DATA_ID_BONDING and the new bonding
+ *                                  data also corresponds to another bonded peer. No data is written
+ *                                  so duplicate entries are avoided.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_peer_data_store(pm_peer_id_t       peer_id,
+                              pm_peer_data_id_t  data_id,
+                              void       const * p_data,
+                              uint32_t           len,
+                              pm_store_token_t * p_token);
+
+/**@brief Function for setting or updating a peer's bonding data (@ref PM_PEER_DATA_ID_BONDING).
+ * @details See @ref pm_peer_data_store for parameters and return values. */
+ret_code_t pm_peer_data_bonding_store(pm_peer_id_t                   peer_id,
+                                      pm_peer_data_bonding_t const * p_data,
+                                      pm_store_token_t             * p_token);
+
+/**@brief Function for setting or updating a peer's remote DB values. (@ref PM_PEER_DATA_ID_GATT_REMOTE).
+ * @details See @ref pm_peer_data_store for parameters and return values. */
+ret_code_t pm_peer_data_remote_db_store(pm_peer_id_t              peer_id,
+                                        ble_gatt_db_srv_t const * p_data,
+                                        uint32_t                  len,
+                                        pm_store_token_t        * p_token);
+
+/**@brief Function for setting or updating a peer's application data. (@ref PM_PEER_DATA_ID_APPLICATION).
+ * @details See @ref pm_peer_data_store for parameters and return values. */
+ret_code_t pm_peer_data_app_data_store(pm_peer_id_t       peer_id,
+                                       void       const * p_data,
+                                       uint32_t           len,
+                                       pm_store_token_t * p_token);
+/** @}*/
+
+
+/**
+ * @{
+ */
+
+/**@brief Function for deleting a peer's stored pieces of data.
+ *
+ * @details This function deletes specific data that is stored for a peer. Note that bonding data
+ *          cannot be cleared separately.
+ *
+ *          To delete all data for a peer (including bonding data), use @ref pm_peer_delete.
+ *
+ * @note Clearing data in persistent storage happens asynchronously.
+ *
+ * @param[in]  peer_id  Peer ID to clear data for.
+ * @param[in]  data_id  Which data to clear.
+ *
+ * @retval NRF_SUCCESS              If the clear procedure was initiated successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p data_id was PM_PEER_DATA_ID_BONDING or invalid, or
+ *                                  @p peer_id was invalid.
+ * @retval NRF_ERROR_NOT_FOUND      If there was no data to clear for this peer ID/data ID combination.
+ * @retval NRF_ERROR_BUSY           If the underlying flash handler is busy with other flash
+ *                                  operations. Try again after receiving a Peer Manager event.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ */
+ret_code_t pm_peer_data_delete(pm_peer_id_t peer_id, pm_peer_data_id_t data_id);
+
+
+/**@brief Function for manually adding a peer to the persistent storage.
+ *
+ * @details This function allocates a new peer ID and stores bonding data for the new peer. The
+ *          bonding data is necessary to prevent ambiguity/inconsistency in peer data.
+ *
+ * @param[in]  p_bonding_data  The bonding data of the new peer (must contain a public/static
+ *                             address or a non-zero IRK).
+ * @param[out] p_new_peer_id   Peer ID for the new peer, or an existing peer if a match was found.
+ * @param[out] p_token         A token that identifies this particular store operation (storing the
+ *                             bonding data). The token can be used to identify events that pertain
+ *                             to this operation. This parameter can be NULL.
+ *
+ * @retval NRF_SUCCESS              If the store operation for bonding data was initiated successfully.
+ * @retval NRF_ERROR_NULL           If @p p_bonding_data or @p p_new_peer_id is NULL.
+ * @retval NRF_ERROR_INVALID_ADDR   If @p p_bonding_data is not word-aligned (4 bytes).
+ * @retval NRF_ERROR_STORAGE_FULL   If there is not enough space in persistent storage.
+ * @retval NRF_ERROR_NO_MEM         If there are no more available peer IDs.
+ * @retval NRF_ERROR_BUSY           If the underlying flash filesystem is busy with other flash
+ *                                  operations. Try again after receiving a Peer Manager event.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ */
+ret_code_t pm_peer_new(pm_peer_id_t           * p_new_peer_id,
+                       pm_peer_data_bonding_t * p_bonding_data,
+                       pm_store_token_t       * p_token);
+
+
+/**@brief Function for freeing persistent storage for a peer.
+ *
+ * @details This function deletes every piece of data that is associated with the specified peer and
+ *          frees the peer ID to be used for another peer. The deletion happens asynchronously, and
+ *          the peer ID is not freed until the data is deleted. When the operation finishes, a @ref
+ *          PM_EVT_PEER_DELETE_SUCCEEDED or @ref PM_EVT_PEER_DELETE_FAILED event is sent.
+ *
+ * @warning Use this function only when not connected to or connectable for the peer that is being
+ *          deleted. If the peer is or becomes connected or data is manually written in flash during
+ *          this procedure (until the success or failure event happens), the behavior is undefined.
+ *
+ * @param[in]  peer_id  Peer ID to be freed and have all associated data deleted.
+ *
+ * @retval NRF_SUCCESS              If the operation was initiated successfully.
+ * @retval NRF_ERROR_INVALID_PARAM  If the peer ID was not valid.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ */
+ret_code_t pm_peer_delete(pm_peer_id_t peer_id);
+
+
+/**@brief Function for deleting all data stored for all peers.
+ *
+ * @details This function sends either a @ref PM_EVT_PEERS_DELETE_SUCCEEDED or a @ref
+ *          PM_EVT_PEERS_DELETE_FAILED event. In addition, a @ref PM_EVT_PEER_DELETE_SUCCEEDED or
+ *          @ref PM_EVT_PEER_DELETE_FAILED event is sent for each deleted peer.
+ *
+ * @note When there is no peer data in flash the @ref PM_EVT_PEER_DELETE_SUCCEEDED event is sent synchronously.
+ *
+ * @warning Use this function only when not connected or connectable. If a peer is or becomes
+ *          connected or a @ref PM_PEER_DATA_FUNCTIONS function is used during this procedure (until
+ *          the success or failure event happens), the behavior is undefined.
+ *
+ * @retval NRF_SUCCESS              If the deletion process was initiated successfully.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ */
+ret_code_t pm_peers_delete(void);
+/** @}*/
+
+
+/**
+ * @{
+ */
+
+
+/**@brief Function for finding the highest and lowest ranked peers.
+ *
+ * @details The rank is saved in persistent storage under the data ID @ref PM_PEER_DATA_ID_PEER_RANK.
+ *
+ * @details The interpretation of rank is up to the user, because the rank is only updated by
+ *          calling @ref pm_peer_rank_highest or by manipulating the value using a @ref
+ *          PM_PEER_DATA_FUNCTIONS function.
+ *
+ * @note Peers with no stored rank are not considered.
+ * @note Any argument that is NULL is ignored.
+ *
+ * @param[out] p_highest_ranked_peer  The peer ID with the highest rank of all peers, for example,
+ *                                    the most recently used peer.
+ * @param[out] p_highest_rank         The highest rank.
+ * @param[out] p_lowest_ranked_peer   The peer ID with the lowest rank of all peers, for example,
+ *                                    the least recently used peer.
+ * @param[out] p_lowest_rank          The lowest rank.
+ *
+ * @retval NRF_SUCCESS              If the operation completed successfully.
+ * @retval NRF_ERROR_NOT_FOUND      If no peer with stored peer rank was found.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ * @retval NRF_ERROR_NOT_SUPPORTED  If peer rank functionality has been disabled via the @ref
+ *                                  PM_PEER_RANKS_ENABLED configuration option.
+ */
+ret_code_t pm_peer_ranks_get(pm_peer_id_t * p_highest_ranked_peer,
+                             uint32_t     * p_highest_rank,
+                             pm_peer_id_t * p_lowest_ranked_peer,
+                             uint32_t     * p_lowest_rank);
+
+
+/**@brief Function for updating the rank of a peer to be highest among all stored peers.
+ *
+ * @details If this function returns @ref NRF_SUCCESS, either a @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED or a
+ *          @ref PM_EVT_PEER_DATA_UPDATE_FAILED event is sent with a @ref
+ *          PM_STORE_TOKEN_INVALID store token when the operation is complete. Until the operation
+ *          is complete, this function returns @ref NRF_ERROR_BUSY.
+ *
+ *          When the operation is complete, the peer is the highest ranked peer as reported by
+ *          @ref pm_peer_ranks_get.
+ *
+ * @note The @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED event can arrive before the function returns if the peer
+ *       is already ranked highest. In this case, the @ref pm_peer_data_update_succeeded_evt_t::flash_changed flag
+ *       in the event will be false.
+ *
+ * @param[in]  peer_id  The peer to rank highest.
+ *
+ * @retval NRF_SUCCESS              If the peer's rank is, or will be updated to be highest.
+ * @retval NRF_ERROR_INVALID_PARAM  If @p peer_id is invalid, or doesn't exist in flash.
+ * @retval NRF_ERROR_STORAGE_FULL   If there is not enough space in persistent storage.
+ * @retval NRF_ERROR_BUSY           If the underlying flash handler is busy with other flash
+ *                                  operations, or if a previous call to this function has not
+ *                                  completed. Try again after receiving a Peer Manager event.
+ * @retval NRF_ERROR_INVALID_STATE  If the Peer Manager is not initialized.
+ * @retval NRF_ERROR_RESOURCES      If the highest rank is UINT32_MAX, so the new rank would wrap
+ *                                  around to 0. To fix this, manually update all ranks to smaller
+ *                                  values, while still keeping their order.
+ * @retval NRF_ERROR_INTERNAL       If an internal error occurred.
+ * @retval NRF_ERROR_NOT_SUPPORTED  If peer rank functionality has been disabled via the @ref
+ *                                  PM_PEER_RANKS_ENABLED configuration option.
+ */
+ret_code_t pm_peer_rank_highest(pm_peer_id_t peer_id);
+
+/** @}*/
+
+/** @} */
+
+/** @} */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // PEER_MANAGER_H__

components/ble/peer_manager/peer_manager_handler.c

@@ -0,0 +1,691 @@
+/**
+ * Copyright (c) 2018 - 2021, 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.
+ *
+ */
+#include "peer_manager_handler.h"
+#include <stdint.h>
+#include <string.h>
+#include "sdk_errors.h"
+#include "app_error.h"
+#include "peer_manager.h"
+#include "ble_gap.h"
+#include "ble_gattc.h"
+#include "ble_conn_state.h"
+#include "fds.h"
+#include "nrf_strerror.h"
+#include "sdk_config.h"
+
+#if PM_HANDLER_SEC_DELAY_MS > 0
+#include "app_timer.h"
+#endif
+
+#define NRF_LOG_MODULE_NAME peer_manager_handler
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+#include "nrf_strerror.h"
+
+static const char * m_roles_str[] =
+{
+    "Invalid Role",
+    "Peripheral",
+    "Central",
+};
+
+static const char * m_sec_procedure_str[] =
+{
+    "Encryption",
+    "Bonding",
+    "Pairing",
+};
+
+#define PM_EVT_STR(_name) [_name] = STRINGIFY(_name)
+
+static const char * m_event_str[] =
+{
+    PM_EVT_STR(PM_EVT_BONDED_PEER_CONNECTED),
+    PM_EVT_STR(PM_EVT_CONN_CONFIG_REQ),
+    PM_EVT_STR(PM_EVT_CONN_SEC_START),
+    PM_EVT_STR(PM_EVT_CONN_SEC_SUCCEEDED),
+    PM_EVT_STR(PM_EVT_CONN_SEC_FAILED),
+    PM_EVT_STR(PM_EVT_CONN_SEC_CONFIG_REQ),
+    PM_EVT_STR(PM_EVT_CONN_SEC_PARAMS_REQ),
+    PM_EVT_STR(PM_EVT_STORAGE_FULL),
+    PM_EVT_STR(PM_EVT_ERROR_UNEXPECTED),
+    PM_EVT_STR(PM_EVT_PEER_DATA_UPDATE_SUCCEEDED),
+    PM_EVT_STR(PM_EVT_PEER_DATA_UPDATE_FAILED),
+    PM_EVT_STR(PM_EVT_PEER_DELETE_SUCCEEDED),
+    PM_EVT_STR(PM_EVT_PEER_DELETE_FAILED),
+    PM_EVT_STR(PM_EVT_PEERS_DELETE_SUCCEEDED),
+    PM_EVT_STR(PM_EVT_PEERS_DELETE_FAILED),
+    PM_EVT_STR(PM_EVT_LOCAL_DB_CACHE_APPLIED),
+    PM_EVT_STR(PM_EVT_LOCAL_DB_CACHE_APPLY_FAILED),
+    PM_EVT_STR(PM_EVT_SERVICE_CHANGED_IND_SENT),
+    PM_EVT_STR(PM_EVT_SERVICE_CHANGED_IND_CONFIRMED),
+    PM_EVT_STR(PM_EVT_SLAVE_SECURITY_REQ),
+    PM_EVT_STR(PM_EVT_FLASH_GARBAGE_COLLECTED),
+    PM_EVT_STR(PM_EVT_FLASH_GARBAGE_COLLECTION_FAILED),
+};
+
+static const char * m_data_id_str[] =
+{
+    "Outdated (0)",
+    "Service changed pending flag",
+    "Outdated (2)",
+    "Outdated (3)",
+    "Application data",
+    "Remote database",
+    "Peer rank",
+    "Bonding data",
+    "Local database",
+    "Central address resolution",
+};
+
+static const char * m_data_action_str[] =
+{
+    "Update",
+    "Delete"
+};
+
+#define PM_SEC_ERR_STR(_name) {.error = _name, .error_str = #_name}
+
+typedef struct
+{
+    pm_sec_error_code_t error;
+    const char * error_str;
+} sec_err_str_t;
+
+static const sec_err_str_t m_pm_sec_error_str[] =
+{
+    PM_SEC_ERR_STR(PM_CONN_SEC_ERROR_PIN_OR_KEY_MISSING),
+    PM_SEC_ERR_STR(PM_CONN_SEC_ERROR_MIC_FAILURE),
+    PM_SEC_ERR_STR(PM_CONN_SEC_ERROR_DISCONNECT),
+    PM_SEC_ERR_STR(PM_CONN_SEC_ERROR_SMP_TIMEOUT),
+};
+
+static const char * sec_err_string_get(pm_sec_error_code_t error)
+{
+    static char errstr[30];
+
+    for (uint32_t i = 0; i < (sizeof(m_pm_sec_error_str)/sizeof(sec_err_str_t)); i++)
+    {
+        if (m_pm_sec_error_str[i].error == error)
+        {
+            return m_pm_sec_error_str[i].error_str;
+        }
+    }
+
+    int len = snprintf(errstr, sizeof(errstr), "%s 0x%hx", (error < PM_CONN_SEC_ERROR_BASE)
+                                                            ? "BLE_GAP_SEC_STATUS"
+                                                            :"PM_CONN_SEC_ERROR", error);
+    UNUSED_VARIABLE(len);
+    return errstr;
+}
+
+
+static void _conn_secure(uint16_t conn_handle, bool force)
+{
+    ret_code_t err_code;
+
+    if (!force)
+    {
+        pm_conn_sec_status_t status;
+        err_code = pm_conn_sec_status_get(conn_handle, &status);
+        if (err_code != BLE_ERROR_INVALID_CONN_HANDLE)
+        {
+            APP_ERROR_CHECK(err_code);
+        }
+
+        // If the link is already secured, don't initiate security procedure.
+        if (status.encrypted)
+        {
+            NRF_LOG_DEBUG("Already encrypted, skipping security.");
+            return;
+        }
+    }
+
+    err_code = pm_conn_secure(conn_handle, false);
+
+    if ((err_code == NRF_SUCCESS) || (err_code == NRF_ERROR_BUSY))
+    {
+        // Success.
+    }
+    else if (err_code == NRF_ERROR_TIMEOUT)
+    {
+        NRF_LOG_WARNING("pm_conn_secure() failed because an SMP timeout is preventing security on "\
+                        "the link. Disconnecting conn_handle %d.",
+                        conn_handle);
+
+        err_code = sd_ble_gap_disconnect(conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_WARNING("sd_ble_gap_disconnect() returned %s on conn_handle %d.",
+                            nrf_strerror_get(err_code),
+                            conn_handle);
+        }
+    }
+    else if (err_code == NRF_ERROR_INVALID_DATA)
+    {
+        NRF_LOG_WARNING("pm_conn_secure() failed because the stored data for conn_handle %d does "\
+                        "not have a valid key.",
+                        conn_handle);
+    }
+    else if (err_code == BLE_ERROR_INVALID_CONN_HANDLE)
+    {
+        NRF_LOG_WARNING("pm_conn_secure() failed because conn_handle %d is not a valid connection.",
+                        conn_handle);
+    }
+    else
+    {
+        NRF_LOG_ERROR("Asserting. pm_conn_secure() returned %s on conn_handle %d.",
+                      nrf_strerror_get(err_code),
+                      conn_handle);
+        APP_ERROR_CHECK(err_code);
+    }
+}
+
+#if PM_HANDLER_SEC_DELAY_MS > 0
+APP_TIMER_DEF(secure_delay_timer);
+
+typedef union
+{
+    struct
+    {
+        uint16_t conn_handle;
+        bool     force;
+    } values;
+    void * p_void;
+} conn_secure_context_t;
+
+STATIC_ASSERT(sizeof(conn_secure_context_t) <= sizeof(void *), "conn_secure_context_t is too large.");
+
+static void _conn_secure(uint16_t conn_handle, bool force);
+
+static void delayed_conn_secure(void * context)
+{
+    conn_secure_context_t sec_context = {.p_void = context};
+    _conn_secure(sec_context.values.conn_handle, sec_context.values.force);
+}
+
+static void conn_secure(uint16_t conn_handle, bool force)
+{
+    ret_code_t err_code;
+    static bool created = false;
+    if (!created)
+    {
+        err_code = app_timer_create(&secure_delay_timer,
+                                    APP_TIMER_MODE_SINGLE_SHOT,
+                                    delayed_conn_secure);
+        APP_ERROR_CHECK(err_code);
+        created = true;
+    }
+
+    conn_secure_context_t sec_context = {0};
+    sec_context.values.conn_handle    = conn_handle;
+    sec_context.values.force          = force;
+
+    err_code = app_timer_start(secure_delay_timer,
+                               APP_TIMER_TICKS(PM_HANDLER_SEC_DELAY_MS),
+                               sec_context.p_void);
+    APP_ERROR_CHECK(err_code);
+}
+
+
+#else
+static void conn_secure(uint16_t conn_handle, bool force)
+{
+    _conn_secure(conn_handle, force);
+}
+#endif
+
+
+void pm_handler_on_pm_evt(pm_evt_t const * p_pm_evt)
+{
+    pm_handler_pm_evt_log(p_pm_evt);
+
+    if (p_pm_evt->evt_id == PM_EVT_BONDED_PEER_CONNECTED)
+    {
+        conn_secure(p_pm_evt->conn_handle, false);
+    }
+    else if (p_pm_evt->evt_id == PM_EVT_ERROR_UNEXPECTED)
+    {
+        NRF_LOG_ERROR("Asserting.");
+        APP_ERROR_CHECK(p_pm_evt->params.error_unexpected.error);
+    }
+}
+
+
+void pm_handler_flash_clean_on_return(void)
+{
+    // Trigger the mechanism to make more room in flash.
+    pm_evt_t storage_full_evt = {.evt_id = PM_EVT_STORAGE_FULL};
+    pm_handler_flash_clean(&storage_full_evt);
+}
+
+
+static void rank_highest(pm_peer_id_t peer_id)
+{
+    // Trigger a pm_peer_rank_highest() with internal bookkeeping.
+    pm_evt_t connected_evt = {.evt_id = PM_EVT_BONDED_PEER_CONNECTED, .peer_id = peer_id};
+    pm_handler_flash_clean(&connected_evt);
+}
+
+
+void pm_handler_flash_clean(pm_evt_t const * p_pm_evt)
+{
+    ret_code_t  err_code;
+    static bool flash_cleaning       = false;     // Indicates whether garbage collection is currently being run.
+    static bool flash_write_after_gc = true;      // Indicates whether a successful write happened after the last garbage
+                                                  // collection. If this is false when flash is full, it means just a
+                                                  // garbage collection won't work, so some data should be deleted.
+    #define RANK_QUEUE_SIZE 8                     // Size of rank_queue.
+    #define RANK_QUEUE_INIT() PM_PEER_ID_INVALID, // Initial value of rank_queue.
+
+    //lint -save -e40 -e26 -esym(628,MACRO_REPEAT_8)
+    static pm_peer_id_t rank_queue[8]  = {MACRO_REPEAT(RANK_QUEUE_SIZE, RANK_QUEUE_INIT)}; // Queue of rank_highest calls that
+                                                                                           // failed because of full flash.
+    //lint -restore
+    static int          rank_queue_wr = 0;        // Write pointer for rank_queue.
+
+    switch (p_pm_evt->evt_id)
+    {
+        case PM_EVT_BONDED_PEER_CONNECTED:
+            err_code = pm_peer_rank_highest(p_pm_evt->peer_id);
+            if ((err_code == NRF_ERROR_STORAGE_FULL) ||
+                (err_code == NRF_ERROR_BUSY))
+            {
+                // Queue pm_peer_rank_highest() call and attempt to clean flash.
+                rank_queue[rank_queue_wr] = p_pm_evt->peer_id;
+                rank_queue_wr = (rank_queue_wr + 1) % RANK_QUEUE_SIZE;
+                pm_handler_flash_clean_on_return();
+            }
+            else if ((err_code != NRF_ERROR_NOT_SUPPORTED) &&
+                     (err_code != NRF_ERROR_INVALID_PARAM) &&
+                     (err_code != NRF_ERROR_RESOURCES))
+            {
+                APP_ERROR_CHECK(err_code);
+            }
+            else
+            {
+                NRF_LOG_DEBUG("pm_peer_rank_highest() returned %s for peer id %d",
+                              nrf_strerror_get(err_code),
+                              p_pm_evt->peer_id);
+            }
+            break;
+
+        case PM_EVT_CONN_SEC_START:
+            break;
+
+        case PM_EVT_CONN_SEC_SUCCEEDED:
+            if (   (p_pm_evt->params.conn_sec_succeeded.procedure == PM_CONN_SEC_PROCEDURE_BONDING)
+                || (p_pm_evt->params.conn_sec_succeeded.procedure == PM_CONN_SEC_PROCEDURE_ENCRYPTION))
+                // PM_CONN_SEC_PROCEDURE_ENCRYPTION in case peer was not recognized at connection time.
+            {
+                rank_highest(p_pm_evt->peer_id);
+            }
+            break;
+
+        case PM_EVT_CONN_SEC_FAILED:
+        case PM_EVT_CONN_SEC_CONFIG_REQ:
+        case PM_EVT_CONN_SEC_PARAMS_REQ:
+            break;
+
+        case PM_EVT_STORAGE_FULL:
+            if (!flash_cleaning)
+            {
+                err_code = NRF_SUCCESS;
+                NRF_LOG_INFO("Attempting to clean flash.");
+                if (!flash_write_after_gc)
+                {
+                    // Check whether another user of FDS has deleted a record that can be GCed.
+                    fds_stat_t fds_stats;
+                    err_code = fds_stat(&fds_stats);
+                    APP_ERROR_CHECK(err_code);
+                    flash_write_after_gc = (fds_stats.dirty_records > 0);
+                }
+                if (!flash_write_after_gc)
+                {
+                    pm_peer_id_t peer_id_to_delete;
+                    err_code = pm_peer_ranks_get(NULL, NULL, &peer_id_to_delete, NULL);
+                    if (err_code == NRF_SUCCESS)
+                    {
+                        NRF_LOG_INFO("Deleting lowest ranked peer (peer_id: %d)", peer_id_to_delete);
+                        err_code = pm_peer_delete(peer_id_to_delete);
+                        APP_ERROR_CHECK(err_code);
+                        flash_write_after_gc = true;
+                    }
+                    if (err_code == NRF_ERROR_NOT_FOUND)
+                    {
+                        NRF_LOG_ERROR("There are no peers to delete.");
+                    }
+                    else if (err_code == NRF_ERROR_NOT_SUPPORTED)
+                    {
+                        NRF_LOG_WARNING("Peer ranks functionality is disabled, so no peers are deleted.");
+                    }
+                    else
+                    {
+                        APP_ERROR_CHECK(err_code);
+                    }
+                }
+                if (err_code == NRF_SUCCESS)
+                {
+                    err_code = fds_gc();
+                    if (err_code == NRF_SUCCESS)
+                    {
+                        NRF_LOG_DEBUG("Running flash garbage collection.");
+                        flash_cleaning = true;
+                    }
+                    else if (err_code != FDS_ERR_NO_SPACE_IN_QUEUES)
+                    {
+                        APP_ERROR_CHECK(err_code);
+                    }
+                }
+            }
+            break;
+
+        case PM_EVT_ERROR_UNEXPECTED:
+            break;
+
+        case PM_EVT_PEER_DATA_UPDATE_SUCCEEDED:
+            flash_write_after_gc = true;
+            break;
+
+        case PM_EVT_PEER_DATA_UPDATE_FAILED:
+            break;
+
+        case PM_EVT_PEER_DELETE_SUCCEEDED:
+            flash_write_after_gc = true;
+            break;
+
+        case PM_EVT_PEER_DELETE_FAILED:
+        case PM_EVT_PEERS_DELETE_SUCCEEDED:
+        case PM_EVT_PEERS_DELETE_FAILED:
+        case PM_EVT_LOCAL_DB_CACHE_APPLIED:
+        case PM_EVT_LOCAL_DB_CACHE_APPLY_FAILED:
+        case PM_EVT_SERVICE_CHANGED_IND_SENT:
+        case PM_EVT_SERVICE_CHANGED_IND_CONFIRMED:
+        case PM_EVT_SLAVE_SECURITY_REQ:
+            break;
+
+        case PM_EVT_FLASH_GARBAGE_COLLECTED:
+            flash_cleaning       = false;
+            flash_write_after_gc = false;
+            {
+                // Reattempt queued pm_peer_rank_highest() calls.
+                int rank_queue_rd = rank_queue_wr;
+                for (int i = 0; i < RANK_QUEUE_SIZE; i++)
+                {
+                    pm_peer_id_t peer_id = rank_queue[(i + rank_queue_rd) % RANK_QUEUE_SIZE];
+                    if (peer_id != PM_PEER_ID_INVALID)
+                    {
+                        rank_queue[(i + rank_queue_rd) % RANK_QUEUE_SIZE] = PM_PEER_ID_INVALID;
+                        rank_highest(peer_id);
+                    }
+                }
+            }
+            break;
+
+        case PM_EVT_FLASH_GARBAGE_COLLECTION_FAILED:
+            flash_cleaning = false;
+
+            if (p_pm_evt->params.garbage_collection_failed.error == FDS_ERR_BUSY
+             || p_pm_evt->params.garbage_collection_failed.error == FDS_ERR_OPERATION_TIMEOUT)
+            {
+                // Retry immediately if error is transient.
+                pm_handler_flash_clean_on_return();
+            }
+            break;
+
+        default:
+            break;
+    }
+}
+
+
+void pm_handler_pm_evt_log(pm_evt_t const * p_pm_evt)
+{
+    NRF_LOG_DEBUG("Event %s", m_event_str[p_pm_evt->evt_id]);
+
+    switch (p_pm_evt->evt_id)
+    {
+        case PM_EVT_BONDED_PEER_CONNECTED:
+            NRF_LOG_DEBUG("Previously bonded peer connected: role: %s, conn_handle: %d, peer_id: %d",
+                          m_roles_str[ble_conn_state_role(p_pm_evt->conn_handle)],
+                          p_pm_evt->conn_handle,
+                          p_pm_evt->peer_id);
+            break;
+
+        case PM_EVT_CONN_CONFIG_REQ:
+            NRF_LOG_DEBUG("Connection configuration request");
+            break;
+
+        case PM_EVT_CONN_SEC_START:
+            NRF_LOG_DEBUG("Connection security procedure started: role: %s, conn_handle: %d, procedure: %s",
+                          m_roles_str[ble_conn_state_role(p_pm_evt->conn_handle)],
+                          p_pm_evt->conn_handle,
+                          m_sec_procedure_str[p_pm_evt->params.conn_sec_start.procedure]);
+            break;
+
+        case PM_EVT_CONN_SEC_SUCCEEDED:
+            NRF_LOG_INFO("Connection secured: role: %s, conn_handle: %d, procedure: %s",
+                         m_roles_str[ble_conn_state_role(p_pm_evt->conn_handle)],
+                         p_pm_evt->conn_handle,
+                         m_sec_procedure_str[p_pm_evt->params.conn_sec_start.procedure]);
+            break;
+
+        case PM_EVT_CONN_SEC_FAILED:
+            NRF_LOG_INFO("Connection security failed: role: %s, conn_handle: 0x%x, procedure: %s, error: %d",
+                         m_roles_str[ble_conn_state_role(p_pm_evt->conn_handle)],
+                         p_pm_evt->conn_handle,
+                         m_sec_procedure_str[p_pm_evt->params.conn_sec_start.procedure],
+                         p_pm_evt->params.conn_sec_failed.error);
+            NRF_LOG_DEBUG("Error (decoded): %s",
+                          sec_err_string_get(p_pm_evt->params.conn_sec_failed.error));
+            break;
+
+        case PM_EVT_CONN_SEC_CONFIG_REQ:
+            NRF_LOG_DEBUG("Security configuration request");
+            break;
+
+        case PM_EVT_CONN_SEC_PARAMS_REQ:
+            NRF_LOG_DEBUG("Security parameter request");
+            break;
+
+        case PM_EVT_STORAGE_FULL:
+            NRF_LOG_WARNING("Flash storage is full");
+            break;
+
+        case PM_EVT_ERROR_UNEXPECTED:
+            NRF_LOG_ERROR("Unexpected fatal error occurred: error: %s",
+                          nrf_strerror_get(p_pm_evt->params.error_unexpected.error));
+            break;
+
+        case PM_EVT_PEER_DATA_UPDATE_SUCCEEDED:
+            NRF_LOG_INFO("Peer data updated in flash: peer_id: %d, data_id: %s, action: %s%s",
+                          p_pm_evt->peer_id,
+                          m_data_id_str[p_pm_evt->params.peer_data_update_succeeded.data_id],
+                          m_data_action_str[p_pm_evt->params.peer_data_update_succeeded.action],
+                          p_pm_evt->params.peer_data_update_succeeded.flash_changed ? "" : ", no change");
+            break;
+
+        case PM_EVT_PEER_DATA_UPDATE_FAILED:
+            // This can happen if the SoftDevice is too busy with BLE operations.
+            NRF_LOG_WARNING("Peer data updated failed: peer_id: %d, data_id: %s, action: %s, error: %s",
+                            p_pm_evt->peer_id,
+                            m_data_id_str[p_pm_evt->params.peer_data_update_failed.data_id],
+                            m_data_action_str[p_pm_evt->params.peer_data_update_succeeded.action],
+                            nrf_strerror_get(p_pm_evt->params.peer_data_update_failed.error));
+            break;
+
+        case PM_EVT_PEER_DELETE_SUCCEEDED:
+            NRF_LOG_ERROR("Peer deleted successfully: peer_id: %d", p_pm_evt->peer_id);
+            break;
+
+        case PM_EVT_PEER_DELETE_FAILED:
+            NRF_LOG_ERROR("Peer deletion failed: peer_id: %d, error: %s",
+                          p_pm_evt->peer_id,
+                          nrf_strerror_get(p_pm_evt->params.peer_delete_failed.error));
+            break;
+
+        case PM_EVT_PEERS_DELETE_SUCCEEDED:
+            NRF_LOG_INFO("All peers deleted.");
+            break;
+
+        case PM_EVT_PEERS_DELETE_FAILED:
+            NRF_LOG_ERROR("All peer deletion failed: error: %s",
+                          nrf_strerror_get(p_pm_evt->params.peers_delete_failed_evt.error));
+            break;
+
+        case PM_EVT_LOCAL_DB_CACHE_APPLIED:
+            NRF_LOG_DEBUG("Previously stored local DB applied: conn_handle: %d, peer_id: %d",
+                          p_pm_evt->conn_handle,
+                          p_pm_evt->peer_id);
+            break;
+
+        case PM_EVT_LOCAL_DB_CACHE_APPLY_FAILED:
+            // This can happen when the local DB has changed.
+            NRF_LOG_WARNING("Local DB could not be applied: conn_handle: %d, peer_id: %d",
+                            p_pm_evt->conn_handle,
+                            p_pm_evt->peer_id);
+            break;
+
+        case PM_EVT_SERVICE_CHANGED_IND_SENT:
+            NRF_LOG_DEBUG("Sending Service Changed indication.");
+            break;
+
+        case PM_EVT_SERVICE_CHANGED_IND_CONFIRMED:
+            NRF_LOG_DEBUG("Service Changed indication confirmed.");
+            break;
+
+        case PM_EVT_SLAVE_SECURITY_REQ:
+            NRF_LOG_DEBUG("Security Request received from peer.");
+            break;
+
+        case PM_EVT_FLASH_GARBAGE_COLLECTED:
+            NRF_LOG_DEBUG("Flash garbage collection complete.");
+            break;
+
+        case PM_EVT_FLASH_GARBAGE_COLLECTION_FAILED:
+            NRF_LOG_WARNING("Flash garbage collection failed with error %s.",
+                            nrf_strerror_get(p_pm_evt->params.garbage_collection_failed.error));
+            break;
+
+        default:
+            NRF_LOG_WARNING("Unexpected PM event ID: 0x%x.", p_pm_evt->evt_id);
+            break;
+    }
+}
+
+
+void pm_handler_disconnect_on_sec_failure(pm_evt_t const * p_pm_evt)
+{
+    ret_code_t err_code;
+
+    if (p_pm_evt->evt_id == PM_EVT_CONN_SEC_FAILED)
+    {
+        NRF_LOG_WARNING("Disconnecting conn_handle %d.", p_pm_evt->conn_handle);
+        err_code = sd_ble_gap_disconnect(p_pm_evt->conn_handle,
+                                         BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
+        if ((err_code != NRF_ERROR_INVALID_STATE) && (err_code != BLE_ERROR_INVALID_CONN_HANDLE))
+        {
+            APP_ERROR_CHECK(err_code);
+        }
+    }
+}
+
+
+void pm_handler_disconnect_on_insufficient_sec(pm_evt_t const * p_pm_evt,
+                                               pm_conn_sec_status_t * p_min_conn_sec)
+{
+    if (p_pm_evt->evt_id == PM_EVT_CONN_SEC_SUCCEEDED)
+    {
+        if (!pm_sec_is_sufficient(p_pm_evt->conn_handle, p_min_conn_sec))
+        {
+            NRF_LOG_WARNING("Connection security is insufficient, disconnecting.");
+            ret_code_t err_code = sd_ble_gap_disconnect(p_pm_evt->conn_handle,
+                                                    BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
+            APP_ERROR_CHECK(err_code);
+        }
+    }
+}
+
+
+void pm_handler_secure_on_connection(ble_evt_t const * p_ble_evt)
+{
+    switch (p_ble_evt->header.evt_id)
+    {
+        case BLE_GAP_EVT_CONNECTED:
+            NRF_LOG_DEBUG("Connected, securing connection. conn_handle: %d", p_ble_evt->evt.gap_evt.conn_handle);
+            conn_secure(p_ble_evt->evt.gap_evt.conn_handle, false);
+            break;
+
+#if PM_HANDLER_SEC_DELAY_MS > 0
+        case BLE_GAP_EVT_DISCONNECTED:
+        {
+            ret_code_t err_code = app_timer_stop(secure_delay_timer);
+            APP_ERROR_CHECK(err_code);
+        } break;
+#endif
+
+        default:
+            break;
+    }
+}
+
+
+void pm_handler_secure_on_error(ble_evt_t const * p_ble_evt)
+{
+    if ((p_ble_evt->header.evt_id >= BLE_GATTC_EVT_BASE) && (p_ble_evt->header.evt_id <= BLE_GATTC_EVT_LAST))
+    {
+        if ((p_ble_evt->evt.gattc_evt.gatt_status == BLE_GATT_STATUS_ATTERR_INSUF_ENCRYPTION) ||
+            (p_ble_evt->evt.gattc_evt.gatt_status == BLE_GATT_STATUS_ATTERR_INSUF_AUTHENTICATION))
+        {
+            NRF_LOG_INFO("GATTC procedure (evt id 0x%x) failed because it needs encryption. Bonding: conn_handle=%d",
+                         p_ble_evt->header.evt_id,
+                         p_ble_evt->evt.gattc_evt.conn_handle);
+            conn_secure(p_ble_evt->evt.gattc_evt.conn_handle, true);
+        }
+    }
+}

components/ble/peer_manager/peer_manager_handler.h

@@ -0,0 +1,166 @@
+/**
+ * Copyright (c) 2018 - 2021, 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 peer_manager_handler.h
+ *
+ * @defgroup pm_handler Peer Manager Standard Event Handlers
+ * @ingroup peer_manager
+ * @{
+ * @brief Standard event handlers implementing some best practices for BLE security.
+ */
+
+
+#ifndef PEER_MANAGER_HANDLER_H__
+#define PEER_MANAGER_HANDLER_H__
+
+#include "ble.h"
+#include "peer_manager.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@brief Standard function for making Peer Manager calls based on Peer Manager events.
+ *
+ * This function does the following:
+ *  - Logs all PM events using @ref nrf_log, at different severity levels.
+ *  - Starts encryption if connected to an already bonded peer. This is affected by @ref
+ *    PM_HANDLER_SEC_DELAY_MS.
+ *  - Calls @ref app_error on fatal errors.
+ *
+ * @note In normal circumstances, this function should be called for every Peer Manager event.
+ *
+ * @param[in]  p_pm_evt  Peer Manager event to handle.
+ */
+void pm_handler_on_pm_evt(pm_evt_t const * p_pm_evt);
+
+/**@brief Auxiliary standard function for logging Peer Manager events.
+ *
+ * This function logs all PM events using @ref nrf_log, at different severity levels. The
+ * @ref PM_LOG_ENABLED and other @c PM_LOG_* configs control these log messages.
+ *
+ * @note This function is called internally by @ref pm_handler_on_pm_evt.
+ *
+ * @param[in]  p_pm_evt  Peer Manager event to log.
+ */
+void pm_handler_pm_evt_log(pm_evt_t const * p_pm_evt);
+
+/**@brief Auxiliary standard function for maintaining room in flash based on Peer Manager events.
+ *
+ * This function does the following:
+ *  - Ranks peers by when they last connected.
+ *  - Garbage collects the flash when needed.
+ *  - Deletes the lowest ranked peer(s) when garbage collection is insufficient.
+ *
+ * @note See also @ref pm_handler_flash_clean_on_return.
+ * @note In normal circumstances, this function should be called for every Peer Manager event.
+ * @note This function is a supplement to @ref pm_handler_on_pm_evt, not its replacement.
+ *
+ * @param[in]  p_pm_evt  Peer Manager event to handle.
+ */
+void pm_handler_flash_clean(pm_evt_t const * p_pm_evt);
+
+/**@brief Function to call when a Peer Manager function returns @ref NRF_ERROR_STORAGE_FULL.
+ *
+ * @note This should only be used if @ref pm_handler_flash_clean is also used.
+ */
+void pm_handler_flash_clean_on_return(void);
+
+/**@brief Auxiliary standard function for disconnecting when the connection could not be secured.
+ *
+ * This function disconnects whenever connection security fails, i.e. whenever it receives a
+ * @ref PM_EVT_CONN_SEC_FAILED.
+ *
+ * @note In normal circumstances, this function should be called for every Peer Manager event.
+ * @note This function is a supplement to @ref pm_handler_on_pm_evt, not its replacement.
+ *
+ * @param[in]  p_pm_evt  Peer Manager event to handle.
+ */
+void pm_handler_disconnect_on_sec_failure(pm_evt_t const * p_pm_evt);
+
+/**@brief Auxiliary standard function for disconnecting on insufficient connection security.
+ *
+ * This function disconnects whenever the connection security succeeds, that is whenever it
+ * receives a @ref PM_EVT_CONN_SEC_SUCCEEDED, but the established security does not fulfill the
+ * provided criteria.
+ *
+ * @note In normal circumstances, this function should be called for every Peer Manager event.
+ * @note This function is a supplement to @ref pm_handler_on_pm_evt, not its replacement.
+ *
+ * @param[in]  p_pm_evt        Peer Manager event to handle.
+ * @param[in]  p_min_conn_sec  Minumum security status below which to disconnect the link.
+ */
+void pm_handler_disconnect_on_insufficient_sec(pm_evt_t const * p_pm_evt,
+                                               pm_conn_sec_status_t * p_min_conn_sec);
+
+/**@brief Function for securing a connection when it is established.
+ *
+ * This function starts security when receiving a @ref BLE_GAP_EVT_CONNECTED event. This is
+ * affected by @ref PM_HANDLER_SEC_DELAY_MS.
+ *
+ * @note In normal circumstances, this function should be called for every BLE event.
+ *
+ * @param[in]  p_ble_evt  BLE event to handle.
+ */
+void pm_handler_secure_on_connection(ble_evt_t const * p_ble_evt);
+
+/**@brief Function for securing a connection if a GATT read or write operation lacks security.
+ *
+ * This function starts pairing if a GATTC procedure fails with insufficient encryption
+ * or insufficient authentication. This is meant to delay performing pairing/bonding until
+ * it is actually needed to access resources. This is affected by @ref PM_HANDLER_SEC_DELAY_MS.
+ *
+ * @note When using this handler, the failed GATTC operation must be retried by the user.
+ * @note This does not work when using Write Without Response (@ref BLE_GATT_OP_WRITE_CMD) because
+ *       the server does not send any response, even on error. Instead, the write will be
+ *       silently dropped by the server.
+ * @note In normal circumstances, this function should be called for every BLE event.
+ *
+ * @param[in]  p_ble_evt  BLE event to handle.
+ */
+void pm_handler_secure_on_error(ble_evt_t const * p_ble_evt);
+
+#ifdef __cplusplus
+}
+#endif
+
+/** @}*/
+
+#endif // PEER_MANAGER_HANDLER_H__

components/ble/peer_manager/peer_manager_internal.h

@@ -0,0 +1,209 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef PEER_MANAGER_INTERNAL_H__
+#define PEER_MANAGER_INTERNAL_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @file peer_manager_types.h
+ *
+ * @addtogroup peer_manager
+ * @brief File containing definitions used solely inside the Peer Manager's modules.
+ * @{
+ */
+
+ANON_UNIONS_ENABLE;
+
+/**@brief One piece of data associated with a peer, together with its type.
+ *
+ * @note This type is deprecated.
+ */
+typedef struct
+{
+    uint16_t              length_words; /**< @brief The length of the data in words. */
+    pm_peer_data_id_t     data_id;      /**< @brief ID that specifies the type of data (defines which member of the union is used). */
+    union
+    {
+        pm_peer_data_bonding_t       * p_bonding_data;            /**< @brief The exchanged bond information in addition to metadata of the bonding. */
+        uint32_t                     * p_peer_rank;               /**< @brief A value locally assigned to this peer. Its interpretation is up to the user. The rank is not set automatically by the Peer Manager, but it is assigned by the user using either @ref pm_peer_rank_highest or a @ref PM_PEER_DATA_FUNCTIONS function. */
+        uint32_t                     * p_central_addr_res;        /**< @brief Value of peer's Central Address Resolution characteristic. */
+        bool                         * p_service_changed_pending; /**< @brief Whether a service changed indication should be sent to the peer. */
+        pm_peer_data_local_gatt_db_t * p_local_gatt_db;           /**< @brief Persistent information pertaining to a peer GATT client. */
+        ble_gatt_db_srv_t            * p_remote_gatt_db;          /**< @brief Persistent information pertaining to a peer GATT server. */
+        uint8_t                      * p_application_data;        /**< @brief Arbitrary data to associate with the peer. This data can be freely used by the application. */
+        void                         * p_all_data;                /**< @brief Generic access pointer to the data. It is used only to handle the data without regard to type. */
+    }; /**< @brief The data. */
+} pm_peer_data_t;
+
+
+/**@brief Immutable version of @ref pm_peer_data_t.
+ *
+ * @note This type is deprecated.
+ */
+typedef struct
+{
+    uint16_t                    length_words; /**< @brief The length of the data in words. */
+    pm_peer_data_id_t           data_id;      /**< @brief ID that specifies the type of data (defines which member of the union is used). */
+    union
+    {
+        pm_peer_data_bonding_t       const * p_bonding_data;            /**< @brief Immutable @ref pm_peer_data_t::p_bonding_data. */
+        uint32_t                     const * p_peer_rank;               /**< @brief Immutable @ref pm_peer_data_t::p_peer_rank. */
+        uint32_t                     const * p_central_addr_res;        /**< @brief Immutable @ref pm_peer_data_t::p_central_addr_res. */
+        bool                         const * p_service_changed_pending; /**< @brief Immutable @ref pm_peer_data_t::p_service_changed_pending. */
+        pm_peer_data_local_gatt_db_t const * p_local_gatt_db;           /**< @brief Immutable @ref pm_peer_data_t::p_local_gatt_db. */
+        ble_gatt_db_srv_t            const * p_remote_gatt_db;          /**< @brief Immutable @ref pm_peer_data_t::p_remote_gatt_db. */
+        uint8_t                      const * p_application_data;        /**< @brief Immutable @ref pm_peer_data_t::p_application_data. */
+        void                         const * p_all_data;                /**< @brief Immutable @ref pm_peer_data_t::p_all_data. */
+    }; /**< @brief The data. */
+} pm_peer_data_const_t;
+
+ANON_UNIONS_DISABLE;
+
+
+/**@brief Version of @ref pm_peer_data_t that reflects the structure of peer data in flash.
+ *
+ * @note This type is deprecated.
+ */
+typedef pm_peer_data_const_t pm_peer_data_flash_t;
+
+
+/**@brief Event handler for events from the @ref peer_manager module.
+ *
+ * @sa pm_register
+ *
+ * @param[in]  p_event  The event that has occurred.
+ */
+typedef void (*pm_evt_handler_internal_t)(pm_evt_t * p_event);
+
+
+/**@brief Macro for calculating the flash size of bonding data.
+ *
+ * @return The number of words that the data takes in flash.
+ */
+#define PM_BONDING_DATA_N_WORDS() BYTES_TO_WORDS(sizeof(pm_peer_data_bonding_t))
+
+
+/**@brief Macro for calculating the flash size of service changed pending state.
+ *
+ * @return The number of words that the data takes in flash.
+ */
+#define PM_SC_STATE_N_WORDS() BYTES_TO_WORDS(sizeof(bool))
+
+
+/**@brief Macro for calculating the flash size of local GATT database data.
+ *
+ * @param[in]  local_db_len  The length, in bytes, of the database as reported by the SoftDevice.
+ *
+ * @return The number of words that the data takes in flash.
+ */
+#define PM_LOCAL_DB_N_WORDS(local_db_len) \
+                                    BYTES_TO_WORDS((local_db_len) + PM_LOCAL_DB_LEN_OVERHEAD_BYTES)
+
+
+/**@brief Macro for calculating the length of a local GATT database attribute array.
+ *
+ * @param[in]  n_words  The number of words that the data takes in flash.
+ *
+ * @return The length of the database attribute array.
+ */
+#define PM_LOCAL_DB_LEN(n_words) (((n_words) * BYTES_PER_WORD) - PM_LOCAL_DB_LEN_OVERHEAD_BYTES)
+
+
+/**@brief Macro for calculating the flash size of remote GATT database data.
+ *
+ * @param[in]  service_count  The number of services in the service array.
+ *
+ * @return The number of words that the data takes in flash.
+ */
+#define PM_REMOTE_DB_N_WORDS(service_count) BYTES_TO_WORDS(sizeof(ble_gatt_db_srv_t) * (service_count))
+
+
+/**@brief Macro for calculating the flash size of remote GATT database data.
+ *
+ * @param[in]  n_words  The length in number of words.
+ *
+ * @return The number of words that the data takes in flash.
+ */
+#define PM_REMOTE_DB_N_SERVICES(n_words) (((n_words) * BYTES_PER_WORD) / sizeof(ble_gatt_db_srv_t))
+
+
+/**@brief Function for calculating the flash size of the usage index.
+ *
+ * @return The number of words that the data takes in flash.
+ */
+#define PM_USAGE_INDEX_N_WORDS() BYTES_TO_WORDS(sizeof(uint32_t))
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef NRF_PM_DEBUG
+
+    #define NRF_PM_DEBUG_CHECK(condition)   \
+        if (!(condition))                   \
+        {                                   \
+            __asm("bkpt #0");               \
+        }
+
+#else
+
+    // Prevent "variable set but never used" compiler warnings.
+    #define NRF_PM_DEBUG_CHECK(condition)   (void)(condition)
+
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PEER_MANAGER_INTERNAL_H__ */

components/ble/peer_manager/peer_manager_types.h

@@ -0,0 +1,369 @@
+/**
+ * Copyright (c) 2015 - 2021, 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 peer_manager_types.h
+ *
+ * @addtogroup peer_manager
+ * @{
+ */
+
+#ifndef PEER_MANAGER_TYPES_H__
+#define PEER_MANAGER_TYPES_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include "nrf.h"
+#include "ble_gap.h"
+#include "ble_hci.h"
+#include "ble_gatt_db.h"
+#include "app_util.h"
+#include "app_util_platform.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**@brief Handle to uniquely identify a peer for which we have persistently stored data.
+ */
+typedef uint16_t pm_peer_id_t;
+
+/**@brief Type that is used to hold a reference to a stored item in flash.
+ */
+typedef uint32_t pm_store_token_t;
+
+/**@brief Errors from security procedures in Peer Manager.
+ *
+ * @details Possible values are defined in @ref PM_SEC_ERRORS and @ref BLE_GAP_SEC_STATUS.
+ */
+typedef uint16_t pm_sec_error_code_t;
+
+
+//lint -emacro(516,PM_LOCAL_DB_LEN_OVERHEAD_BYTES)
+
+#define PM_PEER_ID_INVALID              0xFFFF                                         /**< @brief Invalid value for @ref pm_peer_id_t. */
+#define PM_STORE_TOKEN_INVALID          0                                              /**< @brief Invalid value for store token. */
+#define PM_PEER_ID_N_AVAILABLE_IDS      256                                            /**< @brief The number of available peer IDs. */
+#define PM_LOCAL_DB_LEN_OVERHEAD_BYTES  offsetof(pm_peer_data_local_gatt_db_t, data)   /**< @brief The static-length part of the local GATT data struct. */
+
+
+#define PM_CONN_SEC_ERROR_BASE 0x1000  /**< @brief The base for Peer Manager defined errors. See @ref PM_SEC_ERRORS and @ref pm_sec_error_code_t. */
+
+
+/**@defgroup PM_SEC_ERRORS Peer Manager defined security errors
+ *
+ * @details The first 256 numbers, from PM_CONN_SEC_ERROR_BASE to (PM_CONN_SEC_ERROR_BASE + 0xFF),
+ *          correspond to the status codes in @ref BLE_HCI_STATUS_CODES.
+ * @{ */
+#define PM_CONN_SEC_ERROR_PIN_OR_KEY_MISSING (PM_CONN_SEC_ERROR_BASE + 0x06)  /**< @brief Encryption failed because the peripheral has lost the LTK for this bond. See also @ref BLE_HCI_STATUS_CODE_PIN_OR_KEY_MISSING and Table 3.7 ("Pairing Failed Reason Codes") in the Bluetooth Core Specification 4.2, section 3.H.3.5.5 (@linkBLEcore).  */
+#define PM_CONN_SEC_ERROR_MIC_FAILURE        (PM_CONN_SEC_ERROR_BASE + 0x3D)  /**< @brief Encryption ended with disconnection because of mismatching keys or a stray packet during a procedure. See the SoftDevice GAP Message Sequence Charts on encryption (@linkBLEMSCgap), the Bluetooth Core Specification 4.2, sections 6.B.5.1.3.1 and 3.H.3.5.5 (@linkBLEcore), and @ref BLE_HCI_CONN_TERMINATED_DUE_TO_MIC_FAILURE. */
+#define PM_CONN_SEC_ERROR_DISCONNECT         (PM_CONN_SEC_ERROR_BASE + 0x100) /**< @brief Pairing or encryption did not finish before the link disconnected for an unrelated reason. */
+#define PM_CONN_SEC_ERROR_SMP_TIMEOUT        (PM_CONN_SEC_ERROR_BASE + 0x101) /**< @brief Pairing/bonding could not start because an SMP time-out has already happened on this link. This means that no more pairing or bonding can happen on this link. To be able to pair or bond, the link must be disconnected and then reconnected. See Bluetooth Core Specification 4.2 section 3.H.3.4 (@linkBLEcore). */
+ /** @} */
+
+
+
+/**@defgroup PM_PEER_ID_VERSIONS All versions of Peer IDs.
+ * @brief The data ID for each iteration of the data formats in flash.
+ * @details Each time the format (in flash) of a piece of peer data changes, the data ID will also
+ *          be updated. This list of defines is a record of each data ID that has ever existed, and
+ *          code that caters to legacy formats can find the relevant IDs here.
+ * @{ */
+#define PM_PEER_DATA_ID_FIRST_VX                   0     /**< @brief The smallest data ID. */
+#define PM_PEER_DATA_ID_BONDING_V1                 0     /**< @brief The data ID of the first version of bonding data. */
+#define PM_PEER_DATA_ID_BONDING_V2                 7     /**< @brief The data ID of the second version of bonding data. */
+#define PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING_V1 1     /**< @brief The data ID of the first version of the service changed pending flag. */
+#define PM_PEER_DATA_ID_GATT_LOCAL_V1              2     /**< @brief The data ID of the first version of local GATT data. */
+#define PM_PEER_DATA_ID_GATT_LOCAL_V2              8     /**< @brief The data ID of the second version of local GATT data. */
+#define PM_PEER_DATA_ID_GATT_REMOTE_V1             3     /**< @brief The data ID of the first version of remote GATT data. */
+#define PM_PEER_DATA_ID_APPLICATION_V1             4     /**< @brief The data ID of the first version of application data. */
+#define PM_PEER_DATA_ID_GATT_REMOTE_V2             5     /**< @brief The data ID of the second version of remote GATT data. */
+#define PM_PEER_DATA_ID_PEER_RANK_V1               6     /**< @brief The data ID of the first version of the rank. */
+#define PM_PEER_DATA_ID_CENTRAL_ADDR_RES_V1        9     /**< @brief The data ID of the first version of central address resolution. */
+#define PM_PEER_DATA_ID_LAST_VX                    10     /**< @brief The data ID after the last valid one. */
+#define PM_PEER_DATA_ID_INVALID_VX                 0xFF  /**< @brief A data ID guaranteed to be invalid. */
+/**@}*/
+
+
+/**@brief The different types of data associated with a peer.
+ */
+typedef enum
+{
+    PM_PEER_DATA_ID_FIRST                   = PM_PEER_DATA_ID_FIRST_VX,                   /**< @brief The smallest data ID. */
+    PM_PEER_DATA_ID_BONDING                 = PM_PEER_DATA_ID_BONDING_V2,                 /**< @brief The data ID for bonding data. Type: @ref pm_peer_data_bonding_t. */
+    PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING = PM_PEER_DATA_ID_SERVICE_CHANGED_PENDING_V1, /**< @brief The data ID for service changed state. Type: bool. */
+    PM_PEER_DATA_ID_GATT_LOCAL              = PM_PEER_DATA_ID_GATT_LOCAL_V2,              /**< @brief The data ID for local GATT data (sys attributes). Type: @ref pm_peer_data_local_gatt_db_t. */
+    PM_PEER_DATA_ID_GATT_REMOTE             = PM_PEER_DATA_ID_GATT_REMOTE_V2,             /**< @brief The data ID for remote GATT data. Type: uint8_t array. */
+    PM_PEER_DATA_ID_PEER_RANK               = PM_PEER_DATA_ID_PEER_RANK_V1,               /**< @brief The data ID for peer rank. See @ref pm_peer_rank_highest. Type: uint32_t. */
+    PM_PEER_DATA_ID_CENTRAL_ADDR_RES        = PM_PEER_DATA_ID_CENTRAL_ADDR_RES_V1,        /**< @brief The data ID for central address resolution. See @ref pm_peer_id_list. Type: uint32_t. */
+    PM_PEER_DATA_ID_APPLICATION             = PM_PEER_DATA_ID_APPLICATION_V1,             /**< @brief The data ID for application data. Type: uint8_t array. */
+    PM_PEER_DATA_ID_LAST                    = PM_PEER_DATA_ID_LAST_VX,                    /**< @brief One more than the highest data ID. */
+    PM_PEER_DATA_ID_INVALID                 = PM_PEER_DATA_ID_INVALID_VX,                 /**< @brief A data ID guaranteed to be invalid. */
+} pm_peer_data_id_t;
+
+
+/**@brief Different procedures that can lead to an encrypted link.
+ */
+typedef enum
+{
+    PM_CONN_SEC_PROCEDURE_ENCRYPTION, /**< @brief Using an LTK that was shared during a previous bonding procedure to encrypt the link. */
+    PM_CONN_SEC_PROCEDURE_BONDING,    /**< @brief A pairing procedure, followed by a bonding procedure. */
+    PM_CONN_SEC_PROCEDURE_PAIRING,    /**< @brief A pairing procedure with no bonding. */
+} pm_conn_sec_procedure_t;
+
+
+/**@brief Configuration of a security procedure.
+ */
+typedef struct
+{
+    bool allow_repairing; /** @brief Whether to allow the peer to pair if it wants to, but is already bonded. If this is false, the procedure is rejected, and no more events are sent. Default: false. */
+} pm_conn_sec_config_t;
+
+
+/**@brief Data associated with a bond to a peer.
+ */
+typedef struct
+{
+    uint8_t           own_role;    /**< @brief The BLE role of the local device during bonding. See @ref BLE_GAP_ROLES. */
+    ble_gap_id_key_t  peer_ble_id; /**< @brief The peer's Bluetooth address and identity resolution key (IRK). */
+    ble_gap_enc_key_t peer_ltk;    /**< @brief The peer's long-term encryption key (LTK) and master ID. */
+    ble_gap_enc_key_t own_ltk;     /**< @brief Locally generated long-term encryption key (LTK) and master ID, distributed to the peer. */
+} pm_peer_data_bonding_t;
+
+
+/**@brief Data on a local GATT database.
+ */
+typedef struct
+{
+    uint32_t flags;       /**< @brief Flags that describe the database attributes. */
+    uint16_t len;         /**< @brief Size of the attribute array. */
+    uint8_t  data[1];     /**< @brief Array to hold the database attributes. */
+} pm_peer_data_local_gatt_db_t;
+
+
+/**@brief Device Privacy.
+ *
+ *        The privacy feature provides a way for the device to avoid being tracked over a period of
+ *        time. The privacy feature, when enabled, hides the local device identity and replaces it
+ *        with a private address that is automatically refreshed at a specified interval.
+ *
+ *        If a device still wants to be recognized by other peers, it needs to share it's Identity
+ *        Resolving Key (IRK). With this key, a device can generate a random private address that
+ *        can only be recognized by peers in possession of that key, and devices can establish
+ *        connections without revealing their real identities.
+ *
+ * @note  If the device IRK is updated, the new IRK becomes the one to be distributed in all
+ *        bonding procedures performed after @ref sd_ble_gap_privacy_set returns.
+ *        The IRK distributed during bonding procedure is the device IRK that is active when @ref
+ *        sd_ble_gap_sec_params_reply is called.
+ */
+typedef ble_gap_privacy_params_t pm_privacy_params_t;
+
+
+/**@brief Security status of a connection.
+ */
+typedef struct
+{
+    uint8_t connected      : 1; /**< @brief The connection is active (not disconnected). */
+    uint8_t encrypted      : 1; /**< @brief The communication on this link is encrypted. */
+    uint8_t mitm_protected : 1; /**< @brief The encrypted communication is also protected against man-in-the-middle attacks. */
+    uint8_t bonded         : 1; /**< @brief The peer is bonded. */
+    uint8_t lesc           : 1; /**< @brief The peer is paired using LESC. */
+    uint8_t reserved       : 3; /**< @brief Reserved for future use. */
+} pm_conn_sec_status_t;
+
+
+/**@brief Types of events that can come from the @ref peer_manager module.
+ */
+typedef enum
+{
+    PM_EVT_BONDED_PEER_CONNECTED,           /**< @brief A connected peer has been identified as one with which we have a bond. When performing bonding with a peer for the first time, this event will not be sent until a new connection is established with the peer. When we are central, this event is always sent when the Peer Manager receives the @ref BLE_GAP_EVT_CONNECTED event. When we are peripheral, this event might in rare cases arrive later. */
+    PM_EVT_CONN_CONFIG_REQ,                 /**< @brief A new connection has been established. This event is a wrapper for @ref BLE_GAP_EVT_CONNECTED event and contains its parameters. Reply with @ref pm_conn_exclude before the event handler returns to exclude BLE events targeting this connection from being handled by the Peer Manager */
+    PM_EVT_CONN_SEC_START,                  /**< @brief A security procedure has started on a link, initiated either locally or remotely. The security procedure is using the last parameters provided via @ref pm_sec_params_set. This event is always followed by either a @ref PM_EVT_CONN_SEC_SUCCEEDED or a @ref PM_EVT_CONN_SEC_FAILED event. This is an informational event; no action is needed for the procedure to proceed. */
+    PM_EVT_CONN_SEC_SUCCEEDED,              /**< @brief A link has been encrypted, either as a result of a call to @ref pm_conn_secure or a result of an action by the peer. The event structure contains more information about the circumstances. This event might contain a peer ID with the value @ref PM_PEER_ID_INVALID, which means that the peer (central) used an address that could not be identified, but it used an encryption key (LTK) that is present in the database. */
+    PM_EVT_CONN_SEC_FAILED,                 /**< @brief A pairing or encryption procedure has failed. In some cases, this means that security is not possible on this link (temporarily or permanently). How to handle this error depends on the application. */
+    PM_EVT_CONN_SEC_CONFIG_REQ,             /**< @brief The peer (central) has requested pairing, but a bond already exists with that peer. Reply by calling @ref pm_conn_sec_config_reply before the event handler returns. If no reply is sent, a default is used. */
+    PM_EVT_CONN_SEC_PARAMS_REQ,             /**< @brief Security parameters (@ref ble_gap_sec_params_t) are needed for an ongoing security procedure. Reply with @ref pm_conn_sec_params_reply before the event handler returns. If no reply is sent, the parameters given in @ref pm_sec_params_set are used. If a peripheral connection, the central's sec_params will be available in the event. */
+    PM_EVT_STORAGE_FULL,                    /**< @brief There is no more room for peer data in flash storage. To solve this problem, delete data that is not needed anymore and run a garbage collection procedure in FDS. */
+    PM_EVT_ERROR_UNEXPECTED,                /**< @brief An unrecoverable error happened inside Peer Manager. An operation failed with the provided error. */
+    PM_EVT_PEER_DATA_UPDATE_SUCCEEDED,      /**< @brief A piece of peer data was stored, updated, or cleared in flash storage. This event is sent for all successful changes to peer data, also those initiated internally in Peer Manager. To identify an operation, compare the store token in the event with the store token received during the initiating function call. Events from internally initiated changes might have invalid store tokens. */
+    PM_EVT_PEER_DATA_UPDATE_FAILED,         /**< @brief A piece of peer data could not be stored, updated, or cleared in flash storage. This event is sent instead of @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED for the failed operation. */
+    PM_EVT_PEER_DELETE_SUCCEEDED,           /**< @brief A peer was cleared from flash storage, for example because a call to @ref pm_peer_delete succeeded. This event can also be sent as part of a call to @ref pm_peers_delete or internal cleanup. */
+    PM_EVT_PEER_DELETE_FAILED,              /**< @brief A peer could not be cleared from flash storage. This event is sent instead of @ref PM_EVT_PEER_DELETE_SUCCEEDED for the failed operation. */
+    PM_EVT_PEERS_DELETE_SUCCEEDED,          /**< @brief A call to @ref pm_peers_delete has completed successfully. Flash storage now contains no peer data. */
+    PM_EVT_PEERS_DELETE_FAILED,             /**< @brief A call to @ref pm_peers_delete has failed, which means that at least one of the peers could not be deleted. Other peers might have been deleted, or might still be queued to be deleted. No more @ref PM_EVT_PEERS_DELETE_SUCCEEDED or @ref PM_EVT_PEERS_DELETE_FAILED events are sent until the next time @ref pm_peers_delete is called. */
+    PM_EVT_LOCAL_DB_CACHE_APPLIED,          /**< @brief Local database values for a peer (taken from flash storage) have been provided to the SoftDevice. */
+    PM_EVT_LOCAL_DB_CACHE_APPLY_FAILED,     /**< @brief Local database values for a peer (taken from flash storage) were rejected by the SoftDevice, which means that either the database has changed or the user has manually set the local database to an invalid value (using @ref pm_peer_data_store). */
+    PM_EVT_SERVICE_CHANGED_IND_SENT,        /**< @brief A service changed indication has been sent to a peer, as a result of a call to @ref pm_local_database_has_changed. This event will be followed by a @ref PM_EVT_SERVICE_CHANGED_IND_CONFIRMED event if the peer acknowledges the indication. */
+    PM_EVT_SERVICE_CHANGED_IND_CONFIRMED,   /**< @brief A service changed indication that was sent has been confirmed by a peer. The peer can now be considered aware that the local database has changed. */
+    PM_EVT_SLAVE_SECURITY_REQ,              /**< @brief The peer (peripheral) has requested link encryption, which has been enabled. */
+    PM_EVT_FLASH_GARBAGE_COLLECTED,         /**< @brief The flash has been garbage collected (By FDS), possibly freeing up space. */
+    PM_EVT_FLASH_GARBAGE_COLLECTION_FAILED, /**< @brief Garbage collection was attempted but failed. */
+} pm_evt_id_t;
+
+
+/**@brief Parameters specific to the @ref PM_EVT_CONN_CONFIG_REQ event.
+ */
+typedef struct
+{
+    ble_gap_evt_connected_t const * p_peer_params; /**< @brief Connected Event parameters. */
+    void                    const * p_context;     /**< @brief This pointer must be provided in the reply if the reply function takes a p_context argument. */
+} pm_conn_config_req_evt_t;
+
+
+/**@brief Events parameters specific to the @ref PM_EVT_CONN_SEC_START event.
+ */
+typedef struct
+{
+    pm_conn_sec_procedure_t procedure; /**< @brief The procedure that has started. */
+} pm_conn_sec_start_evt_t;
+
+
+/**@brief Parameters specific to the @ref PM_EVT_CONN_SEC_SUCCEEDED event.
+ */
+typedef struct
+{
+    pm_conn_sec_procedure_t procedure;   /**< @brief The procedure that led to securing the link. */
+    bool                    data_stored; /**< @brief Whether bonding data was successfully requested to be stored. This is false if: No bonding happened, or an internal error occurred when trying to store the data, or if the data was rejected via @ref pm_conn_sec_config_reply. */
+} pm_conn_secured_evt_t;
+
+
+/**@brief Parameters specific to the @ref PM_EVT_CONN_SEC_FAILED event.
+ */
+typedef struct
+{
+    pm_conn_sec_procedure_t procedure; /**< @brief The procedure that failed. */
+    pm_sec_error_code_t     error;     /**< @brief An error code that describes the failure. */
+    uint8_t                 error_src; /**< @brief The party that raised the error, see @ref BLE_GAP_SEC_STATUS_SOURCES. */
+} pm_conn_secure_failed_evt_t;
+
+
+/**@brief Parameters specific to the @ref PM_EVT_CONN_SEC_PARAMS_REQ event.
+ */
+typedef struct
+{
+    ble_gap_sec_params_t const * p_peer_params; /**< @brief Peer security parameters, if role is peripheral. Otherwise, this is NULL. */
+    void                 const * p_context;     /**< @brief This pointer must be provided in the reply if the reply function takes a p_context argument. */
+} pm_conn_sec_params_req_evt_t;
+
+
+/**@brief Actions that can be performed to peer data in persistent storage.
+ */
+typedef enum
+{
+    PM_PEER_DATA_OP_UPDATE, /**< @brief Writing or overwriting the data. */
+    PM_PEER_DATA_OP_DELETE, /**< @brief Removing the data. */
+} pm_peer_data_op_t;
+
+
+/**@brief Parameters specific to the @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED event.
+ */
+typedef struct
+{
+    pm_peer_data_id_t data_id;           /**< @brief The type of the data that was changed. */
+    pm_peer_data_op_t action;            /**< @brief What happened to the data. */
+    pm_store_token_t  token;             /**< @brief Token that identifies the operation. For @ref PM_PEER_DATA_OP_DELETE actions, this token can be disregarded. For @ref PM_PEER_DATA_OP_UPDATE actions, compare this token with the token that is received from a call to a @ref PM_PEER_DATA_FUNCTIONS function. */
+    uint8_t           flash_changed : 1; /**< @brief If this is false, no operation was done in flash, because the value was already what it should be. Please note that in certain scenarios, this flag will be true even if the new value is the same as the old. */
+} pm_peer_data_update_succeeded_evt_t;
+
+
+/**@brief Parameters specific to the @ref PM_EVT_PEER_DATA_UPDATE_FAILED event.
+ */
+typedef struct
+{
+    pm_peer_data_id_t data_id;   /**< @brief The type of the data that was supposed to be changed. */
+    pm_peer_data_op_t action;    /**< @brief The action that failed. */
+    pm_store_token_t  token;     /**< @brief Token that identifies the operation. For @ref PM_PEER_DATA_OP_DELETE actions, this token can be disregarded. For @ref PM_PEER_DATA_OP_UPDATE actions, compare this token with the token that is received from a call to a @ref PM_PEER_DATA_FUNCTIONS function. */
+    ret_code_t        error;     /**< @brief An error code that describes the failure. */
+} pm_peer_data_update_failed_t;
+
+
+/**@brief Standard parameters for failure events.
+ */
+typedef struct
+{
+    ret_code_t error;     /**< @brief The error that occurred. */
+} pm_failure_evt_t;
+
+
+/**@brief An event from the @ref peer_manager module.
+ *
+ * @details The structure contains both standard parameters and parameters that are specific to some events.
+ */
+typedef struct
+{
+    pm_evt_id_t  evt_id;      /**< @brief The type of the event. */
+    uint16_t     conn_handle; /**< @brief The connection that this event pertains to, or @ref BLE_CONN_HANDLE_INVALID. */
+    pm_peer_id_t peer_id;     /**< @brief The bonded peer that this event pertains to, or @ref PM_PEER_ID_INVALID. */
+    union
+    {
+        pm_conn_config_req_evt_t            conn_config_req;            /**< @brief Parameters specific to the @ref PM_EVT_CONN_CONFIG_REQ event. */
+        pm_conn_sec_start_evt_t             conn_sec_start;             /**< @brief Parameters specific to the @ref PM_EVT_CONN_SEC_START event. */
+        pm_conn_secured_evt_t               conn_sec_succeeded;         /**< @brief Parameters specific to the @ref PM_EVT_CONN_SEC_SUCCEEDED event. */
+        pm_conn_secure_failed_evt_t         conn_sec_failed;            /**< @brief Parameters specific to the @ref PM_EVT_CONN_SEC_FAILED event. */
+        pm_conn_sec_params_req_evt_t        conn_sec_params_req;        /**< @brief Parameters specific to the @ref PM_EVT_CONN_SEC_PARAMS_REQ event. */
+        pm_peer_data_update_succeeded_evt_t peer_data_update_succeeded; /**< @brief Parameters specific to the @ref PM_EVT_PEER_DATA_UPDATE_SUCCEEDED event. */
+        pm_peer_data_update_failed_t        peer_data_update_failed;    /**< @brief Parameters specific to the @ref PM_EVT_PEER_DATA_UPDATE_FAILED event. */
+        pm_failure_evt_t                    peer_delete_failed;         /**< @brief Parameters specific to the @ref PM_EVT_PEER_DELETE_FAILED event. */
+        pm_failure_evt_t                    peers_delete_failed_evt;    /**< @brief Parameters specific to the @ref PM_EVT_PEERS_DELETE_FAILED event. */
+        pm_failure_evt_t                    error_unexpected;           /**< @brief Parameters specific to the @ref PM_EVT_ERROR_UNEXPECTED event. */
+        ble_gap_evt_sec_request_t           slave_security_req;         /**< @brief Parameters specific to the @ref PM_EVT_SLAVE_SECURITY_REQ event. */
+        pm_failure_evt_t                    garbage_collection_failed;  /**< @brief Parameters specific to the @ref PM_EVT_FLASH_GARBAGE_COLLECTION_FAILED event. */
+    } params;
+} pm_evt_t;
+
+
+/**@brief Event handler for events from the @ref peer_manager module.
+ *
+ * @sa pm_register
+ *
+ * @param[in]  p_event  The event that has occurred.
+ */
+typedef void (*pm_evt_handler_t)(pm_evt_t const * p_event);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PEER_MANAGER_TYPES_H__ */
+
+/** @} */

components/ble/peer_manager/pm_buffer.c

@@ -0,0 +1,170 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "pm_buffer.h"
+
+#include <stdbool.h>
+#include <string.h>
+#include "nrf_error.h"
+#include "nrf_atflags.h"
+
+
+#define BUFFER_IS_VALID(p_buffer) ((p_buffer != NULL)             \
+                                && (p_buffer->p_memory != NULL)   \
+                                && (p_buffer->p_mutex  != NULL))
+
+
+
+static bool mutex_lock(nrf_atflags_t * p_mutex, uint32_t mutex_id)
+{
+    bool locked = !nrf_atflags_fetch_set(p_mutex, mutex_id);
+    __DMB();
+    return locked;
+}
+
+
+static void mutex_unlock(nrf_atflags_t * p_mutex, uint32_t mutex_id)
+{
+    __DMB();
+    nrf_atflags_clear(p_mutex, mutex_id);
+}
+
+
+static bool mutex_lock_status_get(nrf_atflags_t * p_mutex, uint32_t mutex_id)
+{
+    __DMB();
+    return nrf_atflags_get(p_mutex, mutex_id);
+}
+
+
+ret_code_t pm_buffer_init(pm_buffer_t   * p_buffer,
+                          uint8_t       * p_buffer_memory,
+                          uint32_t        buffer_memory_size,
+                          nrf_atflags_t * p_mutex_memory,
+                          uint32_t        n_blocks,
+                          uint32_t        block_size)
+{
+    if (   (p_buffer           != NULL)
+        && (p_buffer_memory    != NULL)
+        && (p_mutex_memory     != NULL)
+        && (buffer_memory_size >= (n_blocks * block_size))
+        && (n_blocks           != 0)
+        && (block_size         != 0))
+    {
+        p_buffer->p_memory   = p_buffer_memory;
+        p_buffer->p_mutex    = p_mutex_memory;
+        p_buffer->n_blocks   = n_blocks;
+        p_buffer->block_size = block_size;
+
+        return NRF_SUCCESS;
+    }
+    else
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+}
+
+
+uint8_t pm_buffer_block_acquire(pm_buffer_t * p_buffer, uint32_t n_blocks)
+{
+    if (!BUFFER_IS_VALID(p_buffer))
+    {
+        return ( PM_BUFFER_INVALID_ID );
+    }
+
+    uint8_t first_locked_mutex = PM_BUFFER_INVALID_ID;
+
+    for (uint8_t i = 0; i < p_buffer->n_blocks; i++)
+    {
+        if (mutex_lock(p_buffer->p_mutex, i))
+        {
+            if (first_locked_mutex == PM_BUFFER_INVALID_ID)
+            {
+                first_locked_mutex = i;
+            }
+            if ((i - first_locked_mutex + 1U) == n_blocks)
+            {
+                return first_locked_mutex;
+            }
+        }
+        else if (first_locked_mutex != PM_BUFFER_INVALID_ID)
+        {
+            for (uint8_t j = first_locked_mutex; j < i; j++)
+            {
+                pm_buffer_release(p_buffer, j);
+            }
+            first_locked_mutex = PM_BUFFER_INVALID_ID;
+        }
+    }
+
+    return ( PM_BUFFER_INVALID_ID );
+}
+
+
+uint8_t * pm_buffer_ptr_get(pm_buffer_t * p_buffer, uint8_t id)
+{
+    if (!BUFFER_IS_VALID(p_buffer))
+    {
+        return ( NULL );
+    }
+
+    if ( (id != PM_BUFFER_INVALID_ID)
+    &&   mutex_lock_status_get(p_buffer->p_mutex, id) )
+    {
+        return ( &p_buffer->p_memory[id * p_buffer->block_size] );
+    }
+    else
+    {
+        return ( NULL );
+    }
+}
+
+
+void pm_buffer_release(pm_buffer_t * p_buffer, uint8_t id)
+{
+    if (    BUFFER_IS_VALID(p_buffer)
+       &&  (id != PM_BUFFER_INVALID_ID)
+       &&   mutex_lock_status_get(p_buffer->p_mutex, id))
+    {
+        mutex_unlock(p_buffer->p_mutex, id);
+    }
+}
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/pm_buffer.h

@@ -0,0 +1,156 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef BUFFER_H__
+#define BUFFER_H__
+
+#include <stdint.h>
+#include "compiler_abstraction.h"
+#include "sdk_errors.h"
+#include "nrf_atflags.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup pm_buffer Buffer
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. This module provides a simple buffer.
+ */
+
+
+#define PM_BUFFER_INVALID_ID 0xFF //!< Invalid buffer block ID.
+
+
+/**@brief Convenience macro for declaring memory and initializing a buffer instance.
+ *
+ * @param[out] p_buffer    The buffer instance to initialize.
+ * @param[in]  n_blocks    The desired number of blocks in the buffer.
+ * @param[in]  block_size  The desired block size of the buffer.
+ * @param[out] err_code    The return code from @ref pm_buffer_init.
+ */
+#define PM_BUFFER_INIT(p_buffer, n_blocks, block_size, err_code)          \
+do                                                                        \
+{                                                                         \
+    __ALIGN(4) static uint8_t buffer_memory[(n_blocks) * (block_size)];   \
+    static NRF_ATFLAGS_DEF(mutex_memory, n_blocks);                       \
+    err_code = pm_buffer_init((p_buffer),                                 \
+                               buffer_memory,                             \
+                              (n_blocks) * (block_size),                  \
+                               mutex_memory,                              \
+                              (n_blocks),                                 \
+                              (block_size));                              \
+} while (0)
+
+
+typedef struct
+{
+    uint8_t       * p_memory;   /**< The storage for all buffer entries. The size of the buffer must be n_blocks*block_size. */
+    nrf_atflags_t * p_mutex;    /**< A mutex group with one mutex for each buffer entry. */
+    uint32_t        n_blocks;   /**< The number of allocatable blocks in the buffer. */
+    uint32_t        block_size; /**< The size of each block in the buffer. */
+} pm_buffer_t;
+
+/**@brief Function for initializing a buffer instance.
+ *
+ * @param[out] p_buffer            The buffer instance to initialize.
+ * @param[in]  p_buffer_memory     The memory this buffer will use.
+ * @param[in]  buffer_memory_size  The size of p_buffer_memory. This must be at least
+ *                                 n_blocks*block_size.
+ * @param[in]  p_mutex_memory      The memory for the mutexes. This must be at least
+ *                                 @ref NRF_ATFLAGS_ARRAY_LEN(n_blocks).
+ * @param[in]  n_blocks            The number of blocks in the buffer.
+ * @param[in]  block_size          The size of each block.
+ *
+ * @retval NRF_SUCCESS              Successfully initialized buffer instance.
+ * @retval NRF_ERROR_INVALID_PARAM  A parameter was 0 or NULL or a size was too small.
+ */
+ret_code_t pm_buffer_init(pm_buffer_t   * p_buffer,
+                          uint8_t       * p_buffer_memory,
+                          uint32_t        buffer_memory_size,
+                          nrf_atflags_t * p_mutex_memory,
+                          uint32_t        n_blocks,
+                          uint32_t        block_size);
+
+
+/**@brief Function for acquiring a buffer block in a buffer.
+ *
+ * @param[in]  p_buffer  The buffer instance acquire from.
+ * @param[in]  n_blocks  The number of contiguous blocks to acquire.
+ *
+ * @return The id of the acquired block, if successful.
+ * @retval PM_BUFFER_INVALID_ID  If unsuccessful.
+ */
+uint8_t pm_buffer_block_acquire(pm_buffer_t * p_buffer, uint32_t n_blocks);
+
+
+/**@brief Function for getting a pointer to a specific buffer block.
+ *
+ * @param[in]  p_buffer  The buffer instance get from.
+ * @param[in]  id        The id of the buffer to get the pointer for.
+ *
+ * @return A pointer to the buffer for the specified id, if the id is valid.
+ * @retval NULL  If the id is invalid.
+ */
+uint8_t * pm_buffer_ptr_get(pm_buffer_t * p_buffer, uint8_t id);
+
+
+/**@brief Function for releasing a buffer block.
+ *
+ * @param[in]  p_buffer  The buffer instance containing the block to release.
+ * @param[in]  id        The id of the block to release.
+ */
+void pm_buffer_release(pm_buffer_t * p_buffer, uint8_t id);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BUFFER_H__
+
+/**
+ * @}
+ * @endcond
+ */

components/ble/peer_manager/security_dispatcher.h

@@ -0,0 +1,162 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef SECURITY_DISPATCHER_H__
+#define SECURITY_DISPATCHER_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup security_dispatcher Security Dispatcher
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for streamlining pairing, bonding, and
+ *        encryption, including flash storage of shared data.
+ *
+ */
+
+
+/**@brief Function for initializing the Security Dispatcher module.
+ *
+ * @retval NRF_SUCCESS         Initialization was successful.
+ * @retval NRF_ERROR_INTERNAL  An unexpected fatal error occurred.
+ */
+ret_code_t smd_init(void);
+
+
+/**@brief Function for dispatching SoftDevice events to the Security Dispatcher module.
+ *
+ * @param[in]  ble_evt    The SoftDevice event.
+ */
+void smd_ble_evt_handler(ble_evt_t const * ble_evt);
+
+
+/**@brief Function for providing security configuration for a link.
+ *
+ * @details This function is optional, and must be called in reply to a @ref
+ *          PM_EVT_CONN_SEC_CONFIG_REQ event, before the Peer Manager event handler returns. If it
+ *          is not called in time, a default configuration is used. See @ref pm_conn_sec_config_t
+ *          for the value of the default.
+ *
+ * @param[in]  conn_handle        The connection to set the configuration for.
+ * @param[in]  p_conn_sec_config  The configuration.
+ */
+void smd_conn_sec_config_reply(uint16_t conn_handle, pm_conn_sec_config_t * p_conn_sec_config);
+
+
+/**@brief Function for providing pairing and bonding parameters to use for the current pairing
+ *        procedure on a connection.
+ *
+ * @note If this function returns an @ref NRF_ERROR_NULL, @ref NRF_ERROR_INVALID_PARAM, @ref
+ *       BLE_ERROR_INVALID_CONN_HANDLE, or @ref NRF_ERROR_STORAGE_FULL, this function can be called
+ *       again after corrective action.
+ *
+ * @note To reject a request, call this function with NULL p_sec_params.
+ *
+ * @param[in]  conn_handle   The connection handle of the connection the pairing is happening on.
+ * @param[in]  p_sec_params  The security parameters to use for this link.
+ * @param[in]  p_public_key  A pointer to the public key to use if using LESC, or NULL.
+ *
+ * @retval NRF_SUCCESS                    Success.
+ * @retval NRF_ERROR_INVALID_STATE        No parameters have been requested on that conn_handle, or
+ *                                        the link is disconnecting.
+ * @retval NRF_ERROR_INVALID_PARAM        Invalid combination of parameters (not including conn_handle).
+ * @retval NRF_ERROR_TIMEOUT              There has been an SMP timeout, so no more SMP operations
+ *                                        can be performed on this link.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  Invalid connection handle.
+ * @retval NRF_ERROR_BUSY                 No write buffer. Reattempt later.
+ * @retval NRF_ERROR_INTERNAL             A fatal error occurred.
+ */
+ret_code_t smd_params_reply(uint16_t                 conn_handle,
+                            ble_gap_sec_params_t   * p_sec_params,
+                            ble_gap_lesc_p256_pk_t * p_public_key);
+
+
+/**@brief Function for initiating security on the link, with the specified parameters.
+ *
+ * @note If the connection is a peripheral connection, this will send a security request to the
+ *       master, but the master is not obligated to initiate pairing or encryption in response.
+ * @note If the connection is a central connection and a key is available, the parameters will be
+ *       used to determine whether to re-pair or to encrypt using the existing key. If no key is
+ *       available, pairing will be started.
+ *
+ * @param[in]  conn_handle      Handle of the connection to initiate pairing on.
+ * @param[in]  p_sec_params     The security parameters to use for this link. As a central, this can
+ *                              be NULL to reject a slave security request.
+ * @param[in]  force_repairing  Whether to force a pairing procedure to happen regardless of whether
+ *                              an encryption key already exists. This argument is only relevant for
+ *                              the central role. Recommended value: false
+ *
+ * @retval NRF_SUCCESS                    Success.
+ * @retval NRF_ERROR_NULL                 p_sec_params was NULL (peripheral only).
+ * @retval NRF_ERROR_INVALID_STATE        A security procedure is already in progress on the link,
+ *                                        or the link is disconnecting.
+ * @retval NRF_ERROR_INVALID_PARAM        Invalid combination of parameters (not including conn_handle).
+ * @retval NRF_ERROR_INVALID_DATA         Peer is bonded, but no LTK was found, and repairing was
+ *                                        not requested.
+ * @retval NRF_ERROR_BUSY                 Unable to initiate procedure at this time.
+ * @retval NRF_ERROR_TIMEOUT              There has been an SMP timeout, so no more SMP operations
+ *                                        can be performed on this link.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  Invalid connection handle.
+ * @retval NRF_ERROR_INTERNAL             No more available peer IDs.
+ */
+ret_code_t smd_link_secure(uint16_t               conn_handle,
+                           ble_gap_sec_params_t * p_sec_params,
+                           bool                   force_repairing);
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* SECURITY_DISPATCHER_H__ */

components/ble/peer_manager/security_manager.c

@@ -0,0 +1,769 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+#if NRF_MODULE_ENABLED(PEER_MANAGER)
+#include "security_manager.h"
+
+#include <string.h>
+#include "ble_err.h"
+#include "security_dispatcher.h"
+#include "peer_database.h"
+#include "ble_conn_state.h"
+#include "id_manager.h"
+#include "sdk_common.h"
+
+#if PM_LESC_ENABLED
+#include "nrf_ble_lesc.h"
+#endif
+#define NRF_LOG_MODULE_NAME peer_manager_sm
+#if PM_LOG_ENABLED
+    #define NRF_LOG_LEVEL       PM_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR  PM_LOG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // PM_LOG_ENABLED
+#include "nrf_log.h"
+#include "nrf_log_ctrl.h"
+NRF_LOG_MODULE_REGISTER();
+#include "nrf_strerror.h"
+
+
+// The number of registered event handlers.
+#define SM_EVENT_HANDLERS_CNT       (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
+
+
+// Security Manager event handler in Peer Manager.
+extern void pm_sm_evt_handler(pm_evt_t * p_sm_evt);
+
+// Security Manager events' handlers.
+// The number of elements in this array is SM_EVENT_HANDLERS_CNT.
+static pm_evt_handler_internal_t const m_evt_handlers[] =
+{
+    pm_sm_evt_handler
+};
+
+
+// The context type that is used in PM_EVT_CONN_SEC_PARAMS_REQ events and in calls to sm_sec_params_reply().
+typedef struct
+{
+    ble_gap_sec_params_t * p_sec_params;         //!< The security parameters to use in the call to the security_dispatcher
+    ble_gap_sec_params_t   sec_params_mem;       //!< The buffer for holding the security parameters.
+    bool                   params_reply_called;  //!< Whether @ref sm_sec_params_reply has been called for this context instance.
+} sec_params_reply_context_t;
+
+static bool                            m_module_initialized;      //!< Whether the Security Manager module has been initialized.
+
+static ble_gap_sec_params_t            m_sec_params;              //!< The buffer for the default security parameters set by @ref sm_sec_params_set.
+static ble_gap_sec_params_t          * mp_sec_params = NULL;      //!< The default security parameters set by @ref sm_sec_params_set.
+static bool                            m_sec_params_set = false;  //!< Whether @ref sm_sec_params_set has been called.
+
+#if PM_LESC_ENABLED == 0
+static ble_gap_lesc_p256_pk_t        * m_p_public_key;            //!< Pointer, provided by the user, to the public key to use for LESC procedures.
+#endif
+
+static ble_conn_state_user_flag_id_t   m_flag_link_secure_pending_busy        = BLE_CONN_STATE_USER_FLAG_INVALID;  //!< User flag indicating whether a connection has a pending call to @ref sm_link_secure because it returned @ref NRF_ERROR_BUSY.
+static ble_conn_state_user_flag_id_t   m_flag_link_secure_force_repairing     = BLE_CONN_STATE_USER_FLAG_INVALID;  //!< User flag indicating whether a pending call to @ref sm_link_secure should be called with true for the force_repairing parameter.
+static ble_conn_state_user_flag_id_t   m_flag_link_secure_null_params         = BLE_CONN_STATE_USER_FLAG_INVALID;  //!< User flag indicating whether a pending call to @ref sm_link_secure should be called with NULL security parameters.
+static ble_conn_state_user_flag_id_t   m_flag_params_reply_pending_busy       = BLE_CONN_STATE_USER_FLAG_INVALID;  //!< User flag indicating whether a connection has a pending call to @ref sm_sec_params_reply because it returned @ref NRF_ERROR_BUSY.
+
+
+/**@brief Function for sending an SM event to all registered event handlers.
+ *
+ * @param[in]  p_event  The event to send.
+ */
+static void evt_send(pm_evt_t * p_event)
+{
+    for (uint32_t i = 0; i < SM_EVENT_HANDLERS_CNT; i++)
+    {
+        m_evt_handlers[i](p_event);
+    }
+}
+
+
+/**@brief Function for setting or clearing user flags based on error codes returned from @ref
+ *        smd_link_secure or @ref smd_params_reply.
+ *
+ * @param[in]  conn_handle   The connection the call pertained to.
+ * @param[in]  err_code      The error code returned from @ref smd_link_secure or
+ *                           @ref smd_params_reply.
+ * @param[in]  params_reply  Whether the call was to @ref smd_params_reply.
+ */
+static void flags_set_from_err_code(uint16_t conn_handle, ret_code_t err_code, bool params_reply)
+{
+    bool flag_value_busy       = false;
+
+    if (err_code == NRF_ERROR_BUSY)
+    {
+        flag_value_busy       = true;
+    }
+    else
+    {
+        flag_value_busy       = false;
+    }
+
+    if (params_reply)
+    {
+        ble_conn_state_user_flag_set(conn_handle,
+                                     m_flag_params_reply_pending_busy,
+                                     flag_value_busy);
+        ble_conn_state_user_flag_set(conn_handle,
+                                     m_flag_link_secure_pending_busy,
+                                     false);
+    }
+    else
+    {
+        ble_conn_state_user_flag_set(conn_handle,
+                                     m_flag_link_secure_pending_busy,
+                                     flag_value_busy);
+    }
+}
+
+
+static inline pm_evt_t new_evt(pm_evt_id_t evt_id, uint16_t conn_handle)
+{
+    pm_evt_t evt =
+    {
+        .evt_id = evt_id,
+        .conn_handle = conn_handle,
+        .peer_id = im_peer_id_get_by_conn_handle(conn_handle)
+    };
+    return evt;
+}
+
+
+/**@brief Function for sending a PM_EVT_ERROR_UNEXPECTED event.
+ *
+ * @param[in]  conn_handle  The connection handle the event pertains to.
+ * @param[in]  err_code     The unexpected error that occurred.
+ */
+static void send_unexpected_error(uint16_t conn_handle, ret_code_t err_code)
+{
+    pm_evt_t error_evt = new_evt(PM_EVT_ERROR_UNEXPECTED, conn_handle);
+    error_evt.params.error_unexpected.error = err_code;
+    evt_send(&error_evt);
+}
+
+
+/**@brief Returns whether the LTK came from LESC bonding.
+ *
+ * @param[in]  peer_id  The peer to check.
+ *
+ * @return  Whether the key is LESC or not.
+ */
+static bool key_is_lesc(pm_peer_id_t peer_id)
+{
+    pm_peer_data_flash_t peer_data;
+    ret_code_t           err_code;
+
+    err_code = pdb_peer_data_ptr_get(peer_id, PM_PEER_DATA_ID_BONDING, &peer_data);
+
+    return (err_code == NRF_SUCCESS) && (peer_data.p_bonding_data->own_ltk.enc_info.lesc);
+}
+
+
+/**@brief Function for sending an event based on error codes returned from @ref smd_link_secure or
+ *        @ref smd_params_reply.
+ *
+ * @param[in]  conn_handle   The connection the event pertains to.
+ * @param[in]  err_code      The error code returned from @ref smd_link_secure or
+ *                           @ref smd_params_reply.
+ * @param[in]  p_sec_params  The security parameters attempted to pass in the call to
+ *                           @ref smd_link_secure or @ref smd_params_reply.
+ */
+static void events_send_from_err_code(uint16_t               conn_handle,
+                                      ret_code_t             err_code,
+                                      ble_gap_sec_params_t * p_sec_params)
+{
+    if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_BUSY) && (err_code != NRF_ERROR_INVALID_STATE))
+    {
+        if (err_code == NRF_ERROR_TIMEOUT)
+        {
+            NRF_LOG_WARNING("Cannot secure link because a previous security procedure ended in timeout. "\
+                            "Disconnect and retry. smd_params_reply() or smd_link_secure() returned "\
+                            "NRF_ERROR_TIMEOUT. conn_handle: %d",
+                            conn_handle);
+
+            pm_evt_t evt = new_evt(PM_EVT_CONN_SEC_FAILED, conn_handle);
+            evt.params.conn_sec_failed.procedure = ((p_sec_params != NULL) && p_sec_params->bond)
+                                                 ? PM_CONN_SEC_PROCEDURE_BONDING
+                                                 : PM_CONN_SEC_PROCEDURE_PAIRING;
+            evt.params.conn_sec_failed.error_src = BLE_GAP_SEC_STATUS_SOURCE_LOCAL;
+            evt.params.conn_sec_failed.error     = PM_CONN_SEC_ERROR_SMP_TIMEOUT;
+            evt_send(&evt);
+        }
+        else
+        {
+            NRF_LOG_ERROR("Could not perform security procedure. smd_params_reply() or "\
+                          "smd_link_secure() returned %s. conn_handle: %d",
+                          nrf_strerror_get(err_code),
+                          conn_handle);
+            send_unexpected_error(conn_handle, err_code);
+        }
+    }
+}
+
+
+/**@brief Function for sending an PM_EVT_CONN_SEC_PARAMS_REQ event.
+ *
+ * @param[in]  conn_handle    The connection the event pertains to.
+ * @param[in]  p_peer_params  The peer's security parameters to include in the event. Can be NULL.
+ * @param[in]  p_context      Pointer to a context that the user must include in the call to @ref
+ *                            sm_sec_params_reply().
+ */
+static void params_req_send(uint16_t                     conn_handle,
+                            ble_gap_sec_params_t const * p_peer_params,
+                            sec_params_reply_context_t * p_context)
+{
+    pm_evt_t evt = new_evt(PM_EVT_CONN_SEC_PARAMS_REQ, conn_handle);
+    evt.params.conn_sec_params_req.p_peer_params = p_peer_params;
+    evt.params.conn_sec_params_req.p_context     = p_context;
+
+    evt_send(&evt);
+}
+
+
+/**@brief Function for creating a new @ref sec_params_reply_context_t with the correct initial values.
+ *
+ * @return  The new context.
+ */
+static sec_params_reply_context_t new_context_get(void)
+{
+    sec_params_reply_context_t new_context =
+    {
+        .p_sec_params = mp_sec_params,
+        .params_reply_called = false
+    };
+    return new_context;
+}
+
+
+/**@brief Internal function corresponding to @ref sm_link_secure.
+ *
+ * @param[in]  conn_handle      The connection to secure.
+ * @param[in]  null_params      Whether to pass NULL security parameters to the security_dispatcher.
+ * @param[in]  force_repairing  Whether to force rebonding if peer exists.
+ * @param[in]  send_events      Whether to send events based on the result of @ref smd_link_secure.
+ *
+ * @return  Same return codes as @ref sm_link_secure.
+ */
+static ret_code_t link_secure(uint16_t conn_handle,
+                              bool     null_params,
+                              bool     force_repairing,
+                              bool     send_events)
+{
+    ret_code_t             err_code;
+    ret_code_t             return_err_code;
+    ble_gap_sec_params_t * p_sec_params;
+
+    if (null_params)
+    {
+        p_sec_params = NULL;
+    }
+    else
+    {
+        sec_params_reply_context_t context = new_context_get();
+        params_req_send(conn_handle, NULL, &context);
+        p_sec_params = context.p_sec_params;
+
+        if (!m_sec_params_set && !context.params_reply_called)
+        {
+            // Security parameters have not been set.
+            return NRF_ERROR_NOT_FOUND;
+        }
+    }
+
+
+    err_code = smd_link_secure(conn_handle, p_sec_params, force_repairing);
+
+    flags_set_from_err_code(conn_handle, err_code, false);
+
+    switch (err_code)
+    {
+        case NRF_ERROR_BUSY:
+            ble_conn_state_user_flag_set(conn_handle, m_flag_link_secure_null_params, null_params);
+            ble_conn_state_user_flag_set(conn_handle, m_flag_link_secure_force_repairing, force_repairing);
+            return_err_code = NRF_SUCCESS;
+            break;
+        case NRF_SUCCESS:
+        case NRF_ERROR_TIMEOUT:
+        case BLE_ERROR_INVALID_CONN_HANDLE:
+        case NRF_ERROR_INVALID_STATE:
+        case NRF_ERROR_INVALID_DATA:
+            return_err_code = err_code;
+            break;
+        default:
+            NRF_LOG_ERROR("Could not perform security procedure. smd_link_secure() returned %s. "\
+                          "conn_handle: %d",
+                          nrf_strerror_get(err_code),
+                          conn_handle);
+            return_err_code = NRF_ERROR_INTERNAL;
+            break;
+    }
+
+    if (send_events)
+    {
+        events_send_from_err_code(conn_handle, err_code, p_sec_params);
+    }
+
+    return return_err_code;
+}
+
+
+/**@brief Function for requesting security parameters from the user and passing them to the security_dispatcher.
+ *
+ * @param[in]  conn_handle    The connection that needs security parameters.
+ * @param[in]  p_peer_params  The peer's security parameters if present. Otherwise NULL.
+ */
+static void smd_params_reply_perform(uint16_t conn_handle, ble_gap_sec_params_t const * p_peer_params)
+{
+    ret_code_t                 err_code;
+    ble_gap_lesc_p256_pk_t   * p_public_key;
+    sec_params_reply_context_t context = new_context_get();
+
+    params_req_send(conn_handle, p_peer_params, &context);
+
+#if PM_LESC_ENABLED
+    p_public_key = nrf_ble_lesc_public_key_get();
+#else
+    p_public_key = m_p_public_key;
+#endif // PM_LESC_ENABLED
+    err_code = smd_params_reply(conn_handle, context.p_sec_params, p_public_key);
+
+    flags_set_from_err_code(conn_handle, err_code, true);
+    events_send_from_err_code(conn_handle, err_code, context.p_sec_params);
+}
+
+
+/**@brief Function for handling @ref PM_EVT_CONN_SEC_PARAMS_REQ events.
+ *
+ * @param[in]  p_event  The @ref PM_EVT_CONN_SEC_PARAMS_REQ event.
+ */
+static __INLINE void params_req_process(pm_evt_t const * p_event)
+{
+    smd_params_reply_perform(p_event->conn_handle, p_event->params.conn_sec_params_req.p_peer_params);
+}
+
+
+ret_code_t sm_conn_sec_status_get(uint16_t conn_handle, pm_conn_sec_status_t * p_conn_sec_status)
+{
+    VERIFY_PARAM_NOT_NULL(p_conn_sec_status);
+
+    ble_conn_state_status_t status = ble_conn_state_status(conn_handle);
+
+    if (status == BLE_CONN_STATUS_INVALID)
+    {
+        return BLE_ERROR_INVALID_CONN_HANDLE;
+    }
+
+    pm_peer_id_t peer_id = im_peer_id_get_by_conn_handle(conn_handle);
+
+    p_conn_sec_status->connected      = (status == BLE_CONN_STATUS_CONNECTED);
+    p_conn_sec_status->bonded         = (peer_id != PM_PEER_ID_INVALID);
+    p_conn_sec_status->encrypted      = ble_conn_state_encrypted(conn_handle);
+    p_conn_sec_status->mitm_protected = ble_conn_state_mitm_protected(conn_handle);
+    p_conn_sec_status->lesc           = ble_conn_state_lesc(conn_handle)
+                                        || (ble_conn_state_encrypted(conn_handle)
+                                            && key_is_lesc(peer_id));
+    return NRF_SUCCESS;
+}
+
+
+bool sm_sec_is_sufficient(uint16_t conn_handle, pm_conn_sec_status_t * p_sec_status_req)
+{
+    pm_conn_sec_status_t sec_status = {.reserved = ~0}; // Set all bits in reserved to 1 so they are
+                                                        // ignored in subsequent logic.
+    ret_code_t err_code = sm_conn_sec_status_get(conn_handle, &sec_status);
+
+    STATIC_ASSERT(sizeof(pm_conn_sec_status_t) == sizeof(uint8_t));
+
+    uint8_t unmet_reqs = (~(*((uint8_t *) &sec_status)) & *((uint8_t *) p_sec_status_req));
+
+    return (err_code == NRF_SUCCESS) && !unmet_reqs;
+}
+
+
+/**@brief Function for handling @ref PM_EVT_SLAVE_SECURITY_REQ events.
+ *
+ * @param[in]  p_event  The @ref PM_EVT_SLAVE_SECURITY_REQ event.
+ */
+static void sec_req_process(pm_evt_t const * p_event)
+{
+    ret_code_t err_code;
+    bool null_params = false;
+    bool force_repairing = false;
+
+    if (mp_sec_params == NULL)
+    {
+        null_params = true;
+    }
+    else if (ble_conn_state_encrypted(p_event->conn_handle))
+    {
+        pm_conn_sec_status_t sec_status_req =
+        {
+            .bonded         = p_event->params.slave_security_req.bond,
+            .mitm_protected = p_event->params.slave_security_req.mitm,
+            .lesc           = p_event->params.slave_security_req.lesc,
+        };
+
+        force_repairing = !sm_sec_is_sufficient(p_event->conn_handle, &sec_status_req);
+    }
+
+    err_code = link_secure(p_event->conn_handle, null_params, force_repairing, true);
+    UNUSED_VARIABLE(err_code); // The error code has been properly handled inside link_secure().
+}
+
+
+/**@brief Function for translating an SMD event to an SM event and passing it on to SM event handlers.
+ *
+ * @param[in]  p_event  The event to forward.
+ */
+static void evt_forward(pm_evt_t * p_event)
+{
+    evt_send(p_event);
+}
+
+
+/**@brief Event handler for events from the Security Dispatcher module.
+ *        This handler is extern in Security Dispatcher.
+ *
+ * @param[in]  p_event   The event that has happened.
+ */
+void sm_smd_evt_handler(pm_evt_t * p_event)
+{
+    switch (p_event->evt_id)
+    {
+        case PM_EVT_CONN_SEC_PARAMS_REQ:
+            params_req_process(p_event);
+            break;
+        case PM_EVT_SLAVE_SECURITY_REQ:
+            sec_req_process(p_event);
+            /* fallthrough */
+        default:
+            // Forward the event to all registered Security Manager event handlers.
+            evt_forward(p_event);
+            break;
+    }
+}
+
+
+/**@brief Function handling a pending params_reply. See @ref ble_conn_state_user_function_t.
+ */
+static void params_reply_pending_handle(uint16_t conn_handle, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+    smd_params_reply_perform(conn_handle, NULL);
+}
+
+
+/**@brief Function handling a pending link_secure. See @ref ble_conn_state_user_function_t.
+ */
+static void link_secure_pending_handle(uint16_t conn_handle, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+
+    bool force_repairing = ble_conn_state_user_flag_get(conn_handle, m_flag_link_secure_force_repairing);
+    bool null_params     = ble_conn_state_user_flag_get(conn_handle, m_flag_link_secure_null_params);
+
+    // If this fails, it will be automatically retried.
+    ret_code_t err_code = link_secure(conn_handle, null_params, force_repairing, true);
+    UNUSED_VARIABLE(err_code);
+}
+
+
+/**@brief Event handler for events from the Peer Database module.
+ *        This handler is extern in Peer Database.
+ *
+ * @param[in]  p_event   The event that has happened.
+ */
+void sm_pdb_evt_handler(pm_evt_t * p_event)
+{
+    switch (p_event->evt_id)
+    {
+        case PM_EVT_FLASH_GARBAGE_COLLECTED:
+        case PM_EVT_PEER_DATA_UPDATE_SUCCEEDED:
+        case PM_EVT_PEER_DATA_UPDATE_FAILED:
+        case PM_EVT_PEER_DELETE_SUCCEEDED:
+        case PM_EVT_PEER_DELETE_FAILED:
+            (void) ble_conn_state_for_each_set_user_flag(m_flag_params_reply_pending_busy,
+                                                         params_reply_pending_handle,
+                                                         NULL);
+            (void) ble_conn_state_for_each_set_user_flag(m_flag_link_secure_pending_busy,
+                                                         link_secure_pending_handle,
+                                                         NULL);
+            break;
+        default:
+            // Do nothing.
+            break;
+    }
+}
+
+
+/**@brief Funtion for initializing a BLE Connection State user flag.
+ *
+ * @param[out] flag_id  The flag to initialize.
+ */
+static void flag_id_init(ble_conn_state_user_flag_id_t * p_flag_id)
+{
+    if (*p_flag_id == BLE_CONN_STATE_USER_FLAG_INVALID)
+    {
+        *p_flag_id = ble_conn_state_user_flag_acquire();
+    }
+}
+
+
+ret_code_t sm_init(void)
+{
+    NRF_PM_DEBUG_CHECK(!m_module_initialized);
+
+#if PM_LESC_ENABLED
+    ret_code_t err_code = nrf_ble_lesc_init();
+    if (err_code != NRF_SUCCESS)
+    {
+        return err_code;
+    }
+#endif
+
+    flag_id_init(&m_flag_link_secure_pending_busy);
+    flag_id_init(&m_flag_link_secure_force_repairing);
+    flag_id_init(&m_flag_link_secure_null_params);
+    flag_id_init(&m_flag_params_reply_pending_busy);
+
+    if (m_flag_params_reply_pending_busy == BLE_CONN_STATE_USER_FLAG_INVALID)
+    {
+        NRF_LOG_ERROR("Could not acquire conn_state user flags. Increase "\
+                      "BLE_CONN_STATE_USER_FLAG_COUNT in the ble_conn_state module.");
+        return NRF_ERROR_INTERNAL;
+    }
+
+    m_module_initialized = true;
+
+    return NRF_SUCCESS;
+}
+
+
+void sm_ble_evt_handler(ble_evt_t const * p_ble_evt)
+{
+    NRF_PM_DEBUG_CHECK(p_ble_evt != NULL);
+
+    smd_ble_evt_handler(p_ble_evt);
+#if PM_LESC_ENABLED
+    nrf_ble_lesc_on_ble_evt(p_ble_evt);
+#endif
+    (void) ble_conn_state_for_each_set_user_flag(m_flag_params_reply_pending_busy,
+                                                 params_reply_pending_handle,
+                                                 NULL);
+    (void) ble_conn_state_for_each_set_user_flag(m_flag_link_secure_pending_busy,
+                                                 link_secure_pending_handle,
+                                                 NULL);
+}
+
+
+/**@brief Funtion for checking whether security parameters are valid.
+ *
+ * @param[out] p_sec_params  The security parameters to verify.
+ *
+ * @return  Whether the security parameters are valid.
+ */
+static bool sec_params_verify(ble_gap_sec_params_t * p_sec_params)
+{
+    // NULL check.
+    if (p_sec_params == NULL)
+    {
+        return false;
+    }
+
+    // OOB not allowed unless MITM.
+    if (!p_sec_params->mitm && p_sec_params->oob)
+    {
+        return false;
+    }
+
+    // IO Capabilities must be one of the valid values from @ref BLE_GAP_IO_CAPS.
+    if (p_sec_params->io_caps > BLE_GAP_IO_CAPS_KEYBOARD_DISPLAY)
+    {
+        return false;
+    }
+
+    // Must have either IO capabilities or OOB if MITM.
+    if (p_sec_params->mitm && (p_sec_params->io_caps == BLE_GAP_IO_CAPS_NONE) && !p_sec_params->oob)
+    {
+        return false;
+    }
+
+    // Minimum key size cannot be larger than maximum key size.
+    if (p_sec_params->min_key_size > p_sec_params->max_key_size)
+    {
+        return false;
+    }
+
+    // Key size cannot be below 7 bytes.
+    if (p_sec_params->min_key_size < 7)
+    {
+        return false;
+    }
+
+    // Key size cannot be above 16 bytes.
+    if (p_sec_params->max_key_size > 16)
+    {
+        return false;
+    }
+
+    // Signing is not supported.
+    if (p_sec_params->kdist_own.sign || p_sec_params->kdist_peer.sign)
+    {
+        return false;
+    }
+
+    // link bit must be 0.
+    if (p_sec_params->kdist_own.link || p_sec_params->kdist_peer.link)
+    {
+        return false;
+    }
+
+    // If bonding is not enabled, no keys can be distributed.
+    if (!p_sec_params->bond && (   p_sec_params->kdist_own.enc
+                                || p_sec_params->kdist_own.id
+                                || p_sec_params->kdist_peer.enc
+                                || p_sec_params->kdist_peer.id))
+    {
+        return false;
+    }
+
+    // If bonding is enabled, one or more keys must be distributed.
+    if (    p_sec_params->bond
+        && !p_sec_params->kdist_own.enc
+        && !p_sec_params->kdist_own.id
+        && !p_sec_params->kdist_peer.enc
+        && !p_sec_params->kdist_peer.id)
+    {
+        return false;
+    }
+
+    return true;
+}
+
+
+ret_code_t sm_sec_params_set(ble_gap_sec_params_t * p_sec_params)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    if (p_sec_params == NULL)
+    {
+        mp_sec_params    = NULL;
+        m_sec_params_set = true;
+        return NRF_SUCCESS;
+    }
+    else if (sec_params_verify(p_sec_params))
+    {
+        m_sec_params     = *p_sec_params;
+        mp_sec_params    = &m_sec_params;
+        m_sec_params_set = true;
+        return NRF_SUCCESS;
+    }
+    else
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+}
+
+
+void sm_conn_sec_config_reply(uint16_t conn_handle, pm_conn_sec_config_t * p_conn_sec_config)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    NRF_PM_DEBUG_CHECK(p_conn_sec_config != NULL);
+
+    smd_conn_sec_config_reply(conn_handle, p_conn_sec_config);
+}
+
+
+ret_code_t sm_sec_params_reply(uint16_t               conn_handle,
+                               ble_gap_sec_params_t * p_sec_params,
+                               void           const * p_context)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+    VERIFY_PARAM_NOT_NULL(p_context);
+
+    sec_params_reply_context_t * p_sec_params_reply_context = (sec_params_reply_context_t *)p_context;
+    if (p_sec_params == NULL)
+    {
+        // Set the store pointer to NULL, so that NULL is passed to the SoftDevice.
+        p_sec_params_reply_context->p_sec_params = NULL;
+    }
+    else if (sec_params_verify(p_sec_params))
+    {
+        // Copy the provided sec_params into the store.
+        p_sec_params_reply_context->sec_params_mem = *p_sec_params;
+        p_sec_params_reply_context->p_sec_params   = &p_sec_params_reply_context->sec_params_mem;
+    }
+    else
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+    p_sec_params_reply_context->params_reply_called = true;
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t sm_lesc_public_key_set(ble_gap_lesc_p256_pk_t * p_public_key)
+{
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+#if PM_LESC_ENABLED
+    return NRF_ERROR_FORBIDDEN;
+#else
+    m_p_public_key = p_public_key;
+
+    return NRF_SUCCESS;
+#endif // PM_LESC_ENABLED
+}
+
+
+ret_code_t sm_link_secure(uint16_t conn_handle, bool force_repairing)
+{
+    ret_code_t ret;
+
+    NRF_PM_DEBUG_CHECK(m_module_initialized);
+
+    ret = link_secure(conn_handle, false, force_repairing, false);
+    return ret;
+}
+#endif // NRF_MODULE_ENABLED(PEER_MANAGER)

components/ble/peer_manager/security_manager.h

@@ -0,0 +1,205 @@
+/**
+ * Copyright (c) 2015 - 2021, 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.
+ *
+ */
+#ifndef SECURITY_MANAGER_H__
+#define SECURITY_MANAGER_H__
+
+#include <stdint.h>
+#include "sdk_errors.h"
+#include "ble.h"
+#include "ble_gap.h"
+#include "peer_manager_types.h"
+#include "security_dispatcher.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * @cond NO_DOXYGEN
+ * @defgroup security_manager Security Manager
+ * @ingroup peer_manager
+ * @{
+ * @brief An internal module of @ref peer_manager. A module for streamlining pairing, bonding, and
+ *        encryption, including flash storage of shared data.
+ */
+
+
+/**@brief Function for initializing the Security Manager module.
+ *
+ * @retval NRF_SUCCESS        If initialization was successful.
+ * @retval NRF_ERROR_INTERNAL If an unexpected error occurred.
+ */
+ret_code_t sm_init(void);
+
+
+/**@brief Function for dispatching SoftDevice events to the Security Manager module.
+ *
+ * @param[in]  ble_evt    The SoftDevice event.
+ */
+void sm_ble_evt_handler(ble_evt_t const * ble_evt);
+
+
+/**@brief Function for providing pairing and bonding parameters to use for pairing procedures.
+ *
+ * @details Until this is called, all bonding procedures initiated by the peer will be rejected.
+ *          This function can be called multiple times, even with NULL p_sec_params, in which case
+ *          it will go back to rejecting all procedures.
+ *
+ * @param[in]  p_sec_params  The security parameters to use for this link. Can be NULL to reject
+ *                           all pairing procedures.
+ *
+ * @retval NRF_SUCCESS                    Success.
+ * @retval NRF_ERROR_INVALID_PARAM        Invalid combination of parameters.
+ */
+ret_code_t sm_sec_params_set(ble_gap_sec_params_t * p_sec_params);
+
+
+/**@brief Function for providing security configuration for a link.
+ *
+ * @details This function is optional, and must be called in reply to a @ref
+ *          PM_EVT_CONN_SEC_CONFIG_REQ event, before the Peer Manager event handler returns. If it
+ *          is not called in time, a default configuration is used. See @ref pm_conn_sec_config_t
+ *          for the value of the default.
+ *
+ * @param[in]  conn_handle        The connection to set the configuration for.
+ * @param[in]  p_conn_sec_config  The configuration.
+ */
+void sm_conn_sec_config_reply(uint16_t conn_handle, pm_conn_sec_config_t * p_conn_sec_config);
+
+
+/**@brief Function for providing security parameters for a link.
+ *
+ * @details This function is optional, and must be called in reply to a @ref
+ *          PM_EVT_CONN_SEC_PARAMS_REQ event, before the Security Manager event handler returns. If
+ *          it is not called in time, the parameters given in @ref sm_sec_params_set are used. See
+ *          @ref pm_conn_sec_config_t for the value of the default.
+ *
+ * @param[in]  conn_handle   The connection to set the parameters for.
+ * @param[in]  p_sec_params  The parameters. If NULL, the security procedure is rejected.
+ * @param[in]  p_context     The context found in the request event that this function replies to.
+ *
+ * @retval NRF_SUCCESS              Successful reply.
+ * @retval NRF_ERROR_NULL           p_context was null.
+ * @retval NRF_ERROR_INVALID_PARAM  Value of p_sec_params was invalid.
+ * @retval NRF_ERROR_INVALID_STATE  This module is not initialized.
+ */
+ret_code_t sm_sec_params_reply(uint16_t               conn_handle,
+                               ble_gap_sec_params_t * p_sec_params,
+                               void           const * p_context);
+
+
+/**@brief Experimental function for specifying the public key to use for LESC operations.
+ *
+ * @details This function can be called multiple times. The specified public key will be used for
+ *          all subsequent LESC (LE Secure Connections) operations until the next time this function
+ *          is called.
+ *
+ * @note The key must continue to reside in application memory as it is not copied by Peer Manager.
+ *
+ * @param[in]  p_public_key  The public key to use for all subsequent LESC operations.
+ *
+ * @retval NRF_SUCCESS                    Pairing initiated successfully.
+ * @retval NRF_ERROR_INVALID_STATE        This module is not initialized.
+ */
+ret_code_t sm_lesc_public_key_set(ble_gap_lesc_p256_pk_t * p_public_key);
+
+
+/**@brief Function for getting the security status of a connection.
+ *
+ * @param[in]  conn_handle        Connection handle of the link as provided by the SoftDevice.
+ * @param[out] p_conn_sec_status  Security status of the link.
+ *
+ * @retval NRF_SUCCESS                    If pairing was initiated successfully.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  If the connection handle is invalid.
+ * @retval NRF_ERROR_NULL                 If @p p_conn_sec_status was NULL.
+ */
+ret_code_t sm_conn_sec_status_get(uint16_t conn_handle, pm_conn_sec_status_t * p_conn_sec_status);
+
+
+/**@brief Function for comparing the security status of a connection against a baseline.
+ *
+ * @param[in]  conn_handle       Connection handle of the link as provided by the SoftDevice.
+ * @param[out] p_sec_status_req  Target baseline security status to compare against.
+ *
+ * @retval true   If the security status of the connection matches or exceeds the baseline on all
+ *                points.
+ * @retval false  If the security status of the connection does not fulfill the baseline, or could
+ *                not be retrieved.
+ */
+bool sm_sec_is_sufficient(uint16_t conn_handle, pm_conn_sec_status_t * p_sec_status_req);
+
+
+/**@brief Function for initiating security on the link, with the specified parameters.
+ *
+ * @note If the connection is a peripheral connection, this will send a security request to the
+ *       master, but the master is not obligated to initiate pairing or encryption in response.
+ * @note If the connection is a central connection and a key is available, the parameters will be
+ *       used to determine whether to re-pair or to encrypt using the existing key. If no key is
+ *       available, pairing will be started.
+ *
+ * @param[in]  conn_handle      Handle of the connection to initiate pairing on.
+ * @param[in]  force_repairing  Whether to force a pairing procedure to happen regardless of whether
+ *                              an encryption key already exists. This argument is only relevant for
+ *                              the central role. Recommended value: false
+ *
+ * @retval NRF_SUCCESS                    Success.
+ * @retval NRF_ERROR_TIMEOUT              There has been an SMP timeout, so no more SMP operations
+ *                                        can be performed on this link.
+ * @retval BLE_ERROR_INVALID_CONN_HANDLE  Invalid connection handle.
+ * @retval NRF_ERROR_INVALID_DATA         Peer is bonded, but no LTK was found, and repairing was
+ *                                        not requested.
+ * @retval NRF_ERROR_NOT_FOUND            Security parameters have not been set.
+ * @retval NRF_ERROR_INVALID_STATE        A security procedure is already in progress on the link,
+ *                                        or the link is disconnecting.
+ * @retval NRF_ERROR_INTERNAL             An unexpected error occurred.
+ */
+ret_code_t sm_link_secure(uint16_t conn_handle, bool force_repairing);
+
+/** @}
+ * @endcond
+ */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* SECURITY_MANAGER_H__ */

components/boards/boards.c

@@ -0,0 +1,228 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#include "boards.h"
+#if defined(BOARDS_WITH_USB_DFU_TRIGGER) && defined(BOARD_PCA10059)
+#include "nrf_dfu_trigger_usb.h"
+#endif
+#include <stdint.h>
+#include <stdbool.h>
+
+#if LEDS_NUMBER > 0
+static const uint8_t m_board_led_list[LEDS_NUMBER] = LEDS_LIST;
+#endif
+
+#if BUTTONS_NUMBER > 0
+static const uint8_t m_board_btn_list[BUTTONS_NUMBER] = BUTTONS_LIST;
+#endif
+
+#if LEDS_NUMBER > 0
+bool bsp_board_led_state_get(uint32_t led_idx)
+{
+    ASSERT(led_idx < LEDS_NUMBER);
+    bool pin_set = nrf_gpio_pin_out_read(m_board_led_list[led_idx]) ? true : false;
+    return (pin_set == (LEDS_ACTIVE_STATE ? true : false));
+}
+
+void bsp_board_led_on(uint32_t led_idx)
+{
+        ASSERT(led_idx < LEDS_NUMBER);
+        nrf_gpio_pin_write(m_board_led_list[led_idx], LEDS_ACTIVE_STATE ? 1 : 0);
+}
+
+void bsp_board_led_off(uint32_t led_idx)
+{
+    ASSERT(led_idx < LEDS_NUMBER);
+    nrf_gpio_pin_write(m_board_led_list[led_idx], LEDS_ACTIVE_STATE ? 0 : 1);
+}
+
+void bsp_board_leds_off(void)
+{
+    uint32_t i;
+    for (i = 0; i < LEDS_NUMBER; ++i)
+    {
+        bsp_board_led_off(i);
+    }
+}
+
+void bsp_board_leds_on(void)
+{
+    uint32_t i;
+    for (i = 0; i < LEDS_NUMBER; ++i)
+    {
+        bsp_board_led_on(i);
+    }
+}
+
+void bsp_board_led_invert(uint32_t led_idx)
+{
+    ASSERT(led_idx < LEDS_NUMBER);
+    nrf_gpio_pin_toggle(m_board_led_list[led_idx]);
+}
+
+#if defined(BOARD_PCA10059)
+/**
+ * Function for configuring UICR_REGOUT0 register
+ * to set GPIO output voltage to 3.0V.
+ */
+static void gpio_output_voltage_setup(void)
+{
+    // Configure UICR_REGOUT0 register only if it is set to default value.
+    if ((NRF_UICR->REGOUT0 & UICR_REGOUT0_VOUT_Msk) ==
+        (UICR_REGOUT0_VOUT_DEFAULT << UICR_REGOUT0_VOUT_Pos))
+    {
+        NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Wen;
+        while (NRF_NVMC->READY == NVMC_READY_READY_Busy){}
+
+        NRF_UICR->REGOUT0 = (NRF_UICR->REGOUT0 & ~((uint32_t)UICR_REGOUT0_VOUT_Msk)) |
+                            (UICR_REGOUT0_VOUT_3V0 << UICR_REGOUT0_VOUT_Pos);
+
+        NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren;
+        while (NRF_NVMC->READY == NVMC_READY_READY_Busy){}
+
+        // System reset is needed to update UICR registers.
+        NVIC_SystemReset();
+    }
+}
+#endif
+
+static void bsp_board_leds_init(void)
+{
+    #if defined(BOARD_PCA10059)
+    // If nRF52 USB Dongle is powered from USB (high voltage mode),
+    // GPIO output voltage is set to 1.8 V by default, which is not
+    // enough to turn on green and blue LEDs. Therefore, GPIO voltage
+    // needs to be increased to 3.0 V by configuring the UICR register.
+    if (NRF_POWER->MAINREGSTATUS &
+       (POWER_MAINREGSTATUS_MAINREGSTATUS_High << POWER_MAINREGSTATUS_MAINREGSTATUS_Pos))
+    {
+        gpio_output_voltage_setup();
+    }
+    #endif
+
+    uint32_t i;
+    for (i = 0; i < LEDS_NUMBER; ++i)
+    {
+        nrf_gpio_cfg_output(m_board_led_list[i]);
+    }
+    bsp_board_leds_off();
+}
+
+uint32_t bsp_board_led_idx_to_pin(uint32_t led_idx)
+{
+    ASSERT(led_idx < LEDS_NUMBER);
+    return m_board_led_list[led_idx];
+}
+
+uint32_t bsp_board_pin_to_led_idx(uint32_t pin_number)
+{
+    uint32_t ret = 0xFFFFFFFF;
+    uint32_t i;
+    for (i = 0; i < LEDS_NUMBER; ++i)
+    {
+        if (m_board_led_list[i] == pin_number)
+        {
+            ret = i;
+            break;
+        }
+    }
+    return ret;
+}
+#endif //LEDS_NUMBER > 0
+
+#if BUTTONS_NUMBER > 0
+bool bsp_board_button_state_get(uint32_t button_idx)
+{
+    ASSERT(button_idx < BUTTONS_NUMBER);
+    bool pin_set = nrf_gpio_pin_read(m_board_btn_list[button_idx]) ? true : false;
+    return (pin_set == (BUTTONS_ACTIVE_STATE ? true : false));
+}
+
+static void bsp_board_buttons_init(void)
+{
+    uint32_t i;
+    for (i = 0; i < BUTTONS_NUMBER; ++i)
+    {
+        nrf_gpio_cfg_input(m_board_btn_list[i], BUTTON_PULL);
+    }
+}
+
+uint32_t bsp_board_pin_to_button_idx(uint32_t pin_number)
+{
+    uint32_t i;
+    uint32_t ret = 0xFFFFFFFF;
+    for (i = 0; i < BUTTONS_NUMBER; ++i)
+    {
+        if (m_board_btn_list[i] == pin_number)
+        {
+            ret = i;
+            break;
+        }
+    }
+    return ret;
+}
+
+uint32_t bsp_board_button_idx_to_pin(uint32_t button_idx)
+{
+    ASSERT(button_idx < BUTTONS_NUMBER);
+    return m_board_btn_list[button_idx];
+}
+#endif //BUTTONS_NUMBER > 0
+
+
+void bsp_board_init(uint32_t init_flags)
+{
+    #if defined(BOARDS_WITH_USB_DFU_TRIGGER) && defined(BOARD_PCA10059)
+    (void) nrf_dfu_trigger_usb_init();
+    #endif
+
+    #if LEDS_NUMBER > 0
+    if (init_flags & BSP_INIT_LEDS)
+    {
+        bsp_board_leds_init();
+    }
+    #endif //LEDS_NUMBER > 0
+
+    #if BUTTONS_NUMBER > 0
+    if (init_flags & BSP_INIT_BUTTONS)
+    {
+        bsp_board_buttons_init();
+    }
+    #endif //BUTTONS_NUMBER > 0
+}

components/boards/boards.h

@@ -0,0 +1,368 @@
+/**
+ * Copyright (c) 2014 - 2021, 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.
+ *
+ */
+#ifndef BOARDS_H
+#define BOARDS_H
+
+#include "nrf_gpio.h"
+#include "nordic_common.h"
+
+#if defined(BOARD_NRF6310)
+  #include "nrf6310.h"
+#elif defined(BOARD_PCA10000)
+  #include "pca10000.h"
+#elif defined(BOARD_PCA10001)
+  #include "pca10001.h"
+#elif defined(BOARD_PCA10002)
+  #include "pca10000.h"
+#elif defined(BOARD_PCA10003)
+  #include "pca10003.h"
+#elif defined(BOARD_PCA20006)
+  #include "pca20006.h"
+#elif defined(BOARD_PCA10028)
+  #include "pca10028.h"
+#elif defined(BOARD_PCA10031)
+  #include "pca10031.h"
+#elif defined(BOARD_PCA10036)
+  #include "pca10036.h"
+#elif defined(BOARD_PCA10040)
+  #include "pca10040.h"
+#elif defined(BOARD_PCA10056)
+  #include "pca10056.h"
+#elif defined(BOARD_PCA10100)
+  #include "pca10100.h"
+#elif defined(BOARD_PCA10112)
+  #include "pca10112.h"  
+#elif defined(BOARD_PCA20020)
+  #include "pca20020.h"
+#elif defined(BOARD_PCA10059)
+  #include "pca10059.h"
+#elif defined(BOARD_WT51822)
+  #include "wt51822.h"
+#elif defined(BOARD_N5DK1)
+  #include "n5_starterkit.h"
+#elif defined (BOARD_D52DK1)
+  #include "d52_starterkit.h"
+#elif defined (BOARD_ARDUINO_PRIMO)
+  #include "arduino_primo.h"
+#elif defined (CUSTOM_BOARD_INC)
+  #include STRINGIFY(CUSTOM_BOARD_INC.h)
+#elif defined(BOARD_CUSTOM)
+  #include "custom_board.h"
+#else
+#error "Board is not defined"
+
+#endif
+
+#if defined (SHIELD_BSP_INC)
+  #include STRINGIFY(SHIELD_BSP_INC.h)
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**@defgroup BSP_BOARD_INIT_FLAGS Board initialization flags.
+ * @{ */
+#define BSP_INIT_NONE    0        /**< No initialization of LEDs or buttons (@ref bsp_board_init).*/
+#define BSP_INIT_LEDS    (1 << 0) /**< Enable LEDs during initialization (@ref bsp_board_init).*/
+#define BSP_INIT_BUTTONS (1 << 1) /**< Enable buttons during initialization (@ref bsp_board_init).*/
+/**@} */
+
+/**
+ * Function for returning the state of an LED.
+ *
+ * @param led_idx LED index (starting from 0), as defined in the board-specific header.
+ *
+ * @return True if the LED is turned on.
+ */
+bool bsp_board_led_state_get(uint32_t led_idx);
+
+/**
+ * Function for turning on an LED.
+ *
+ * @param led_idx LED index (starting from 0), as defined in the board-specific header.
+ */
+void bsp_board_led_on(uint32_t led_idx);
+
+/**
+ * Function for turning off an LED.
+ *
+ * @param led_idx LED index (starting from 0), as defined in the board-specific header.
+ */
+void bsp_board_led_off(uint32_t led_idx);
+
+/**
+ * Function for inverting the state of an LED.
+ *
+ * @param led_idx LED index (starting from 0), as defined in the board-specific header.
+ */
+void bsp_board_led_invert(uint32_t led_idx);
+/**
+ * Function for turning off all LEDs.
+ */
+void bsp_board_leds_off(void);
+
+/**
+ * Function for turning on all LEDs.
+ */
+void bsp_board_leds_on(void);
+
+/**
+ * Function for initializing the BSP handling for the board.
+ *
+ * @note This also initializes the USB DFU trigger library if @ref BOARDS_WITH_USB_DFU_TRIGGER is 1.
+ *
+ * @param[in]  init_flags  Flags specifying what to initialize (LEDs/buttons).
+ *                         See @ref BSP_BOARD_INIT_FLAGS.
+ */
+void bsp_board_init(uint32_t init_flags);
+
+/**
+ * Function for converting pin number to LED index.
+ *
+ * @param pin_number Pin number.
+ *
+ * @return LED index of the given pin or 0xFFFFFFFF if invalid pin provided.
+ */
+uint32_t bsp_board_pin_to_led_idx(uint32_t pin_number);
+
+/**
+ * Function for converting LED index to pin number.
+ *
+ * @param led_idx LED index.
+ *
+ * @return Pin number.
+ */
+uint32_t bsp_board_led_idx_to_pin(uint32_t led_idx);
+
+/**
+ * Function for returning the state of a button.
+ *
+ * @param button_idx Button index (starting from 0), as defined in the board-specific header.
+ *
+ * @return True if the button is pressed.
+ */
+bool bsp_board_button_state_get(uint32_t button_idx);
+
+/**
+ * Function for converting pin number to button index.
+ *
+ * @param pin_number Pin number.
+ *
+ * @return Button index of the given pin or 0xFFFFFFFF if invalid pin provided.
+ */
+uint32_t bsp_board_pin_to_button_idx(uint32_t pin_number);
+
+
+/**
+ * Function for converting button index to pin number.
+ *
+ * @param button_idx Button index.
+ *
+ * @return Pin number.
+ */
+uint32_t bsp_board_button_idx_to_pin(uint32_t button_idx);
+
+#define BSP_BOARD_LED_0 0
+#define BSP_BOARD_LED_1 1
+#define BSP_BOARD_LED_2 2
+#define BSP_BOARD_LED_3 3
+#define BSP_BOARD_LED_4 4
+#define BSP_BOARD_LED_5 5
+#define BSP_BOARD_LED_6 6
+#define BSP_BOARD_LED_7 7
+
+#define PIN_MASK(_pin)  /*lint -save -e504 */                     \
+                        (1u << (uint32_t)((_pin) & (~P0_PIN_NUM))) \
+                        /*lint -restore    */
+
+#define PIN_PORT(_pin) (((_pin) >= P0_PIN_NUM) ? NRF_P1 : NRF_GPIO)
+
+#ifdef BSP_LED_0
+#define BSP_LED_0_MASK PIN_MASK(BSP_LED_0)
+#define BSP_LED_0_PORT PIN_PORT(BSP_LED_0)
+#else
+#define BSP_LED_0_MASK 0
+#define BSP_LED_0_PORT 0
+#endif
+#ifdef BSP_LED_1
+#define BSP_LED_1_MASK PIN_MASK(BSP_LED_1)
+#define BSP_LED_1_PORT PIN_PORT(BSP_LED_1)
+#else
+#define BSP_LED_1_MASK 0
+#define BSP_LED_1_PORT 0
+#endif
+#ifdef BSP_LED_2
+#define BSP_LED_2_MASK PIN_MASK(BSP_LED_2)
+#define BSP_LED_2_PORT PIN_PORT(BSP_LED_2)
+#else
+#define BSP_LED_2_MASK 0
+#define BSP_LED_2_PORT 0
+#endif
+#ifdef BSP_LED_3
+#define BSP_LED_3_MASK PIN_MASK(BSP_LED_3)
+#define BSP_LED_3_PORT PIN_PORT(BSP_LED_3)
+#else
+#define BSP_LED_3_MASK 0
+#define BSP_LED_3_PORT 0
+#endif
+#ifdef BSP_LED_4
+#define BSP_LED_4_MASK PIN_MASK(BSP_LED_4)
+#define BSP_LED_4_PORT PIN_PORT(BSP_LED_4)
+#else
+#define BSP_LED_4_MASK 0
+#define BSP_LED_4_PORT 0
+#endif
+#ifdef BSP_LED_5
+#define BSP_LED_5_MASK PIN_MASK(BSP_LED_5)
+#define BSP_LED_5_PORT PIN_PORT(BSP_LED_5)
+#else
+#define BSP_LED_5_MASK 0
+#define BSP_LED_5_PORT 0
+#endif
+#ifdef BSP_LED_6
+#define BSP_LED_6_MASK PIN_MASK(BSP_LED_6)
+#define BSP_LED_6_PORT PIN_PORT(BSP_LED_6)
+#else
+#define BSP_LED_6_MASK 0
+#define BSP_LED_6_PORT 0
+#endif
+#ifdef BSP_LED_7
+#define BSP_LED_7_MASK PIN_MASK(BSP_LED_7)
+#define BSP_LED_7_PORT PIN_PORT(BSP_LED_7)
+#else
+#define BSP_LED_7_MASK 0
+#define BSP_LED_7_PORT 0
+#endif
+
+
+#define LEDS_MASK      (BSP_LED_0_MASK | BSP_LED_1_MASK | \
+                        BSP_LED_2_MASK | BSP_LED_3_MASK | \
+                        BSP_LED_4_MASK | BSP_LED_5_MASK | \
+                        BSP_LED_6_MASK | BSP_LED_7_MASK)
+
+#define BSP_BOARD_BUTTON_0 0
+#define BSP_BOARD_BUTTON_1 1
+#define BSP_BOARD_BUTTON_2 2
+#define BSP_BOARD_BUTTON_3 3
+#define BSP_BOARD_BUTTON_4 4
+#define BSP_BOARD_BUTTON_5 5
+#define BSP_BOARD_BUTTON_6 6
+#define BSP_BOARD_BUTTON_7 7
+
+
+#ifdef BSP_BUTTON_0
+#define BSP_BUTTON_0_MASK (1<<BSP_BUTTON_0)
+#else
+#define BSP_BUTTON_0_MASK 0
+#endif
+#ifdef BSP_BUTTON_1
+#define BSP_BUTTON_1_MASK (1<<BSP_BUTTON_1)
+#else
+#define BSP_BUTTON_1_MASK 0
+#endif
+#ifdef BSP_BUTTON_2
+#define BSP_BUTTON_2_MASK (1<<BSP_BUTTON_2)
+#else
+#define BSP_BUTTON_2_MASK 0
+#endif
+#ifdef BSP_BUTTON_3
+#define BSP_BUTTON_3_MASK (1<<BSP_BUTTON_3)
+#else
+#define BSP_BUTTON_3_MASK 0
+#endif
+#ifdef BSP_BUTTON_4
+#define BSP_BUTTON_4_MASK (1<<BSP_BUTTON_4)
+#else
+#define BSP_BUTTON_4_MASK 0
+#endif
+#ifdef BSP_BUTTON_5
+#define BSP_BUTTON_5_MASK (1<<BSP_BUTTON_5)
+#else
+#define BSP_BUTTON_5_MASK 0
+#endif
+#ifdef BSP_BUTTON_6
+#define BSP_BUTTON_6_MASK (1<<BSP_BUTTON_6)
+#else
+#define BSP_BUTTON_6_MASK 0
+#endif
+#ifdef BSP_BUTTON_7
+#define BSP_BUTTON_7_MASK (1<<BSP_BUTTON_7)
+#else
+#define BSP_BUTTON_7_MASK 0
+#endif
+
+#define BUTTONS_MASK   (BSP_BUTTON_0_MASK | BSP_BUTTON_1_MASK | \
+                        BSP_BUTTON_2_MASK | BSP_BUTTON_3_MASK | \
+                        BSP_BUTTON_4_MASK | BSP_BUTTON_5_MASK | \
+                        BSP_BUTTON_6_MASK | BSP_BUTTON_7_MASK)
+
+
+/* This macro is supporting only P0 and should not be used if LEDs are on other ports. */
+#define LEDS_OFF(leds_mask) do {  ASSERT(sizeof(leds_mask) == 4);                     \
+                        NRF_GPIO->OUTSET = (leds_mask) & (LEDS_MASK & LEDS_INV_MASK); \
+                        NRF_GPIO->OUTCLR = (leds_mask) & (LEDS_MASK & ~LEDS_INV_MASK); } while (0)
+
+/* This macro is supporting only P0 and should not be used if LEDs are on other ports. */
+#define LEDS_ON(leds_mask) do {  ASSERT(sizeof(leds_mask) == 4);                     \
+                       NRF_GPIO->OUTCLR = (leds_mask) & (LEDS_MASK & LEDS_INV_MASK); \
+                       NRF_GPIO->OUTSET = (leds_mask) & (LEDS_MASK & ~LEDS_INV_MASK); } while (0)
+
+/* This macro is supporting only P0 and should not be used if LEDs are on other ports. */
+#define LED_IS_ON(leds_mask) ((leds_mask) & (NRF_GPIO->OUT ^ LEDS_INV_MASK) )
+
+/* This macro is supporting only P0 and should not be used if LEDs are on other ports. */
+#define LEDS_INVERT(leds_mask) do { uint32_t gpio_state = NRF_GPIO->OUT;      \
+                              ASSERT(sizeof(leds_mask) == 4);                 \
+                              NRF_GPIO->OUTSET = ((leds_mask) & ~gpio_state); \
+                              NRF_GPIO->OUTCLR = ((leds_mask) & gpio_state); } while (0)
+
+/* This macro is supporting only P0 and should not be used if LEDs are on other ports. */
+#define LEDS_CONFIGURE(leds_mask) do { uint32_t pin;                  \
+                                  ASSERT(sizeof(leds_mask) == 4);     \
+                                  for (pin = 0; pin < 32; pin++)      \
+                                      if ( (leds_mask) & (1 << pin) ) \
+                                          nrf_gpio_cfg_output(pin); } while (0)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

components/boards/pca10056.h

@@ -0,0 +1,151 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#ifndef PCA10056_H
+#define PCA10056_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "nrf_gpio.h"
+
+// LEDs definitions for PCA10056
+#define LEDS_NUMBER    1
+
+#define LED_GR          NRF_GPIO_PIN_MAP(0,12)	//Green LED for Advertise
+#define LED_YL_TEST     NRF_GPIO_PIN_MAP(0,29)	// Yello LED for Function 260311
+
+#define LEDS_ACTIVE_STATE 0
+#define LEDS_LIST { LED_GR }
+#define LEDS_INV_MASK  LEDS_MASK
+#define BSP_LED_0      LED_GR
+
+#define BUTTONS_NUMBER 3
+
+#define BUTTON_1       19   //24 change  for eeprom 2025.03.24						//Dummy Pin
+#define BUTTON_2       23						//Bonds Erase & Reset  2025.05.06  18-->19 use rest pin as trigger signal
+#define BUTTON_3       NRF_GPIO_PIN_MAP(1,8)	//Power Button
+#define BUTTON_PULL    NRF_GPIO_PIN_PULLUP
+
+#define BUTTONS_ACTIVE_STATE 0
+
+#define BUTTONS_LIST { BUTTON_1, BUTTON_2, BUTTON_3 }
+
+#define BSP_BUTTON_0   BUTTON_1
+#define BSP_BUTTON_1   BUTTON_2
+#define BSP_BUTTON_2   BUTTON_3
+
+#define RX_PIN_NUMBER  7
+#define TX_PIN_NUMBER  6
+#define CTS_PIN_NUMBER 21   //24 change  for eeprom 2025.03.24
+#define RTS_PIN_NUMBER 27
+#define HWFC           true
+
+#define BSP_QSPI_SCK_PIN   19
+#define BSP_QSPI_CSN_PIN   17
+#define BSP_QSPI_IO0_PIN   20
+#define BSP_QSPI_IO1_PIN   21
+#define BSP_QSPI_IO2_PIN   22
+#define BSP_QSPI_IO3_PIN   23
+
+
+// serialization APPLICATION board - temp. setup for running serialized MEMU tests
+#define SER_APP_RX_PIN              NRF_GPIO_PIN_MAP(1,13)    // UART RX pin number.
+#define SER_APP_TX_PIN              NRF_GPIO_PIN_MAP(1,14)    // UART TX pin number.
+#define SER_APP_CTS_PIN             NRF_GPIO_PIN_MAP(0,2)     // UART Clear To Send pin number.
+#define SER_APP_RTS_PIN             NRF_GPIO_PIN_MAP(1,15)    // UART Request To Send pin number.
+
+#define SER_APP_SPIM0_SCK_PIN       NRF_GPIO_PIN_MAP(0,27)     // SPI clock GPIO pin number.
+#define SER_APP_SPIM0_MOSI_PIN      NRF_GPIO_PIN_MAP(0,2)      // SPI Master Out Slave In GPIO pin number
+#define SER_APP_SPIM0_MISO_PIN      NRF_GPIO_PIN_MAP(0,26)     // SPI Master In Slave Out GPIO pin number
+#define SER_APP_SPIM0_SS_PIN        NRF_GPIO_PIN_MAP(1,13)     // SPI Slave Select GPIO pin number
+#define SER_APP_SPIM0_RDY_PIN       NRF_GPIO_PIN_MAP(1,15)     // SPI READY GPIO pin number
+#define SER_APP_SPIM0_REQ_PIN       NRF_GPIO_PIN_MAP(1,14)     // SPI REQUEST GPIO pin number
+
+// serialization CONNECTIVITY board
+#define SER_CON_RX_PIN              NRF_GPIO_PIN_MAP(1,14)    // UART RX pin number.
+#define SER_CON_TX_PIN              NRF_GPIO_PIN_MAP(1,13)    // UART TX pin number.
+#define SER_CON_CTS_PIN             NRF_GPIO_PIN_MAP(1,15)    // UART Clear To Send pin number. Not used if HWFC is set to false.
+#define SER_CON_RTS_PIN             NRF_GPIO_PIN_MAP(0,2)     // UART Request To Send pin number. Not used if HWFC is set to false.
+
+
+#define SER_CON_SPIS_SCK_PIN        NRF_GPIO_PIN_MAP(0,27)    // SPI SCK signal.
+#define SER_CON_SPIS_MOSI_PIN       NRF_GPIO_PIN_MAP(0,2)     // SPI MOSI signal.
+#define SER_CON_SPIS_MISO_PIN       NRF_GPIO_PIN_MAP(0,26)    // SPI MISO signal.
+#define SER_CON_SPIS_CSN_PIN        NRF_GPIO_PIN_MAP(1,13)    // SPI CSN signal.
+#define SER_CON_SPIS_RDY_PIN        NRF_GPIO_PIN_MAP(1,15)    // SPI READY GPIO pin number.
+#define SER_CON_SPIS_REQ_PIN        NRF_GPIO_PIN_MAP(1,14)    // SPI REQUEST GPIO pin number.
+
+#define SER_CONN_CHIP_RESET_PIN     NRF_GPIO_PIN_MAP(1,1)    // Pin used to reset connectivity chip
+
+// Arduino board mappings
+#define ARDUINO_SCL_PIN             27    // SCL signal pin
+#define ARDUINO_SDA_PIN             26    // SDA signal pin
+#define ARDUINO_AREF_PIN            2     // Aref pin
+
+#define ARDUINO_13_PIN              NRF_GPIO_PIN_MAP(1, 15)  // Digital pin 13
+#define ARDUINO_12_PIN              NRF_GPIO_PIN_MAP(1, 14)  // Digital pin 12
+#define ARDUINO_11_PIN              NRF_GPIO_PIN_MAP(1, 13)  // Digital pin 11
+#define ARDUINO_10_PIN              NRF_GPIO_PIN_MAP(1, 12)  // Digital pin 10
+#define ARDUINO_9_PIN               NRF_GPIO_PIN_MAP(1, 11)  // Digital pin 9
+#define ARDUINO_8_PIN               NRF_GPIO_PIN_MAP(1, 10)  // Digital pin 8
+
+#define ARDUINO_7_PIN               NRF_GPIO_PIN_MAP(1, 8) // Digital pin 7
+#define ARDUINO_6_PIN               NRF_GPIO_PIN_MAP(1, 7) // Digital pin 6
+#define ARDUINO_5_PIN               NRF_GPIO_PIN_MAP(1, 6) // Digital pin 5
+#define ARDUINO_4_PIN               NRF_GPIO_PIN_MAP(1, 5) // Digital pin 4
+#define ARDUINO_3_PIN               NRF_GPIO_PIN_MAP(1, 4) // Digital pin 3
+#define ARDUINO_2_PIN               NRF_GPIO_PIN_MAP(1, 3) // Digital pin 2
+#define ARDUINO_1_PIN               NRF_GPIO_PIN_MAP(1, 2) // Digital pin 1
+#define ARDUINO_0_PIN               NRF_GPIO_PIN_MAP(1, 1) // Digital pin 0
+
+#define ARDUINO_A0_PIN              3     // Analog channel 0
+#define ARDUINO_A1_PIN              4     // Analog channel 1
+#define ARDUINO_A2_PIN              28    // Analog channel 2
+#define ARDUINO_A3_PIN              29    // Analog channel 3
+#define ARDUINO_A4_PIN              30    // Analog channel 4
+#define ARDUINO_A5_PIN              31    // Analog channel 5
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // PCA10056_H

components/libraries/atomic/nrf_atomic.c

@@ -0,0 +1,449 @@
+/**
+ * Copyright (c) 2018 - 2021, 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.
+ *
+ */
+#include "nrf_atomic.h"
+
+#ifndef NRF_ATOMIC_USE_BUILD_IN
+#if (defined(__GNUC__) && defined(WIN32))
+    #define NRF_ATOMIC_USE_BUILD_IN 1
+#else
+    #define NRF_ATOMIC_USE_BUILD_IN 0
+#endif
+#endif // NRF_ATOMIC_USE_BUILD_IN
+
+#if ((__CORTEX_M >= 0x03U) || (__CORTEX_SC >= 300U))
+#define STREX_LDREX_PRESENT
+#else
+#include "app_util_platform.h"
+#endif
+
+
+#if (NRF_ATOMIC_USE_BUILD_IN == 0) && defined(STREX_LDREX_PRESENT)
+#include "nrf_atomic_internal.h"
+#endif
+
+uint32_t nrf_atomic_u32_fetch_store(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_exchange_n(p_data, value, __ATOMIC_SEQ_CST);
+
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+    NRF_ATOMIC_OP(mov, old_val, new_val, p_data, value);
+
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data = value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_store(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    __atomic_store_n(p_data, value, __ATOMIC_SEQ_CST);
+    return value;
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(mov, old_val, new_val, p_data, value);
+
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data = value;
+    CRITICAL_REGION_EXIT();
+    return value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_fetch_or(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_fetch_or(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(orr, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data |= value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_or(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_or_fetch(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(orr, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data |= value;
+    uint32_t new_value = *p_data;
+    CRITICAL_REGION_EXIT();
+    return new_value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_fetch_and(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_fetch_and(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(and, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data &= value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_and(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_and_fetch(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(and, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data &= value;
+    uint32_t new_value = *p_data;
+    CRITICAL_REGION_EXIT();
+    return new_value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_fetch_xor(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_fetch_xor(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(eor, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data ^= value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_xor(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_xor_fetch(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(eor, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data ^= value;
+    uint32_t new_value = *p_data;
+    CRITICAL_REGION_EXIT();
+    return new_value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_fetch_add(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_fetch_add(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(add, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data += value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_add(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_add_fetch(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(add, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data += value;
+    uint32_t new_value = *p_data;
+    CRITICAL_REGION_EXIT();
+    return new_value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_fetch_sub(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_fetch_sub(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(sub, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data -= value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_sub(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_sub_fetch(p_data, value, __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(sub, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data -= value;
+    uint32_t new_value = *p_data;
+    CRITICAL_REGION_EXIT();
+    return new_value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+bool nrf_atomic_u32_cmp_exch(nrf_atomic_u32_t * p_data,
+                                           uint32_t *         p_expected,
+                                           uint32_t           desired)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    return __atomic_compare_exchange(p_data,
+                                     p_expected,
+                                     &desired,
+                                     1,
+                                     __ATOMIC_SEQ_CST,
+                                     __ATOMIC_SEQ_CST);
+#elif defined(STREX_LDREX_PRESENT)
+    return nrf_atomic_internal_cmp_exch(p_data, p_expected, desired);
+#else
+    bool ret;
+    CRITICAL_REGION_ENTER();
+    if (*p_data == *p_expected)
+    {
+        *p_data = desired;
+        ret = true;
+    }
+    else
+    {
+        *p_expected = *p_data;
+        ret = false;
+    }
+    CRITICAL_REGION_EXIT();
+    return ret;
+#endif
+}
+
+uint32_t nrf_atomic_u32_fetch_sub_hs(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    uint32_t expected = *p_data;
+    uint32_t new_val;
+    bool     success;
+
+    do
+    {
+        if (expected >= value)
+        {
+            new_val = expected - value;
+        }
+        else
+        {
+            new_val = expected;
+        }
+        success = __atomic_compare_exchange(p_data,
+                                            &expected,
+                                            &new_val,
+                                            1,
+                                            __ATOMIC_SEQ_CST,
+                                            __ATOMIC_SEQ_CST);
+    } while(!success);
+    return expected;
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(sub_hs, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return old_val;
+#else
+    CRITICAL_REGION_ENTER();
+    uint32_t old_val = *p_data;
+    *p_data -= value;
+    CRITICAL_REGION_EXIT();
+    return old_val;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_u32_sub_hs(nrf_atomic_u32_t * p_data, uint32_t value)
+{
+#if NRF_ATOMIC_USE_BUILD_IN
+    uint32_t expected = *p_data;
+    uint32_t new_val;
+    bool     success;
+
+    do
+    {
+        if (expected >= value)
+        {
+            new_val = expected - value;
+        }
+        else
+        {
+            new_val = expected;
+        }
+        success = __atomic_compare_exchange(p_data,
+                                            &expected,
+                                            &new_val,
+                                            1,
+                                            __ATOMIC_SEQ_CST,
+                                            __ATOMIC_SEQ_CST);
+    } while(!success);
+    return new_val;
+#elif defined(STREX_LDREX_PRESENT)
+    uint32_t old_val;
+    uint32_t new_val;
+
+    NRF_ATOMIC_OP(sub_hs, old_val, new_val, p_data, value);
+    UNUSED_PARAMETER(old_val);
+    UNUSED_PARAMETER(new_val);
+    return new_val;
+#else
+    CRITICAL_REGION_ENTER();
+    *p_data -= value;
+    uint32_t new_value = *p_data;
+    CRITICAL_REGION_EXIT();
+    return new_value;
+#endif //NRF_ATOMIC_USE_BUILD_IN
+}
+
+uint32_t nrf_atomic_flag_set_fetch(nrf_atomic_flag_t * p_data)
+{
+    return nrf_atomic_u32_fetch_or(p_data, 1);
+}
+
+uint32_t nrf_atomic_flag_set(nrf_atomic_flag_t * p_data)
+{
+    return nrf_atomic_u32_or(p_data, 1);
+}
+
+uint32_t nrf_atomic_flag_clear_fetch(nrf_atomic_flag_t * p_data)
+{
+    return nrf_atomic_u32_fetch_and(p_data, 0);
+}
+
+uint32_t nrf_atomic_flag_clear(nrf_atomic_flag_t * p_data)
+{
+    return nrf_atomic_u32_and(p_data, 0);
+}
+

components/libraries/atomic/nrf_atomic.h

@@ -0,0 +1,274 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_atomic Atomic operations API
+ * @ingroup app_common
+ * @{
+ *
+ * @brief @tagAPI52 This module implements C11 stdatomic.h simplified API.
+          At this point only Cortex-M3/M4 cores are supported (LDREX/STREX instructions).
+ *        Atomic types are limited to @ref nrf_atomic_u32_t and @ref nrf_atomic_flag_t.
+ */
+
+#ifndef NRF_ATOMIC_H__
+#define NRF_ATOMIC_H__
+
+#include "sdk_common.h"
+
+/**
+ * @brief Atomic 32 bit unsigned type
+ * */
+typedef volatile uint32_t nrf_atomic_u32_t;
+
+/**
+ * @brief Atomic 1 bit flag type (technically 32 bit)
+ * */
+typedef volatile uint32_t nrf_atomic_flag_t;
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Stores value to an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value to store
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_store(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Stores value to an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value to store
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_store(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Logical OR operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand OR operation
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_or(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Logical OR operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand OR operation
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_or(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Logical AND operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand AND operation
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_and(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Logical AND operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand AND operation
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_and(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Logical XOR operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand XOR operation
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_xor(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Logical XOR operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand XOR operation
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_xor(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Arithmetic ADD operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand ADD operation
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_add(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Arithmetic ADD operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand ADD operation
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_add(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Arithmetic SUB operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand SUB operation
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_sub(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Arithmetic SUB operation on an atomic object
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand SUB operation
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_sub(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief If value at pointer is equal to expected value, changes value at pointer to desired
+ *
+ * Atomically compares the value pointed to by p_data with the value pointed to by p_expected,
+ * and if those are equal, replaces the former with desired. Otherwise, loads the actual value
+ * pointed to by p_data into *p_expected.
+ *
+ * @param p_data     Atomic memory pointer to test and modify.
+ * @param p_expected Pointer to test value.
+ * @param desired    Value to be stored to atomic memory.
+ *
+ * @retval true  *p_data was equal to *p_expected
+ * @retval false *p_data was not equal to *p_expected
+ */
+bool nrf_atomic_u32_cmp_exch(nrf_atomic_u32_t * p_data,
+                             uint32_t *         p_expected,
+                             uint32_t           desired);
+
+/**
+ * @brief Arithmetic SUB operation on an atomic object performed if object >= value.
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand SUB operation
+ *
+ * @return Old value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_fetch_sub_hs(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**
+ * @brief Arithmetic SUB operation on an atomic object performed if object >= value.
+ *
+ * @param[in] p_data    Atomic memory pointer
+ * @param[in] value     Value of second operand SUB operation
+ *
+ * @return New value stored into atomic object
+ * */
+uint32_t nrf_atomic_u32_sub_hs(nrf_atomic_u32_t * p_data, uint32_t value);
+
+/**************************************************************************************************/
+
+/**
+ * @brief Logic one bit flag set operation on an atomic object
+ *
+ * @param[in] p_data    Atomic flag memory pointer
+ *
+ * @return Old flag value
+ * */
+uint32_t nrf_atomic_flag_set_fetch(nrf_atomic_flag_t * p_data);
+
+/**
+ * @brief Logic one bit flag set operation on an atomic object
+ *
+ * @param[in] p_data    Atomic flag memory pointer
+ *
+ * @return New flag value
+ * */
+uint32_t nrf_atomic_flag_set(nrf_atomic_flag_t * p_data);
+
+/**
+ * @brief Logic one bit flag clear operation on an atomic object
+ *
+ * @param[in] p_data    Atomic flag memory pointer
+ *
+ * @return Old flag value
+ * */
+uint32_t nrf_atomic_flag_clear_fetch(nrf_atomic_flag_t * p_data);
+
+/**
+ * @brief Logic one bit flag clear operation on an atomic object
+ *
+ * @param[in] p_data    Atomic flag memory pointer
+ *
+ * @return New flag value
+ * */
+uint32_t nrf_atomic_flag_clear(nrf_atomic_flag_t * p_data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NRF_ATOMIC_H__ */
+
+/** @} */

components/libraries/atomic/nrf_atomic_internal.h

@@ -0,0 +1,343 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#ifndef NRF_ATOMIC_INTERNAL_H__
+#define NRF_ATOMIC_INTERNAL_H__
+
+#include "sdk_common.h"
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ *
+ * @defgroup nrf_atomic_internal Atomic operations internals
+ * @ingroup nrf_atomic
+ * @{
+ *
+ */
+
+/* Only Cortex M cores > 3 support LDREX/STREX instructions*/
+#if ((__CORTEX_M >= 0x03U) || (__CORTEX_SC >= 300U)) == 0
+#error "Unsupported core version"
+#endif
+
+#if defined ( __CC_ARM )
+static __asm uint32_t nrf_atomic_internal_mov(nrf_atomic_u32_t * p_ptr,
+                                              uint32_t value,
+                                              uint32_t * p_new)
+{
+    /* The base standard provides for passing arguments in core registers (r0-r3) and on the stack.
+     * Registers r4 and r5 have to be saved on stack. Note that only even number of register push are
+     * allowed. This is a requirement of the Procedure Call Standard for the ARM Architecture [AAPCS].
+     * */
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_mov
+    ldrex r0, [r4]
+    mov   r5, r1
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_mov
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+
+static __asm uint32_t nrf_atomic_internal_orr(nrf_atomic_u32_t * p_ptr,
+                                              uint32_t value,
+                                              uint32_t * p_new)
+{
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_orr
+    ldrex r0, [r4]
+    orr   r5, r0, r1
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_orr
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+static __asm uint32_t nrf_atomic_internal_and(nrf_atomic_u32_t * p_ptr,
+                                              uint32_t value,
+                                              uint32_t * p_new)
+{
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_and
+    ldrex r0, [r4]
+    and   r5, r0, r1
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_and
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+static __asm uint32_t nrf_atomic_internal_eor(nrf_atomic_u32_t * p_ptr,
+                                              uint32_t value,
+                                              uint32_t * p_new)
+{
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_eor
+    ldrex r0, [r4]
+    eor   r5, r0, r1
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_eor
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+static __asm uint32_t nrf_atomic_internal_add(nrf_atomic_u32_t * p_ptr,
+                                              uint32_t value,
+                                              uint32_t * p_new)
+{
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_add
+    ldrex r0, [r4]
+    add   r5, r0, r1
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_add
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+static __asm uint32_t nrf_atomic_internal_sub(nrf_atomic_u32_t * p_ptr,
+                                              uint32_t value,
+                                              uint32_t * p_new)
+{
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_sub
+    ldrex r0, [r4]
+    sub   r5, r0, r1
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_sub
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+static __asm bool nrf_atomic_internal_cmp_exch(nrf_atomic_u32_t * p_data,
+                                               uint32_t *         p_expected,
+                                               uint32_t           value)
+{
+#define RET_REG  r0
+#define P_EXPC   r1
+#define VALUE    r2
+#define STR_RES  r3
+#define P_DATA   r4
+#define EXPC_VAL r5
+#define ACT_VAL  r6
+
+    push {r4-r6}
+    mov  P_DATA, r0
+    mov  RET_REG, #0
+
+loop_cmp_exch
+    ldrex   ACT_VAL, [P_DATA]
+    ldr     EXPC_VAL, [P_EXPC]
+    cmp     ACT_VAL, EXPC_VAL
+    ittee   eq
+    strexeq STR_RES, VALUE, [P_DATA]
+    moveq   RET_REG, #1
+    strexne STR_RES, ACT_VAL, [P_DATA]
+    strne   ACT_VAL, [P_EXPC]
+    cmp     STR_RES, #0
+    itt     ne
+    movne   RET_REG, #0
+    bne     loop_cmp_exch
+
+    pop {r4-r6}
+    bx    lr
+
+#undef RET_REG
+#undef P_EXPC
+#undef VALUE
+#undef STR_RES
+#undef P_DATA
+#undef EXPC_VAL
+#undef ACT_VAL
+}
+
+static __asm uint32_t nrf_atomic_internal_sub_hs(nrf_atomic_u32_t * p_ptr,
+                                                 uint32_t value,
+                                                 uint32_t * p_new)
+{
+    push  {r4, r5}
+    mov   r4, r0
+
+loop_sub_ge
+    ldrex r0, [r4]
+    cmp   r0, r1
+    ite   hs
+    subhs r5, r0, r1
+    movlo r5, r0
+    strex r3, r5, [r4]
+    cmp   r3, #0
+    bne   loop_sub_ge
+
+    str   r5, [r2]
+    pop   {r4, r5}
+    bx    lr
+}
+
+
+#define NRF_ATOMIC_OP(asm_op, old_val, new_val, ptr, value)          \
+        old_val = nrf_atomic_internal_##asm_op(ptr, value, &new_val)
+
+#elif defined ( __ICCARM__ ) || defined ( __GNUC__ )
+
+/**
+ * @brief Atomic operation generic macro
+ * @param[in] asm_op operation: mov, orr, and, eor, add, sub
+ * @param[out] old_val atomic object output (uint32_t), value before operation
+ * @param[out] new_val atomic object output (uint32_t), value after operation
+ * @param[in] value atomic operation operand
+ * */
+#define NRF_ATOMIC_OP(asm_op, old_val, new_val, ptr, value)                 \
+{                                                                           \
+    uint32_t str_res;                                                       \
+            __ASM volatile(                                                 \
+    "1:     ldrex   %["#old_val"], [%["#ptr"]]\n"                           \
+    NRF_ATOMIC_OP_##asm_op(new_val, old_val, value)                         \
+    "       strex   %[str_res], %["#new_val"], [%["#ptr"]]\n"               \
+    "       teq     %[str_res], #0\n"                                       \
+    "       bne.n     1b"                                                   \
+            :                                                               \
+        [old_val]"=&r" (old_val),                                           \
+        [new_val]"=&r" (new_val),                                           \
+        [str_res]"=&r" (str_res)                                            \
+            :                                                               \
+        [ptr]"r" (ptr),                                                     \
+        [value]"r" (value)                                                  \
+            : "cc");                                                        \
+    UNUSED_PARAMETER(str_res);                                              \
+}
+
+#define NRF_ATOMIC_OP_mov(new_val, old_val, value) "mov %["#new_val"], %["#value"]\n"
+#define NRF_ATOMIC_OP_orr(new_val, old_val, value) "orr %["#new_val"], %["#old_val"], %["#value"]\n"
+#define NRF_ATOMIC_OP_and(new_val, old_val, value) "and %["#new_val"], %["#old_val"], %["#value"]\n"
+#define NRF_ATOMIC_OP_eor(new_val, old_val, value) "eor %["#new_val"], %["#old_val"], %["#value"]\n"
+#define NRF_ATOMIC_OP_add(new_val, old_val, value) "add %["#new_val"], %["#old_val"], %["#value"]\n"
+#define NRF_ATOMIC_OP_sub(new_val, old_val, value) "sub %["#new_val"], %["#old_val"], %["#value"]\n"
+#define NRF_ATOMIC_OP_sub_hs(new_val, old_val, value)                                              \
+    "cmp %["#old_val"], %["#value"]\n "                                                            \
+    "ite hs\n"                                                                                     \
+    "subhs %["#new_val"], %["#old_val"], %["#value"]\n"                                            \
+    "movlo %["#new_val"], %["#old_val"]\n"
+
+static inline bool nrf_atomic_internal_cmp_exch(nrf_atomic_u32_t * p_data,
+                                                uint32_t *         p_expected,
+                                                uint32_t           value)
+{
+    bool res = false;
+    uint32_t str_res = 0;
+    uint32_t act_val = 0;
+    uint32_t exp_val = 0;
+    UNUSED_VARIABLE(str_res);
+    UNUSED_VARIABLE(act_val);
+    UNUSED_VARIABLE(exp_val);
+    __ASM volatile(
+    "1:     ldrex   %[act_val], [%[ptr]]\n"
+    "       ldr     %[exp_val], [%[expc]]\n"
+    "       cmp     %[act_val], %[exp_val]\n"
+    "       ittee   eq\n"
+    "       strexeq %[str_res], %[value], [%[ptr]]\n"
+    "       moveq   %[res], #1\n"
+    "       strexne %[str_res], %[act_val], [%[ptr]]\n"
+    "       strne   %[act_val], [%[expc]]\n"
+    "       cmp     %[str_res], #0\n"
+    "       itt     ne\n"
+    "       movne   %[res], #0\n"
+    "       bne.n     1b"
+            :
+        [res]     "=&r" (res),
+        [exp_val] "=&r" (exp_val),
+        [act_val]    "=&r" (act_val),
+        [str_res] "=&r" (str_res)
+            :
+        "0" (res),
+        "1" (exp_val),
+        "2" (act_val),
+        [expc]  "r" (p_expected),
+        [ptr]   "r" (p_data),
+        [value] "r" (value)
+            : "cc");
+    return res;
+}
+
+#else
+#error "Unsupported compiler"
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NRF_ATOMIC_INTERNAL_H__ */
+
+/** @} */

components/libraries/atomic/nrf_atomic_sanity_check.h

@@ -0,0 +1,153 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#ifndef NRF_ATOMIC_SANITY_CHECK_H__
+#define NRF_ATOMIC_SANITY_CHECK_H__
+
+#include "nrf_atomic.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Quick sanity check of nrf_atomic API
+ * */
+static inline void nrf_atomic_sanity_check(void)
+{
+#if defined(DEBUG_NRF) || defined(DEBUG_NRF_USER)
+    nrf_atomic_u32_t val;
+    nrf_atomic_u32_t flag;
+
+    /*Fetch version tests*/
+    val = 0;
+    ASSERT(nrf_atomic_u32_store_fetch(&val, 10) == 0);
+    ASSERT(nrf_atomic_u32_store_fetch(&val, 0) == 10);
+
+    val = 0;
+    ASSERT(nrf_atomic_u32_or_fetch(&val, 1 << 16) == 0);
+    ASSERT(nrf_atomic_u32_or_fetch(&val, 1 << 5) == ((1 << 16)));
+    ASSERT(nrf_atomic_u32_or_fetch(&val, 1 << 5) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_or_fetch(&val, 0) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_or_fetch(&val, 0xFFFFFFFF) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_or_fetch(&val, 0xFFFFFFFF) == (0xFFFFFFFF));
+
+    val = 0xFFFFFFFF;
+    ASSERT(nrf_atomic_u32_and_fetch(&val, ~(1 << 16)) == 0xFFFFFFFF);
+    ASSERT(nrf_atomic_u32_and_fetch(&val, ~(1 << 5)) == (0xFFFFFFFF & ~((1 << 16))));
+    ASSERT(nrf_atomic_u32_and_fetch(&val, 0) == (0xFFFFFFFF & ~(((1 << 16) | (1 << 5)))));
+    ASSERT(nrf_atomic_u32_and_fetch(&val, 0xFFFFFFFF) == (0));
+
+    val = 0;
+    ASSERT(nrf_atomic_u32_xor_fetch(&val, (1 << 16)) == 0);
+    ASSERT(nrf_atomic_u32_xor_fetch(&val, (1 << 5)) == ((1 << 16)));
+    ASSERT(nrf_atomic_u32_xor_fetch(&val, 0) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_xor_fetch(&val, (1 << 16) | (1 << 5)) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_xor_fetch(&val, 0) == (0));
+
+    val = 0;
+    ASSERT(nrf_atomic_u32_add_fetch(&val, 100) == 0);
+    ASSERT(nrf_atomic_u32_add_fetch(&val, 100) == 100);
+    ASSERT(nrf_atomic_u32_add_fetch(&val, 1 << 24) == 200);
+    ASSERT(nrf_atomic_u32_add_fetch(&val, 0) == (200 + (1 << 24)));
+    ASSERT(nrf_atomic_u32_add_fetch(&val, 0xFFFFFFFF) == (200 + (1 << 24)));
+    ASSERT(nrf_atomic_u32_add_fetch(&val, 0) == (200 - 1 + (1 << 24)));
+
+    val = 1000;
+    ASSERT(nrf_atomic_u32_sub_fetch(&val, 100) == 1000);
+    ASSERT(nrf_atomic_u32_sub_fetch(&val, 100) == 900);
+    ASSERT(nrf_atomic_u32_sub_fetch(&val, 0) == 800);
+    ASSERT(nrf_atomic_u32_sub_fetch(&val, 0xFFFFFFFF) == 800);
+    ASSERT(nrf_atomic_u32_sub_fetch(&val, 0) == 801);
+
+    flag = 0;
+    ASSERT(nrf_atomic_flag_set_fetch(&flag) == 0);
+    ASSERT(nrf_atomic_flag_set_fetch(&flag) == 1);
+    ASSERT(nrf_atomic_flag_clear_fetch(&flag) == 1);
+    ASSERT(nrf_atomic_flag_clear_fetch(&flag) == 0);
+
+    /*No fetch version tests*/
+    val = 0;
+    ASSERT(nrf_atomic_u32_store(&val, 10) == 10);
+    ASSERT(nrf_atomic_u32_store(&val, 0) == 0);
+
+    val = 0;
+    ASSERT(nrf_atomic_u32_or(&val, 1 << 16) == 1 << 16);
+    ASSERT(nrf_atomic_u32_or(&val, 1 << 5) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_or(&val, 1 << 5) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_or(&val, 0) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_or(&val, 0xFFFFFFFF) == 0xFFFFFFFF);
+
+    val = 0xFFFFFFFF;
+    ASSERT(nrf_atomic_u32_and(&val, ~(1 << 16)) == (0xFFFFFFFF & ~((1 << 16))));
+    ASSERT(nrf_atomic_u32_and(&val, ~(1 << 5)) == (0xFFFFFFFF & ~(((1 << 16) | (1 << 5)))));
+    ASSERT(nrf_atomic_u32_and(&val, 0) == 0);
+
+    val = 0;
+    ASSERT(nrf_atomic_u32_xor(&val, (1 << 16)) == ((1 << 16)));
+    ASSERT(nrf_atomic_u32_xor(&val, (1 << 5)) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_xor(&val, 0) == ((1 << 16) | (1 << 5)));
+    ASSERT(nrf_atomic_u32_xor(&val, (1 << 16) | (1 << 5)) == 0);
+
+    val = 0;
+    ASSERT(nrf_atomic_u32_add(&val, 100) == 100);
+    ASSERT(nrf_atomic_u32_add(&val, 100) == 200);
+    ASSERT(nrf_atomic_u32_add(&val, 1 << 24) == (200 + (1 << 24)));
+    ASSERT(nrf_atomic_u32_add(&val, 0) == (200 + (1 << 24)));
+    ASSERT(nrf_atomic_u32_add(&val, 0xFFFFFFFF) == (200 - 1 + (1 << 24)));
+
+    val = 1000;
+    ASSERT(nrf_atomic_u32_sub(&val, 100) == 900);
+    ASSERT(nrf_atomic_u32_sub(&val, 100) == 800);
+    ASSERT(nrf_atomic_u32_sub(&val, 0) == 800);
+    ASSERT(nrf_atomic_u32_sub(&val, 0xFFFFFFFF) == 801);
+
+    flag = 0;
+    ASSERT(nrf_atomic_flag_set(&flag) == 1);
+    ASSERT(nrf_atomic_flag_set(&flag) == 1);
+    ASSERT(nrf_atomic_flag_clear(&flag) == 0);
+    ASSERT(nrf_atomic_flag_clear(&flag) == 0);
+#endif
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NRF_ATOMIC_SANITY_CHECK_H__ */

components/libraries/atomic_fifo/nrf_atfifo.c

@@ -0,0 +1,189 @@
+/**
+ * Copyright (c) 2011 - 2021, 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.
+ *
+ */
+
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include "app_util.h"
+#include "nrf_atfifo.h"
+#include "nrf_atfifo_internal.h"
+
+#if NRF_ATFIFO_CONFIG_LOG_ENABLED
+    #define NRF_LOG_LEVEL             NRF_ATFIFO_CONFIG_LOG_LEVEL
+    #define NRF_LOG_INIT_FILTER_LEVEL NRF_ATFIFO_CONFIG_LOG_INIT_FILTER_LEVEL
+    #define NRF_LOG_INFO_COLOR        NRF_ATFIFO_CONFIG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR       NRF_ATFIFO_CONFIG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // NRF_ATFIFO_CONFIG_LOG_ENABLED
+#include "nrf_log.h"
+
+/* Unions testing */
+STATIC_ASSERT(sizeof(nrf_atfifo_postag_t) == sizeof(uint32_t));
+
+
+ret_code_t nrf_atfifo_init(nrf_atfifo_t * const p_fifo, void * p_buf, uint16_t buf_size, uint16_t item_size)
+{
+    if (NULL == p_buf)
+    {
+        NRF_LOG_INST_ERROR(p_fifo->p_log, "Initialization failed. p_buf == NULL");
+        return NRF_ERROR_NULL;
+    }
+    if (0 != (buf_size % item_size))
+    {
+        NRF_LOG_INST_ERROR(p_fifo->p_log, "Initialization failed. Buf_size not multiple of item_size");
+        return NRF_ERROR_INVALID_LENGTH;
+    }
+
+    p_fifo->p_buf     = p_buf;
+    p_fifo->tail.tag  = 0;
+    p_fifo->head.tag  = 0;
+    p_fifo->buf_size  = buf_size;
+    p_fifo->item_size = item_size;
+
+    NRF_LOG_INST_INFO(p_fifo->p_log, "Initialized.");
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t nrf_atfifo_clear(nrf_atfifo_t * const p_fifo)
+{
+    bool released = nrf_atfifo_space_clear(p_fifo);
+    NRF_LOG_INST_INFO(p_fifo->p_log, "Cleared result:%s", released ? "success" : "busy");
+    return released ? NRF_SUCCESS : NRF_ERROR_BUSY;
+}
+
+
+ret_code_t nrf_atfifo_alloc_put(nrf_atfifo_t * const p_fifo, void const * p_var, size_t size, bool * const p_visible)
+{
+    nrf_atfifo_item_put_t context;
+    bool visible;
+    void * p_data = nrf_atfifo_item_alloc(p_fifo, &context);
+    if (NULL == p_data)
+    {
+        NRF_LOG_INST_WARNING(p_fifo->p_log, "Copying in element (0x%08X) failed - no space.", p_var);
+        return NRF_ERROR_NO_MEM;
+    }
+
+    memcpy(p_data, p_var, size);
+
+    visible = nrf_atfifo_item_put(p_fifo, &context);
+    if (NULL != p_visible)
+    {
+        *p_visible = visible;
+    }
+    NRF_LOG_INST_DEBUG(p_fifo->p_log, "Element (0x%08X) copied in.", p_var);
+    return NRF_SUCCESS;
+}
+
+
+void * nrf_atfifo_item_alloc(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_put_t * p_context)
+{
+    if (nrf_atfifo_wspace_req(p_fifo, &(p_context->last_tail)))
+    {
+        void * p_item = ((uint8_t*)(p_fifo->p_buf)) + p_context->last_tail.pos.wr;
+        NRF_LOG_INST_DEBUG(p_fifo->p_log, "Allocated  element (0x%08X).", p_item);
+        return p_item;
+    }
+    NRF_LOG_INST_WARNING(p_fifo->p_log, "Allocation failed - no space.");
+    return NULL;
+}
+
+
+bool nrf_atfifo_item_put(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_put_t * p_context)
+{
+    if ((p_context->last_tail.pos.wr) == (p_context->last_tail.pos.rd))
+    {
+        NRF_LOG_INST_DEBUG(p_fifo->p_log, "Put (uninterrupted)");
+        nrf_atfifo_wspace_close(p_fifo);
+        return true;
+    }
+    NRF_LOG_INST_DEBUG(p_fifo->p_log, "Put (interrupted!)");
+    return false;
+}
+
+
+ret_code_t nrf_atfifo_get_free(nrf_atfifo_t * const p_fifo, void * const p_var, size_t size, bool * p_released)
+{
+    nrf_atfifo_item_get_t context;
+    bool released;
+    void const * p_s = nrf_atfifo_item_get(p_fifo, &context);
+    if (NULL == p_s)
+    {
+        NRF_LOG_INST_WARNING(p_fifo->p_log, "Copying out failed - no item in the FIFO.");
+        return NRF_ERROR_NOT_FOUND;
+    }
+
+    memcpy(p_var, p_s, size);
+
+    released = nrf_atfifo_item_free(p_fifo, &context);
+    if (NULL != p_released)
+    {
+        *p_released = released;
+    }
+    NRF_LOG_INST_DEBUG(p_fifo->p_log, "Element (0x%08X) copied out.", p_var);
+    return NRF_SUCCESS;
+}
+
+
+void * nrf_atfifo_item_get(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_get_t * p_context)
+{
+    if (nrf_atfifo_rspace_req(p_fifo, &(p_context->last_head)))
+    {
+        void * p_item = ((uint8_t*)(p_fifo->p_buf)) + p_context->last_head.pos.rd;
+        NRF_LOG_INST_DEBUG(p_fifo->p_log, "Get element: 0x%08X", p_item);
+        return p_item;
+    }
+    NRF_LOG_INST_WARNING(p_fifo->p_log, "Get failed - no item in the FIFO.");
+    return NULL;
+}
+
+
+bool nrf_atfifo_item_free(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_get_t * p_context)
+{
+    if ((p_context->last_head.pos.wr) == (p_context->last_head.pos.rd))
+    {
+        NRF_LOG_INST_DEBUG(p_fifo->p_log, "Free (uninterrupted)");
+        nrf_atfifo_rspace_close(p_fifo);
+        return true;
+    }
+    NRF_LOG_INST_DEBUG(p_fifo->p_log, "Free (interrupted)");
+    return false;
+}

components/libraries/atomic_fifo/nrf_atfifo.h

@@ -0,0 +1,424 @@
+/**
+ * Copyright (c) 2011 - 2021, 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.
+ *
+ */
+#ifndef NRF_ATFIFO_H__
+#define NRF_ATFIFO_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "sdk_config.h"
+#include "nordic_common.h"
+#include "nrf_assert.h"
+#include "sdk_errors.h"
+#include "nrf_log_instance.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup nrf_atfifo Atomic FIFO
+ * @ingroup app_common
+ *
+ * @brief @tagAPI52 FIFO implementation that allows for making atomic transactions without
+ * locking interrupts.
+ *
+ * @details There are two types of functions to prepare the FIFO writing:
+ * - Single function for simple access:
+ * @code
+ * if (NRF_SUCCESS != nrf_atfifo_simple_put(my_fifo, &data, NULL))
+ * {
+ *      // Error handling
+ * }
+ * @endcode
+ * - Function pair to limit data copying:
+ * @code
+ * struct point3d
+ * {
+ *      int x, y, z;
+ * }point3d_t;
+ * nrf_atfifo_context_t context;
+ * point3d_t * point;
+ *
+ * if (NULL != (point = nrf_atfifo_item_alloc(my_fifo, &context)))
+ * {
+ *      point->x = a;
+ *      point->y = b;
+ *      point->z = c;
+ *      if (nrf_atfifo_item_put(my_fifo, &context))
+ *      {
+ *          // Send information to the rest of the system
+ *          // that there is new data in the FIFO available for reading.
+ *      }
+ * }
+ * else
+ * {
+ *      // Error handling
+ * }
+ *
+ * @endcode
+ * @note
+ * This atomic FIFO implementation requires that the operation that is
+ * opened last is finished (committed/flushed) first.
+ * This is typical for operations performed from the interrupt runtime
+ * when the other operation is performed from the main thread.
+ *
+ * This implementation does not support typical multithreading operating system
+ * access where operations can be started and finished in totally unrelated order.
+ *
+ * @{
+ */
+
+/**
+ * @brief Read and write position structure.
+ *
+ * A structure that holds the read and write position used by the FIFO head and tail.
+ */
+typedef struct nrf_atfifo_postag_pos_s
+{
+    uint16_t wr; //!< First free space to write the data
+    uint16_t rd; //!< A place after the last data to read
+}nrf_atfifo_postag_pos_t;
+
+/**
+ * @brief End data index tag.
+ *
+ * A tag used to mark the end of data.
+ * To properly realize atomic data committing, the whole variable has to be
+ * accessed atomically.
+ */
+typedef union nrf_atfifo_postag_u
+{
+    uint32_t                tag; //!< Whole tag, used for atomic, 32-bit access
+    nrf_atfifo_postag_pos_t pos; //!< Structure that holds reading and writing position separately
+}nrf_atfifo_postag_t;
+
+/**
+ * @brief The FIFO instance.
+ *
+ * The instance of atomic FIFO.
+ * Used with all FIFO functions.
+ */
+typedef struct nrf_atfifo_s
+{
+    void                * p_buf;        //!< Pointer to the data buffer
+    nrf_atfifo_postag_t   tail;         //!< Read and write tail position tag
+    nrf_atfifo_postag_t   head;         //!< Read and write head position tag
+    uint16_t              buf_size;     //!< FIFO size in number of bytes (has to be divisible by @c item_size)
+    uint16_t              item_size;    //!< Size of a single FIFO item
+    NRF_LOG_INSTANCE_PTR_DECLARE(p_log) //!< Pointer to instance of the logger object (Conditionally compiled).
+}nrf_atfifo_t;
+
+/**
+ * @brief FIFO write operation item context.
+ *
+ * Context structure used to mark an allocated space in FIFO that is ready for put.
+ * All the data required to properly put allocated and written data.
+ */
+typedef struct nrf_atfifo_item_put_s
+{
+    nrf_atfifo_postag_t last_tail; //!< Tail tag value that was here when opening the FIFO to write
+}nrf_atfifo_item_put_t;
+
+
+/**
+ * @brief FIFO read operation item context.
+ *
+ * Context structure used to mark an opened get operation to properly free an item after reading.
+ */
+typedef struct nrf_atfifo_rcontext_s
+{
+    nrf_atfifo_postag_t last_head; //!< Head tag value that was here when opening the FIFO to read
+}nrf_atfifo_item_get_t;
+
+
+/** @brief Name of the module used for logger messaging.
+ */
+#define NRF_ATFIFO_LOG_NAME atfifo
+
+/**
+ * @defgroup nrf_atfifo_instmacros FIFO instance macros
+ *
+ * A group of macros helpful for FIFO instance creation and initialization.
+ * They may be used to create and initialize instances for most use cases.
+ *
+ * FIFO may also be created and initialized directly using
+ * @ref nrf_atfifo_init function.
+ * @{
+ */
+    /**
+     * @brief Macro for generating the name for a data buffer.
+     *
+     * The name of the data buffer that would be created by
+     * @ref NRF_ATFIFO_DEF macro.
+     *
+     * @param[in] fifo_id Identifier of the FIFO object.
+     *
+     * @return Name of the buffer variable.
+     *
+     * @note This is auxiliary internal macro and in normal usage
+     *       it should not be called.
+     */
+    #define NRF_ATFIFO_BUF_NAME(fifo_id) CONCAT_2(fifo_id, _data)
+
+    /**
+     * @brief Macro for generating the name for a FIFO instance.
+     *
+     * The name of the instance variable that will be created by the
+     * @ref NRF_ATFIFO_DEF macro.
+     *
+     * @param[in] fifo_id Identifier of the FIFO object.
+     *
+     * @return Name of the instance variable.
+     *
+     * @note This is auxiliary internal macro and in normal usage
+     *       it should not be called.
+     */
+    #define NRF_ATFIFO_INST_NAME(fifo_id) CONCAT_2(fifo_id, _inst)
+
+    /**
+     * @brief Macro for creating an instance.
+     *
+     * Creates the FIFO object variable itself.
+     *
+     * Usage example:
+     * @code
+     * NRF_ATFIFO_DEF(my_fifo, uint16_t, 12);
+     * NRF_ATFIFO_INIT(my_fifo);
+     *
+     * uint16_t some_val = 45;
+     * nrf_atfifo_item_put(my_fifo, &some_val, sizeof(some_val), NULL);
+     * nrf_atfifo_item_get(my_fifo, &some_val, sizeof(some_val), NULL);
+     * @endcode
+     *
+     * @param[in] fifo_id      Identifier of a FIFO object.
+     *                         This identifier will be a pointer to the instance.
+     *                         It makes it possible to use this directly for the functions
+     *                         that operate on the FIFO.
+     *                         Because it is a static const object, it should be optimized by the compiler.
+     * @param[in] storage_type Type of data that will be stored in the FIFO.
+     * @param[in] item_cnt     Capacity of the created FIFO in maximum number of items that may be stored.
+     *                         The phisical size of the buffer will be 1 element bigger.
+     */
+    #define NRF_ATFIFO_DEF(fifo_id, storage_type, item_cnt)                                     \
+        static storage_type NRF_ATFIFO_BUF_NAME(fifo_id)[(item_cnt)+1];                         \
+        NRF_LOG_INSTANCE_REGISTER(NRF_ATFIFO_LOG_NAME, fifo_id,                                 \
+                                  NRF_ATFIFO_CONFIG_INFO_COLOR,                                 \
+                                  NRF_ATFIFO_CONFIG_DEBUG_COLOR,                                \
+                                  NRF_ATFIFO_CONFIG_LOG_INIT_FILTER_LEVEL,                      \
+                                  NRF_ATFIFO_CONFIG_LOG_ENABLED ?                               \
+                                          NRF_ATFIFO_CONFIG_LOG_LEVEL : NRF_LOG_SEVERITY_NONE); \
+        static nrf_atfifo_t NRF_ATFIFO_INST_NAME(fifo_id) = {                                   \
+                .p_buf = NULL,                                                                  \
+                NRF_LOG_INSTANCE_PTR_INIT(p_log, NRF_ATFIFO_LOG_NAME, fifo_id)                  \
+        };                                                                                      \
+        static nrf_atfifo_t * const fifo_id = &NRF_ATFIFO_INST_NAME(fifo_id)
+
+    /**
+     * @brief Macro for initializing the FIFO that was previously declared by the macro.
+     *
+     * Use this macro to simplify FIFO initialization.
+     *
+     * @note
+     * This macro can be only used on a FIFO object defined by @ref NRF_ATFIFO_DEF macro.
+     *
+     * @param[in] fifo_id Identifier of the FIFO object.
+     *
+     * @return Value from the @ref nrf_atfifo_init function.
+     */
+    #define NRF_ATFIFO_INIT(fifo_id)                \
+        nrf_atfifo_init(                            \
+            fifo_id,                                \
+            NRF_ATFIFO_BUF_NAME(fifo_id),           \
+            sizeof(NRF_ATFIFO_BUF_NAME(fifo_id)),   \
+            sizeof(NRF_ATFIFO_BUF_NAME(fifo_id)[0]) \
+        )
+
+/** @} */
+
+/**
+ * @brief Function for initializing the FIFO.
+ *
+ * Preparing the FIFO instance to work.
+ *
+ * @param[out]    p_fifo    FIFO object to initialize.
+ * @param[in,out] p_buf     FIFO buffer for storing data.
+ * @param[in]     buf_size  Total buffer size (has to be divisible by @c item_size).
+ * @param[in]     item_size Size of a single item held inside the FIFO.
+ *
+ * @retval     NRF_SUCCESS              If initialization was successful.
+ * @retval     NRF_ERROR_NULL           If a NULL pointer is provided as the buffer.
+ * @retval     NRF_ERROR_INVALID_LENGTH If size of the buffer provided is not divisible by @c item_size.
+ *
+ * @note
+ * Buffer size must be able to hold one element more than the designed FIFO capacity.
+ * This one, empty element is used for overflow checking.
+ */
+ret_code_t nrf_atfifo_init(nrf_atfifo_t * const p_fifo, void * p_buf, uint16_t buf_size, uint16_t item_size);
+
+/**
+ * @brief Function for clearing the FIFO.
+ *
+ * Function for clearing the FIFO.
+ *
+ * If this function is called during an opened and uncommitted write operation,
+ * the FIFO is cleared up to the currently ongoing commit.
+ * There is no possibility to cancel an ongoing commit.
+ *
+ * If this function is called during an opened and unflushed read operation,
+ * the read position in the head is set, but copying it into the write head position
+ * is left to read closing operation.
+ *
+ * This way, there is no more data to read, but the memory is released
+ * in the moment when it is safe.
+ *
+ * @param[in,out] p_fifo FIFO object.
+ *
+ * @retval NRF_SUCCESS    FIFO totally cleared.
+ * @retval NRF_ERROR_BUSY Function called in the middle of writing or reading operation.
+ *                        If it is called in the middle of writing operation,
+ *                        FIFO was cleared up to the already started and uncommitted write.
+ *                        If it is called in the middle of reading operation,
+ *                        write head was only moved. It will be copied into read tail when the reading operation
+ *                        is flushed.
+ */
+ret_code_t nrf_atfifo_clear(nrf_atfifo_t * const p_fifo);
+
+/**
+ * @brief Function for atomically putting data into the FIFO.
+ *
+ * It uses memcpy function inside and in most situations, it is more suitable to
+ * use @ref nrf_atfifo_item_alloc, write the data, and @ref nrf_atfifo_item_put to store a new value
+ * in a FIFO.
+ *
+ * @param[in,out] p_fifo    FIFO object.
+ * @param[in]     p_var     Variable to copy.
+ * @param[in]     size      Size of the variable to copy.
+ *                          Can be smaller or equal to the FIFO item size.
+ * @param[out]    p_visible See value returned by @ref nrf_atfifo_item_put.
+ *                          It may be NULL if the caller does not require the current operation status.
+ *
+ * @retval NRF_SUCCESS      If an element has been successfully added to the FIFO.
+ * @retval NRF_ERROR_NO_MEM If the FIFO is full.
+ *
+ * @note
+ * To avoid data copying, you can use the @ref nrf_atfifo_item_alloc and @ref nrf_atfifo_item_put
+ * functions pair.
+ */
+ret_code_t nrf_atfifo_alloc_put(nrf_atfifo_t * const p_fifo, void const * const p_var, size_t size, bool * const p_visible);
+
+/**
+ * @brief Function for opening the FIFO for writing.
+ *
+ * Function called to start the FIFO write operation and access the given FIFO buffer directly.
+ *
+ * @param[in,out] p_fifo    FIFO object.
+ * @param[out]    p_context Operation context, required by @ref nrf_atfifo_item_put.
+ *
+ * @return Pointer to the space where variable data can be stored.
+ *         NULL if there is no space in the buffer.
+ */
+void * nrf_atfifo_item_alloc(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_put_t * p_context);
+
+/**
+ * @brief Function for closing the writing operation.
+ *
+ * Puts a previously allocated context into FIFO.
+ * This function must be called to commit an opened write operation.
+ * It sets all the buffers and marks the data, so that it is visible to read.
+ *
+ * @param[in,out] p_fifo    FIFO object.
+ * @param[in]     p_context Operation context, filled by the @ref nrf_atfifo_item_alloc function.
+ *
+ * @retval true  Data is currently ready and will be visible to read.
+ * @retval false The internal commit was marked, but the writing operation interrupted another writing operation.
+ *               The data will be available to read when the interrupted operation is committed.
+ */
+bool nrf_atfifo_item_put(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_put_t * p_context);
+
+/**
+ * @brief Function for getting a single value from the FIFO.
+ *
+ * This function gets the value from the top of the FIFO.
+ * The value is removed from the FIFO memory.
+ *
+ * @param[in,out] p_fifo     FIFO object.
+ * @param[out]    p_var      Pointer to the variable to store the data.
+ * @param[in]     size       Size of the data to be loaded.
+ * @param[out]    p_released See the values returned by @ref nrf_atfifo_item_free.
+ *
+ * @retval NRF_SUCCESS         Element was successfully copied from the FIFO memory.
+ * @retval NRF_ERROR_NOT_FOUND No data in the FIFO.
+ */
+ret_code_t nrf_atfifo_get_free(nrf_atfifo_t * const p_fifo, void * const p_var, size_t size, bool * p_released);
+
+/**
+ * @brief Function for opening the FIFO for reading.
+ *
+ * Function called to start the FIFO read operation and access the given FIFO buffer directly.
+ *
+ * @param[in,out] p_fifo    FIFO object.
+ * @param[out]    p_context The operation context, required by @ref nrf_atfifo_item_free
+ *
+ * @return Pointer to data buffer or NULL if there is no data in the FIFO.
+ */
+void * nrf_atfifo_item_get(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_get_t * p_context);
+
+/**
+ * @brief Function for closing the reading operation.
+ *
+ * Function used to finish the reading operation.
+ * If this reading operation does not interrupt another reading operation, the head write buffer is moved.
+ * If this reading operation is placed in the middle of another reading, only the new read pointer is written.
+ *
+ * @param[in,out] p_fifo    FIFO object.
+ * @param[in]     p_context Context of the reading operation to be closed.
+ *
+ * @retval true  This operation is not generated in the middle of another read operation and the write head will be updated to the read head (space is released).
+ * @retval false This operation was performed in the middle of another read operation and the write buffer head was not moved (no space is released).
+ */
+bool nrf_atfifo_item_free(nrf_atfifo_t * const p_fifo, nrf_atfifo_item_get_t * p_context);
+
+
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NRF_ATFIFO_H__ */

components/libraries/atomic_fifo/nrf_atfifo_internal.h

@@ -0,0 +1,577 @@
+/**
+ * Copyright (c) 2011 - 2021, 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
+ * @brief Atomic FIFO internal file
+ *
+ * This file should be included only by nrf_atfifo internally.
+ * Needs nrf_atfifo.h included first.
+ */
+#ifndef NRF_ATFIFO_H__
+#error This is internal file. Do not include this file in your program.
+#endif
+
+#ifndef NRF_ATFIFO_INTERNAL_H__
+#define NRF_ATFIFO_INTERNAL_H__
+#include <stddef.h>
+#include "nrf.h"
+#include "app_util.h"
+#include "nordic_common.h"
+
+#if ((__CORTEX_M >= 0x03U) || (__CORTEX_SC >= 300U)) == 0
+#error Unsupported core version
+#endif
+
+/*
+ * Make sure that rd and wr pos in a tag are aligned like expected
+ * Changing this would require changes inside assembly code!
+ */
+STATIC_ASSERT(offsetof(nrf_atfifo_postag_pos_t, wr) == 0);
+STATIC_ASSERT(offsetof(nrf_atfifo_postag_pos_t, rd) == 2);
+
+/**
+ * @brief Atomically reserve space for a new write.
+ *
+ * @param[in,out] p_fifo   FIFO object.
+ * @param[out]    old_tail Tail position tag before new space is reserved.
+ *
+ * @retval true  Space available.
+ * @retval false Memory full.
+ *
+ * @sa nrf_atfifo_wspace_close
+ */
+static bool nrf_atfifo_wspace_req(nrf_atfifo_t * const p_fifo, nrf_atfifo_postag_t * const p_old_tail);
+
+/**
+ * @brief Atomically mark all written data available.
+ *
+ * This function marks all data available for reading.
+ * This marking is done by copying tail.pos.wr into tail.pos.rd.
+ *
+ * It must be called only when closing the first write.
+ * It cannot be called if any write access was interrupted.
+ * See the code below:
+ * @code
+ *  if (old_tail.pos.wr == old_tail.pos.rd)
+ *  {
+ *      nrf_atfifo_wspace_close(my_fifo);
+ *      return true;
+ *  }
+ *  return false;
+ * @endcode
+ *
+ * @param[in,out] p_fifo     FIFO object.
+ *
+ * @sa nrf_atfifo_wspace_req
+ */
+static void nrf_atfifo_wspace_close(nrf_atfifo_t * const p_fifo);
+
+/**
+ * @brief Atomically get a part of a buffer to read data.
+ *
+ * @param[in,out] p_fifo   FIFO object.
+ * @param[out]    old_head Head position tag before the data buffer is read.
+ *
+ * @retval true  Data available for reading.
+ * @retval false No data in the buffer.
+ *
+ * @sa nrf_atfifo_rspace_close
+ */
+static bool nrf_atfifo_rspace_req(nrf_atfifo_t * const p_fifo, nrf_atfifo_postag_t * const p_old_head);
+
+/**
+ * @brief Atomically release all read data.
+ *
+ * This function marks all data that was read as free space,
+ * which is available for writing.
+ * This marking is done by copying head.pos.rd into head.pos.wr.
+ *
+ * It must be called only when closing the first read.
+ * It cannot be called when the current read access interrupted any other read access.
+ * See code below:
+ * @code
+ * if (old_head.pos.wr == old_head.pos.rd)
+ *  {
+ *      nrf_atfifo_rspace_close(my_fifo);
+ *      return true;
+ *  }
+ *  return false;
+ * @endcode
+ *
+ * @param[in,out] p_fifo     FIFO object.
+ *
+ * @sa nrf_atfifo_rspace_req
+ */
+static void nrf_atfifo_rspace_close(nrf_atfifo_t * const p_fifo);
+
+/**
+ * @brief Safely clear the FIFO, internal function.
+ *
+ * This function realizes the functionality required by @ref nrf_atfifo_clear.
+ *
+ * @param[in,out] p_fifo     FIFO object.
+ *
+ * @retval true  All the data was released.
+ * @retval false All the data available for releasing was released, but there is some pending transfer.
+ */
+static bool nrf_atfifo_space_clear(nrf_atfifo_t * const p_fifo);
+
+
+/* ---------------------------------------------------------------------------
+ * Implementation starts here
+ */
+
+#if defined ( __CC_ARM )
+
+
+__ASM bool nrf_atfifo_wspace_req(nrf_atfifo_t * const p_fifo, nrf_atfifo_postag_t * const p_old_tail)
+{
+    /* Registry usage:
+     * R0 - p_fifo
+     * R1 - p_old_tail
+     * R2 - internal variable old_tail (saved by caller)
+     * R3 - internal variable new_tail (saved by caller)
+     * R4 - internal temporary register (saved by this function)
+     * R5 - not used stored to keep the stack aligned to 8 bytes
+     * Returned value:
+     * R0 (bool - 32 bits)
+     */
+    push {r4, r5}
+nrf_atfifo_wspace_req_repeat
+    /* Load tail tag and set memory monitor !!! R2 - old tail !!! */
+    ldrex r2, [r0, #__cpp(offsetof(nrf_atfifo_t, tail))]
+    /* Extract write position !!! R3 !!! */
+    uxth r3, r2
+    /* Increment address with overload support !!! R4 used temporary !!! */
+    ldrh  r4, [r0, #__cpp(offsetof(nrf_atfifo_t, item_size))]
+    add   r3, r4
+    ldrh  r4, [r0, #__cpp(offsetof(nrf_atfifo_t, buf_size))]
+    cmp   r3, r4
+    it    hs
+    subhs r3, r3, r4
+
+    /* Check if FIFO would overload after making this increment !!! R4 used temporary !!! */
+    ldrh  r4, [r0, #__cpp(offsetof(nrf_atfifo_t, head) + offsetof(nrf_atfifo_postag_pos_t, wr))]
+    cmp   r3, r4
+    ittt  eq
+    clrexeq
+    moveq r0, #__cpp(false)
+    beq   nrf_atfifo_wspace_req_exit
+
+    /* Pack everything back !!! R3 - new tail !!! */
+    /* Copy lower byte from new_tail, and higher byte is a value from the top of old_tail */
+    pkhbt r3, r3, r2
+
+    /* Store new value clearing memory monitor !!! R4 used temporary !!! */
+    strex r4, r3, [r0, #__cpp(offsetof(nrf_atfifo_t, tail))]
+    cmp   r4, #0
+    bne   nrf_atfifo_wspace_req_repeat
+
+    /* Return true */
+    mov r0, #__cpp(true)
+nrf_atfifo_wspace_req_exit
+    /* Save old tail */
+    str r2, [r1]
+    pop {r4, r5}
+    bx  lr
+}
+
+
+__ASM void nrf_atfifo_wspace_close(nrf_atfifo_t * const p_fifo)
+{
+    /* Registry usage:
+     * R0 - p_fifo
+     * R1 - internal temporary register
+     * R2 - new_tail
+     */
+nrf_atfifo_wspace_close_repeat
+    ldrex r2, [r0, #__cpp(offsetof(nrf_atfifo_t, tail))]
+    /* Copy from lower byte to higher  */
+    pkhbt r2, r2, r2, lsl #16
+
+    strex r1, r2, [r0, #__cpp(offsetof(nrf_atfifo_t, tail))]
+    cmp   r1, #0
+    bne   nrf_atfifo_wspace_close_repeat
+    bx    lr
+}
+
+
+__ASM bool nrf_atfifo_rspace_req(nrf_atfifo_t * const p_fifo, nrf_atfifo_postag_t * const p_old_head)
+{
+    /* Registry usage:
+     * R0 - p_fifo
+     * R1 - p_old_head
+     * R2 - internal variable old_head (saved by caller)
+     * R3 - internal variable new_head (saved by caller)
+     * R4 - internal temporary register (saved by this function)
+     * R5 - not used stored to keep the stack aligned to 8 bytes
+     * Returned value:
+     * R0 (bool - 32 bits)
+     */
+    push {r4, r5}
+nrf_atfifo_rspace_req_repeat
+    /* Load tail tag and set memory monitor !!! R2 - old tail !!! */
+    ldrex r2, [r0, #__cpp(offsetof(nrf_atfifo_t, head))]
+    /* Extract read position !!! R3 !!! */
+    uxth r3, r2, ror #16
+
+    /* Check if we have any data !!! R4 used temporary !!! */
+    ldrh  r4, [r0, #__cpp(offsetof(nrf_atfifo_t, tail) + offsetof(nrf_atfifo_postag_pos_t, rd))]
+    cmp   r3, r4
+    ittt  eq
+    clrexeq
+    moveq r0, #__cpp(false)
+    beq   nrf_atfifo_rspace_req_exit
+
+    /* Increment address with overload support !!! R4 used temporary !!! */
+    ldrh  r4, [r0, #__cpp(offsetof(nrf_atfifo_t, item_size))]
+    add   r3, r4
+    ldrh  r4, [r0, #__cpp(offsetof(nrf_atfifo_t, buf_size))]
+    cmp   r3, r4
+    it    hs
+    subhs r3, r3, r4
+
+    /* Pack everything back !!! R3 - new tail !!! */
+    /* Copy lower byte from old_head, and higher byte is a value from write_pos */
+    pkhbt r3, r2, r3, lsl #16
+
+    /* Store new value clearing memory monitor !!! R4 used temporary !!! */
+    strex r4, r3, [r0, #__cpp(offsetof(nrf_atfifo_t, head))]
+    cmp   r4, #0
+    bne   nrf_atfifo_rspace_req_repeat
+
+    /* Return true */
+    mov r0, #__cpp(true)
+nrf_atfifo_rspace_req_exit
+    /* Save old head */
+    str r2, [r1]
+    pop {r4, r5}
+    bx  lr
+}
+
+
+__ASM void nrf_atfifo_rspace_close(nrf_atfifo_t * const p_fifo)
+{
+    /* Registry usage:
+     * R0 - p_fifo
+     * R1 - internal temporary register
+     * R2 - new_tail
+     */
+nrf_atfifo_rspace_close_repeat
+    ldrex r2, [r0, #__cpp(offsetof(nrf_atfifo_t, head))]
+    /* Copy from higher byte to lower */
+    pkhtb r2, r2, r2, asr #16
+
+    strex r1, r2, [r0, #__cpp(offsetof(nrf_atfifo_t, head))]
+    cmp   r1, #0
+    bne   nrf_atfifo_rspace_close_repeat
+    bx    lr
+}
+
+
+__ASM bool nrf_atfifo_space_clear(nrf_atfifo_t * const p_fifo)
+{
+    /* Registry usage:
+     * R0 - p_fifo as input, bool output after
+     * R1 - tail, rd pointer, new_head
+     * R2 - head_old, destroyed when creating new_head
+     * R3 - p_fifo - copy
+     */
+    mov     r3, r0
+nrf_atfifo_space_clear_repeat
+    /* Load old head in !!! R2 register !!! and read pointer of tail in !!! R1 register !!! */
+    ldrex   r2, [r3, #__cpp(offsetof(nrf_atfifo_t, head))]
+    ldrh    r1, [r3, #__cpp(offsetof(nrf_atfifo_t, tail) + offsetof(nrf_atfifo_postag_pos_t, rd))]
+    cmp     r2, r2, ror #16
+    /* Return false as default */
+    mov     r0, #__cpp(false)
+    /* Create new head in !!! R1 register !!! Data in !!! R2 register broken !!! */
+    itett   ne
+    uxthne  r2, r2
+    orreq   r1, r1, r1, lsl #16
+    orrne   r1, r2, r1, lsl #16
+
+    /* Skip header test */
+    bne     nrf_atfifo_space_clear_head_test_skip
+
+    /* Load whole tail and test it !!! R2 used !!! */
+    ldr     r2, [r3, #__cpp(offsetof(nrf_atfifo_t, tail))]
+    cmp     r2, r2, ror #16
+    /* Return true if equal */
+    it      eq
+    moveq   r0, #__cpp(true)
+
+nrf_atfifo_space_clear_head_test_skip
+    /* Store and test if success !!! R2 used temporary !!! */
+    strex   r2, r1, [r3, #__cpp(offsetof(nrf_atfifo_t, head))]
+    cmp     r2, #0
+    bne     nrf_atfifo_space_clear_repeat
+    bx      lr
+}
+
+#elif defined ( __ICCARM__ ) || defined ( __GNUC__ )
+
+bool nrf_atfifo_wspace_req(nrf_atfifo_t * const p_fifo, nrf_atfifo_postag_t * const p_old_tail)
+{
+    volatile bool ret;
+    volatile uint32_t old_tail;
+    uint32_t new_tail;
+    uint32_t temp;
+
+    __ASM volatile(
+        /* For more comments see Keil version above */
+        "1:                                                             \n"
+        "   ldrex %[old_tail], [%[p_fifo], %[offset_tail]]              \n"
+        "   uxth %[new_tail], %[old_tail]                               \n"
+        "                                                               \n"
+        "   ldrh  %[temp], [%[p_fifo], %[offset_item_size]]             \n"
+        "   add   %[new_tail], %[temp]                                  \n"
+        "   ldrh  %[temp], [%[p_fifo], %[offset_buf_size]]              \n"
+        "   cmp   %[new_tail], %[temp]                                  \n"
+        "   it    hs                                                    \n"
+        "   subhs %[new_tail], %[new_tail], %[temp]                     \n"
+        "                                                               \n"
+        "   ldrh  %[temp], [%[p_fifo], %[offset_head_wr]]               \n"
+        "   cmp   %[new_tail], %[temp]                                  \n"
+        "   ittt  eq                                                    \n"
+        "   clrexeq                                                     \n"
+        "   moveq %[ret], %[false_val]                                  \n"
+        "   beq.n 2f                                                    \n"
+        "                                                               \n"
+        "   pkhbt %[new_tail], %[new_tail], %[old_tail]                 \n"
+        "                                                               \n"
+        "   strex %[temp], %[new_tail], [%[p_fifo], %[offset_tail]]     \n"
+        "   cmp   %[temp], #0                                           \n"
+        "   bne.n 1b                                                    \n"
+        "                                                               \n"
+        "   mov %[ret], %[true_val]                                     \n"
+        "2:                                                             \n"
+        : /* Output operands */
+            [ret]     "=r"(ret),
+            [temp]    "=&r"(temp),
+            [old_tail]"=&r"(old_tail),
+            [new_tail]"=&r"(new_tail)
+        : /* Input operands */
+            [p_fifo]          "r"(p_fifo),
+            [offset_tail]     "J"(offsetof(nrf_atfifo_t, tail)),
+            [offset_head_wr]  "J"(offsetof(nrf_atfifo_t, head) + offsetof(nrf_atfifo_postag_pos_t, wr)),
+            [offset_item_size]"J"(offsetof(nrf_atfifo_t, item_size)),
+            [offset_buf_size] "J"(offsetof(nrf_atfifo_t, buf_size)),
+            [true_val]        "I"(true),
+            [false_val]       "I"(false)
+        : /* Clobbers */
+            "cc");
+
+    p_old_tail->tag = old_tail;
+    UNUSED_VARIABLE(new_tail);
+    UNUSED_VARIABLE(temp);
+    return ret;
+}
+
+
+void nrf_atfifo_wspace_close(nrf_atfifo_t * const p_fifo)
+{
+    uint32_t temp;
+    uint32_t new_tail;
+
+    __ASM volatile(
+        /* For more comments see Keil version above */
+        "1:                                                         \n"
+        "   ldrex %[new_tail], [%[p_fifo], %[offset_tail]]          \n"
+        "   pkhbt %[new_tail],%[new_tail], %[new_tail], lsl #16     \n"
+        "                                                           \n"
+        "   strex %[temp], %[new_tail], [%[p_fifo], %[offset_tail]] \n"
+        "   cmp   %[temp], #0                                       \n"
+        "   bne.n 1b                                                \n"
+        : /* Output operands */
+            [temp]     "=&r"(temp),
+            [new_tail] "=&r"(new_tail)
+        : /* Input operands */
+            [p_fifo]      "r"(p_fifo),
+            [offset_tail] "J"(offsetof(nrf_atfifo_t, tail))
+        : /* Clobbers */
+            "cc");
+
+    UNUSED_VARIABLE(temp);
+    UNUSED_VARIABLE(new_tail);
+}
+
+
+bool nrf_atfifo_rspace_req(nrf_atfifo_t * const p_fifo, nrf_atfifo_postag_t * const p_old_head)
+{
+    volatile bool ret;
+    volatile uint32_t old_head;
+    uint32_t new_head;
+    uint32_t temp;
+
+    __ASM volatile(
+        /* For more comments see Keil version above */
+        "1:                                                         \n"
+        "   ldrex %[old_head], [%[p_fifo], %[offset_head]]          \n"
+        "   uxth %[new_head], %[old_head], ror #16                  \n"
+        "                                                           \n"
+        "   ldrh  %[temp], [%[p_fifo], %[offset_tail_rd]]           \n"
+        "   cmp   %[new_head], %[temp]                              \n"
+        "   ittt  eq                                                \n"
+        "   clrexeq                                                 \n"
+        "   moveq %[ret], %[false_val]                              \n"
+        "   beq.n   2f                                              \n"
+        "                                                           \n"
+        "   ldrh  %[temp], [%[p_fifo], %[offset_item_size]]         \n"
+        "   add   %[new_head], %[temp]                              \n"
+        "   ldrh  %[temp], [%[p_fifo], %[offset_buf_size]]          \n"
+        "   cmp   %[new_head], %[temp]                              \n"
+        "   it    hs                                                \n"
+        "   subhs %[new_head], %[new_head], %[temp]                 \n"
+        "                                                           \n"
+        "   pkhbt %[new_head], %[old_head], %[new_head], lsl #16    \n"
+        "                                                           \n"
+        "   strex %[temp], %[new_head], [%[p_fifo], %[offset_head]] \n"
+        "   cmp   %[temp], #0                                       \n"
+        "   bne.n 1b                                                \n"
+        "                                                           \n"
+        "   mov %[ret], %[true_val]                                 \n"
+        "2:                                                         \n"
+        : /* Output operands */
+            [ret]     "=r"(ret),
+            [temp]    "=&r"(temp),
+            [old_head]"=&r"(old_head),
+            [new_head]"=&r"(new_head)
+        : /* Input operands */
+            [p_fifo]          "r"(p_fifo),
+            [offset_head]     "J"(offsetof(nrf_atfifo_t, head)),
+            [offset_tail_rd]  "J"(offsetof(nrf_atfifo_t, tail) + offsetof(nrf_atfifo_postag_pos_t, rd)),
+            [offset_item_size]"J"(offsetof(nrf_atfifo_t, item_size)),
+            [offset_buf_size] "J"(offsetof(nrf_atfifo_t, buf_size)),
+            [true_val]        "I"(true),
+            [false_val]       "I"(false)
+        : /* Clobbers */
+            "cc");
+
+    p_old_head->tag = old_head;
+    UNUSED_VARIABLE(new_head);
+    UNUSED_VARIABLE(temp);
+    return ret;
+}
+
+
+void nrf_atfifo_rspace_close(nrf_atfifo_t * const p_fifo)
+{
+    uint32_t temp;
+    uint32_t new_head;
+
+    __ASM volatile(
+        /* For more comments see Keil version above */
+        "1:                                                         \n"
+        "   ldrex %[new_head], [%[p_fifo], %[offset_head]]          \n"
+        "   pkhtb %[new_head],%[new_head], %[new_head], asr #16     \n"
+        "                                                           \n"
+        "   strex %[temp], %[new_head], [%[p_fifo], %[offset_head]] \n"
+        "   cmp   %[temp], #0                                       \n"
+        "   bne.n 1b                                                \n"
+        : /* Output operands */
+            [temp]     "=&r"(temp),
+            [new_head] "=&r"(new_head)
+        : /* Input operands */
+            [p_fifo]      "r"(p_fifo),
+            [offset_head] "J"(offsetof(nrf_atfifo_t, head))
+        : /* Clobbers */
+            "cc");
+
+    UNUSED_VARIABLE(temp);
+    UNUSED_VARIABLE(new_head);
+}
+
+
+bool nrf_atfifo_space_clear(nrf_atfifo_t * const p_fifo)
+{
+    volatile bool ret;
+    uint32_t old_head; /* This variable is left broken after assembly code finishes */
+    uint32_t new_head;
+
+    __ASM volatile(
+        "1:                                                               \n"
+        "   ldrex   %[old_head], [%[p_fifo], %[offset_head]]              \n"
+        "   ldrh    %[new_head], [%[p_fifo], %[offset_tail_rd]]           \n"
+        "   cmp     %[old_head], %[old_head], ror #16                     \n"
+        "                                                                 \n"
+        "   mov     %[ret], %[false_val]                                  \n"
+        "                                                                 \n"
+        "   itett   ne                                                    \n"
+        "   uxthne  %[old_head], %[old_head]                              \n"
+        "   orreq   %[new_head], %[new_head], %[new_head], lsl #16        \n"
+        "   orrne   %[new_head], %[old_head], %[new_head], lsl #16        \n"
+        "                                                                 \n"
+        "   bne.n   2f                                                    \n"
+        "                                                                 \n"
+        "   ldr     %[old_head], [%[p_fifo], %[offset_tail]]              \n"
+        "   cmp     %[old_head], %[old_head], ror #16                     \n"
+        "   it      eq                                                    \n"
+        "   moveq   %[ret], %[true_val]                                   \n"
+        "                                                                 \n"
+        "2:                                                               \n"
+        "   strex   %[old_head], %[new_head], [%[p_fifo], %[offset_head]] \n"
+        "   cmp     %[old_head], #0                                       \n"
+        "   bne.n     1b                                                  \n"
+        : /* Output operands */
+            [ret]      "=&r"(ret),
+            [old_head] "=&r"(old_head),
+            [new_head] "=&r"(new_head)
+        : /* Input operands */
+            [p_fifo]         "r"(p_fifo),
+            [offset_head]    "J"(offsetof(nrf_atfifo_t, head)),
+            [offset_tail]    "J"(offsetof(nrf_atfifo_t, tail)),
+            [offset_tail_rd] "J"(offsetof(nrf_atfifo_t, tail) + offsetof(nrf_atfifo_postag_pos_t, rd)),
+            [true_val]       "I"(true),
+            [false_val]      "I"(false)
+        : /* Clobbers */
+            "cc");
+
+    UNUSED_VARIABLE(old_head);
+    UNUSED_VARIABLE(new_head);
+    return ret;
+}
+
+#else
+#error Unsupported compiler
+#endif
+
+#endif /* NRF_ATFIFO_INTERNAL_H__ */

components/libraries/atomic_flags/nrf_atflags.c

@@ -0,0 +1,160 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+
+#include "nrf.h"
+#include "nrf_atomic.h"
+#include "nrf_atflags.h"
+#include "sdk_common.h"
+
+
+
+/**@brief Macro for getting the index inside the flag array where a flag can be found.
+ *
+ * @param flag_index  Index of the flag.
+ *
+ * @return Index of the @ref nrf_atflags_t the flag can be found in.
+ */
+#define FLAG_BASE(flag_index) ((flag_index) / NRF_ATFLAGS_FLAGS_PER_ELEMENT)
+
+/**@brief Macro for getting the mask representing the flag within the flag array member.
+ *
+ * @param flag_index  ID of the flag.
+ *
+ * @return Mask representing the flag within a single @ref nrf_atflags_t.
+ */
+#define FLAG_MASK(flag_index) (1UL << ((flag_index) % NRF_ATFLAGS_FLAGS_PER_ELEMENT))
+
+
+void nrf_atflags_set(nrf_atflags_t * p_flags, uint32_t flag_index)
+{
+    uint32_t new_value = nrf_atomic_u32_or(&p_flags[FLAG_BASE(flag_index)], FLAG_MASK(flag_index));
+    UNUSED_RETURN_VALUE(new_value);
+}
+
+
+bool nrf_atflags_fetch_set(nrf_atflags_t * p_flags, uint32_t flag_index)
+{
+    return (nrf_atomic_u32_fetch_or(&p_flags[FLAG_BASE(flag_index)], FLAG_MASK(flag_index))
+            & FLAG_MASK(flag_index)) != 0;
+}
+
+
+void nrf_atflags_clear(nrf_atflags_t * p_flags, uint32_t flag_index)
+{
+    uint32_t new_value = nrf_atomic_u32_and(&p_flags[FLAG_BASE(flag_index)], ~FLAG_MASK(flag_index));
+    UNUSED_RETURN_VALUE(new_value);
+}
+
+
+bool nrf_atflags_fetch_clear(nrf_atflags_t * p_flags, uint32_t flag_index)
+{
+    return (nrf_atomic_u32_fetch_and(&p_flags[FLAG_BASE(flag_index)], ~FLAG_MASK(flag_index))
+            & FLAG_MASK(flag_index)) != 0;
+}
+
+
+bool nrf_atflags_get(nrf_atflags_t const * p_flags, uint32_t flag_index)
+{
+    return (p_flags[FLAG_BASE(flag_index)] & FLAG_MASK(flag_index)) != 0;
+}
+
+
+uint32_t nrf_atflags_init(nrf_atflags_t * p_flags, uint32_t flags_array_len, uint32_t flag_count)
+{
+    uint32_t required_flags_array_len = NRF_ATFLAGS_ARRAY_LEN(flag_count);
+
+    if (required_flags_array_len <= flags_array_len)
+    {
+        for (uint32_t i = 0; i < required_flags_array_len; i++)
+        {
+            p_flags[i] = 0;
+        }
+        return required_flags_array_len;
+    }
+    return 0;
+}
+
+uint32_t nrf_atflags_find_and_set_flag(nrf_atflags_t * p_flags, uint32_t flag_count)
+{
+    for (uint32_t i = 0; i < NRF_ATFLAGS_ARRAY_LEN(flag_count); i++)
+    {
+        // Using __RBIT to make the order of flags more traditional.
+        uint32_t first_zero = __CLZ(__RBIT(~p_flags[i]));
+        while (first_zero < 32)
+        {
+            uint32_t first_zero_global = first_zero + (i * 32);
+            if (first_zero_global >= flag_count)
+            {
+                break;
+            }
+            if (!nrf_atflags_fetch_set(p_flags, first_zero_global))
+            {
+                return first_zero_global;
+            }
+            first_zero = __CLZ(__RBIT(~p_flags[i]));
+        }
+    }
+
+    return flag_count;
+}
+
+uint32_t nrf_atflags_find_and_clear_flag(nrf_atflags_t * p_flags, uint32_t flag_count)
+{
+    for (uint32_t i = 0; i < NRF_ATFLAGS_ARRAY_LEN(flag_count); i++)
+    {
+        // Using __RBIT to make the order of flags more traditional.
+        uint32_t first_one = __CLZ(__RBIT(p_flags[i]));
+        while (first_one < 32)
+        {
+            uint32_t first_one_global = first_one + (i * 32);
+            if (first_one_global >= flag_count)
+            {
+                break;
+            }
+            if (nrf_atflags_fetch_clear(p_flags, first_one_global))
+            {
+                return first_one_global;
+            }
+            first_one = __CLZ(__RBIT(p_flags[i]));
+        }
+    }
+
+    return flag_count;
+}

components/libraries/atomic_flags/nrf_atflags.h

@@ -0,0 +1,184 @@
+/**
+ * Copyright (c) 2017 - 2021, 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 nrf_atflags Atomic flags (bitmaps)
+ * @ingroup app_common
+ * @{
+ *
+ * @brief @tagAPI52 This module implements atomic flags as bitmaps.
+ *
+ * Operations on the individual flags are atomic, meaning that you are always sure that the flag is
+ * set to the desired value, and you always know what value was there before. You also know that no
+ * other flags were affected by the operation.
+ *
+ * Operations on the entire flag collection are NOT atomic. This essentially means that you can't
+ * know the order in which operations on different flags happened, and you can't know the state of
+ * the entire flag collection at any instant. These limitations can be overcome by protecting
+ * operations with a mutex.
+ */
+
+#ifndef NRF_ATFLAGS_H__
+#define NRF_ATFLAGS_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * @brief Array of atomic flags.
+ * */
+typedef volatile uint32_t nrf_atflags_t;
+
+
+/**
+ * @brief Number of flags per @ref nrf_atflags_t.
+ * */
+#define NRF_ATFLAGS_FLAGS_PER_ELEMENT (sizeof(nrf_atflags_t) * 8)
+
+/**@brief Macro for the length of an array of @ref nrf_atflags_t needed to keep \p flag_count flags.
+ *
+ * @param flag_count  Number of flags to keep in a flag array.
+ *
+ * @return Length of the array needed to house flag_count flags.
+ */
+#define NRF_ATFLAGS_ARRAY_LEN(flag_count) ((flag_count - 1) / NRF_ATFLAGS_FLAGS_PER_ELEMENT) + 1
+
+/**@brief Macro for declaring a flag array with the right size and initial value.
+ *
+ * @note When using this macro, no call to @ref nrf_atflags_init is necessary for this array.
+ *
+ * @param _name    Name to be given to the array.
+ * @param flag_count  Number of flags to be kept in the flag array.
+ *
+ * @return Flag array definition.
+ */
+#define NRF_ATFLAGS_DEF(_name, flag_count) \
+    nrf_atflags_t _name[NRF_ATFLAGS_ARRAY_LEN((flag_count))] = {0}
+
+/**@brief Macro for defining a flag array inside a struct.
+ *
+ * @note When using this macro, make sure to set the array to 0 or use @ref nrf_atflags_init
+ *
+ * @param _name    Name to be given to the array.
+ * @param flag_count  Number of flags to be kept in the flag array.
+ *
+ * @return The flag array definition.
+ */
+#define NRF_ATFLAGS_DEF_MEMBER(_name, flag_count) \
+    nrf_atflags_t _name[NRF_ATFLAGS_ARRAY_LEN((flag_count))]
+
+/**@brief Function for safely initializing a flag array to 0.
+ *
+ * @param p_flags          Flag array to initialize.
+ * @param flags_array_len  Length of \p p_flags.
+ * @param flag_count          Number of flags to be kept in the flag array.
+ *
+ * @retval  0 if the given length is not sufficient to house \p flag_count flags in the array.
+ * @return  If successful: The actual length required.
+ */
+uint32_t nrf_atflags_init(nrf_atflags_t * p_flags, uint32_t flags_array_len, uint32_t flag_count);
+
+/**@brief Function for atomically setting a flag to 1.
+ *
+ * @param[in] p_flags     Atomic flag array.
+ * @param[in] flag_index  Index of the flag in the array.
+ */
+void nrf_atflags_set(nrf_atflags_t * p_flags, uint32_t flag_index);
+
+/**@brief Function for atomically setting a flag to 1, returning the previous value of the flag.
+ *
+ * @param[in] p_flags     Atomic flag array.
+ * @param[in] flag_index  Index of the flag in the array.
+ *
+ * @return Old flag value.
+ */
+bool nrf_atflags_fetch_set(nrf_atflags_t * p_flags, uint32_t flag_index);
+
+/**@brief Function for atomically setting a flag to 0.
+ *
+ * @param[in] p_flags     Atomic flag array.
+ * @param[in] flag_index  Index of the flag in the array.
+ */
+void nrf_atflags_clear(nrf_atflags_t * p_flags, uint32_t flag_index);
+
+/**@brief Function for atomically setting a flag to 0, returning the previous value of the flag.
+ *
+ * @param[in] p_flags     Atomic flag array.
+ * @param[in] flag_index  Index of the flag in the array.
+ *
+ * @return Old flag value.
+ */
+bool nrf_atflags_fetch_clear(nrf_atflags_t * p_flags, uint32_t flag_index);
+
+/**@brief Function for getting the value of a flag in a flag array.
+ *
+ * @param[in] p_flags     Atomic flag array.
+ * @param[in] flag_index  Index of the flag in the array.
+ *
+ * @return Flag value.
+ */
+bool nrf_atflags_get(nrf_atflags_t const * p_flags, uint32_t flag_index);
+
+/**@brief Function for finding a flag with value 0, and atomically setting it to one.
+ *
+ * @param[in] p_flags  Atomic flag array.
+ * @param[in] flag_count  Number of flags in the array.
+ *
+ * @return Index of the cleared flag that has been set.
+ */
+uint32_t nrf_atflags_find_and_set_flag(nrf_atflags_t * p_flags, uint32_t flag_count);
+
+/**@brief Function for finding a flag with value 1, and atomically clearing it to 0.
+ *
+ * @param[in] p_flags  Atomic flag array.
+ * @param[in] flag_count  The number of flags in the array.
+ *
+ * @return The index of the set flag that has been cleared.
+ */
+uint32_t nrf_atflags_find_and_clear_flag(nrf_atflags_t * p_flags, uint32_t flag_count);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NRF_ATFLAGS_H__ */
+
+/** @} */

components/libraries/balloc/nrf_balloc.c

@@ -0,0 +1,399 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#include "sdk_common.h"
+ #if NRF_MODULE_ENABLED(NRF_BALLOC)
+
+#include "nrf_section.h"
+#include "nrf_balloc.h"
+#include "app_util_platform.h"
+
+
+#if NRF_BALLOC_CONFIG_LOG_ENABLED
+    #define NRF_LOG_LEVEL             NRF_BALLOC_CONFIG_LOG_LEVEL
+    #define NRF_LOG_INITIAL_LEVEL     NRF_BALLOC_CONFIG_INITIAL_LOG_LEVEL
+    #define NRF_LOG_INFO_COLOR        NRF_BALLOC_CONFIG_INFO_COLOR
+    #define NRF_LOG_DEBUG_COLOR       NRF_BALLOC_CONFIG_DEBUG_COLOR
+#else
+    #define NRF_LOG_LEVEL       0
+#endif // NRF_BALLOC_CONFIG_LOG_ENABLED
+#include "nrf_log.h"
+
+#define HEAD_GUARD_FILL     0xBAADF00D      /**< Magic number used to mark head guard.*/
+#define TAIL_GUARD_FILL     0xBAADCAFE      /**< Magic number used to mark tail guard.*/
+#define FREE_MEM_FILL       0xBAADBAAD      /**< Magic number used to mark free memory.*/
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+#define POOL_ID(_p_pool) _p_pool->p_name
+#define POOL_MARKER     "%s"
+#else
+#define POOL_ID(_p_pool) _p_pool
+#define POOL_MARKER     "0x%08X"
+#endif
+
+NRF_SECTION_DEF(nrf_balloc, nrf_balloc_t);
+
+#if NRF_BALLOC_CLI_CMDS && NRF_CLI_ENABLED
+#include "nrf_cli.h"
+
+static void nrf_balloc_status(nrf_cli_t const * p_cli, size_t argc, char **argv)
+{
+    UNUSED_PARAMETER(argv);
+
+    if (nrf_cli_help_requested(p_cli))
+    {
+        nrf_cli_help_print(p_cli, NULL, 0);
+        return;
+    }
+
+    if (argc > 1)
+    {
+        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Bad argument count");
+        return;
+    }
+
+    uint32_t num_of_instances = NRF_SECTION_ITEM_COUNT(nrf_balloc, nrf_balloc_t);
+    uint32_t i;
+
+    for (i = 0; i < num_of_instances; i++)
+    {
+        const nrf_balloc_t * p_instance = NRF_SECTION_ITEM_GET(nrf_balloc, nrf_balloc_t, i);
+
+        uint32_t element_size = NRF_BALLOC_ELEMENT_SIZE(p_instance);
+        uint32_t dbg_addon  = p_instance->block_size - element_size;
+        uint32_t pool_size  = p_instance->p_stack_limit - p_instance->p_stack_base;
+        uint32_t max_util   = nrf_balloc_max_utilization_get(p_instance);
+        uint32_t util       = nrf_balloc_utilization_get(p_instance);
+        const char * p_name = p_instance->p_name;
+        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL,
+                        "%s\r\n\t- Element size:\t%d + %d bytes of debug information\r\n"
+                        "\t- Usage:\t%u%% (%u out of %u elements)\r\n"
+                        "\t- Maximum:\t%u%% (%u out of %u elements)\r\n\r\n",
+                        p_name, element_size, dbg_addon,
+                        100ul * util/pool_size, util,pool_size,
+                        100ul * max_util/pool_size, max_util,pool_size);
+
+    }
+}
+// Register "balloc" command and its subcommands in CLI.
+NRF_CLI_CREATE_STATIC_SUBCMD_SET(nrf_balloc_commands)
+{
+     NRF_CLI_CMD(status, NULL, "Print status of balloc instances.", nrf_balloc_status),
+     NRF_CLI_SUBCMD_SET_END
+};
+
+NRF_CLI_CMD_REGISTER(balloc, &nrf_balloc_commands, "Commands for BALLOC management", nrf_balloc_status);
+#endif //NRF_BALLOC_CLI_CMDS
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+/**@brief  Validate block memory, prepare block guards, and calculate pointer to the element.
+ *
+ * @param[in]   p_pool  Pointer to the memory pool.
+ * @param[in]   p_head  Pointer to the beginning of the block.
+ *
+ * @return      Pointer to the element.
+ */
+__STATIC_INLINE void * nrf_balloc_block_unwrap(nrf_balloc_t const * p_pool, void * p_head)
+{
+    ASSERT((p_pool != NULL) && ((p_pool->block_size % sizeof(uint32_t)) == 0));
+    ASSERT((p_head != NULL) && (((uint32_t)(p_head) % sizeof(uint32_t)) == 0));
+
+    uint32_t head_words  = NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_GET(p_pool->debug_flags);
+    uint32_t tail_words  = NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_GET(p_pool->debug_flags);
+
+    uint32_t * p_tail    = (uint32_t *)((size_t)(p_head) + p_pool->block_size);
+    uint32_t * p_element = (uint32_t *)p_head + head_words;
+
+    if (NRF_BALLOC_DEBUG_DATA_TRASHING_CHECK_GET(p_pool->debug_flags))
+    {
+        for (uint32_t * ptr = p_head; ptr < p_tail; ptr++)
+        {
+            if (*ptr != FREE_MEM_FILL)
+            {
+                NRF_LOG_INST_ERROR(p_pool->p_log,
+                                   "Detected free memory corruption at 0x%08X (0x%08X != 0x%08X)",
+                                   ptr, *ptr, FREE_MEM_FILL);
+                APP_ERROR_CHECK_BOOL(false);
+            }
+        }
+    }
+
+    for (uint32_t * ptr = p_head; ptr < p_element; ptr++)
+    {
+        *ptr = HEAD_GUARD_FILL;
+    }
+
+    for (uint32_t * ptr = ( p_tail - tail_words); ptr < p_tail; ptr++)
+    {
+        *ptr = TAIL_GUARD_FILL;
+    }
+
+    return p_element;
+}
+
+/**@brief  Calculate pointer to the block, validate block guards, and mark block memory as free.
+ *
+ * @param[in]   p_pool      Pointer to the memory pool.
+ * @param[in]   p_element   Pointer to the element.
+ *
+ * @return      Pointer to the beginning of the block.
+ */
+__STATIC_INLINE void * nrf_balloc_element_wrap(nrf_balloc_t const * p_pool, void * p_element)
+{
+    ASSERT((p_pool    != NULL) && ((p_pool->block_size % sizeof(uint32_t))    == 0));
+    ASSERT((p_element != NULL) && (((uint32_t)(p_element) % sizeof(uint32_t)) == 0));
+
+    uint32_t head_words  = NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_GET(p_pool->debug_flags);
+    uint32_t tail_words  = NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_GET(p_pool->debug_flags);
+
+    uint32_t * p_head   = (uint32_t *)p_element - head_words;
+    uint32_t * p_tail   = (uint32_t *)((size_t)(p_head) + p_pool->block_size);
+
+    for (uint32_t * ptr = p_head; ptr < (uint32_t *)p_element; ptr++)
+    {
+        if (*ptr != HEAD_GUARD_FILL)
+        {
+            NRF_LOG_INST_ERROR(p_pool->p_log,
+                               "Detected Head Guard corruption at 0x%08X (0x%08X != 0x%08X)",
+                               ptr, *ptr, HEAD_GUARD_FILL);
+            APP_ERROR_CHECK_BOOL(false);
+        }
+    }
+
+    for (uint32_t * ptr = ( p_tail - tail_words); ptr < p_tail; ptr++)
+    {
+        if (*ptr != TAIL_GUARD_FILL)
+        {
+            NRF_LOG_INST_ERROR(p_pool->p_log,
+                               "Detected Tail Guard corruption at 0x%08X (0x%08X != 0x%08X)",
+                               ptr, *ptr, TAIL_GUARD_FILL);
+            APP_ERROR_CHECK_BOOL(false);
+        }
+    }
+
+    if (NRF_BALLOC_DEBUG_DATA_TRASHING_CHECK_GET(p_pool->debug_flags))
+    {
+        for (uint32_t * ptr = p_head; ptr < p_tail; ptr++)
+        {
+            *ptr = FREE_MEM_FILL;
+        }
+    }
+
+    return p_head;
+}
+
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+
+/**@brief  Convert block index to a pointer.
+ *
+ * @param[in]   p_pool      Pointer to the memory pool.
+ * @param[in]   idx         Index of the block.
+ *
+ * @return      Pointer to the beginning of the block.
+ */
+static void * nrf_balloc_idx2block(nrf_balloc_t const * p_pool, uint8_t idx)
+{
+    ASSERT(p_pool != NULL);
+    return (uint8_t *)(p_pool->p_memory_begin) + ((size_t)(idx) * p_pool->block_size);
+}
+
+/**@brief  Convert block pointer to index.
+ *
+ * @param[in]   p_pool      Pointer to the memory pool.
+ * @param[in]   p_block     Pointer to the beginning of the block.
+ *
+ * @return      Index of the block.
+ */
+static uint8_t nrf_balloc_block2idx(nrf_balloc_t const * p_pool, void const * p_block)
+{
+    ASSERT(p_pool != NULL);
+    return ((size_t)(p_block) - (size_t)(p_pool->p_memory_begin)) / p_pool->block_size;
+}
+
+ret_code_t nrf_balloc_init(nrf_balloc_t const * p_pool)
+{
+    uint8_t pool_size;
+
+    VERIFY_PARAM_NOT_NULL(p_pool);
+
+    ASSERT(p_pool->p_cb);
+    ASSERT(p_pool->p_stack_base);
+    ASSERT(p_pool->p_stack_limit);
+    ASSERT(p_pool->p_memory_begin);
+    ASSERT(p_pool->block_size);
+
+    pool_size       = p_pool->p_stack_limit - p_pool->p_stack_base;
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    void *p_memory_end = (uint8_t *)(p_pool->p_memory_begin) + (pool_size * p_pool->block_size);
+    if (NRF_BALLOC_DEBUG_DATA_TRASHING_CHECK_GET(p_pool->debug_flags))
+    {
+        for (uint32_t * ptr = p_pool->p_memory_begin; ptr < (uint32_t *)(p_memory_end); ptr++)
+        {
+            *ptr = FREE_MEM_FILL;
+        }
+    }
+#endif
+
+    NRF_LOG_INST_INFO(p_pool->p_log, "Initialized (size: %u x %u = %u bytes)",
+                      pool_size,
+                      p_pool->block_size,
+                      pool_size * p_pool->block_size);
+
+    p_pool->p_cb->p_stack_pointer = p_pool->p_stack_base;
+    while (pool_size--)
+    {
+        *(p_pool->p_cb->p_stack_pointer)++ = pool_size;
+    }
+
+    p_pool->p_cb->max_utilization = 0;
+
+    return NRF_SUCCESS;
+}
+
+void * nrf_balloc_alloc(nrf_balloc_t const * p_pool)
+{
+    ASSERT(p_pool != NULL);
+
+    void * p_block = NULL;
+
+    CRITICAL_REGION_ENTER();
+
+    if (p_pool->p_cb->p_stack_pointer > p_pool->p_stack_base)
+    {
+        // Allocate block.
+        p_block = nrf_balloc_idx2block(p_pool, *--(p_pool->p_cb->p_stack_pointer));
+
+        // Update utilization statistics.
+        uint8_t utilization = p_pool->p_stack_limit - p_pool->p_cb->p_stack_pointer;
+        if (p_pool->p_cb->max_utilization < utilization)
+        {
+            p_pool->p_cb->max_utilization = utilization;
+        }
+    }
+
+    CRITICAL_REGION_EXIT();
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    if (p_block != NULL)
+    {
+        p_block = nrf_balloc_block_unwrap(p_pool, p_block);
+    }
+#endif
+
+    NRF_LOG_INST_DEBUG(p_pool->p_log, "Allocating element: 0x%08X", p_block);
+
+    return p_block;
+}
+
+void nrf_balloc_free(nrf_balloc_t const * p_pool, void * p_element)
+{
+    ASSERT(p_pool != NULL);
+    ASSERT(p_element != NULL)
+
+    NRF_LOG_INST_DEBUG(p_pool->p_log, "Freeing element: 0x%08X", p_element);
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    void * p_block = nrf_balloc_element_wrap(p_pool, p_element);
+
+    // These checks could be done outside critical region as they use only pool configuration data.
+    if (NRF_BALLOC_DEBUG_BASIC_CHECKS_GET(p_pool->debug_flags))
+    {
+        uint8_t pool_size  = p_pool->p_stack_limit - p_pool->p_stack_base;
+        void *p_memory_end = (uint8_t *)(p_pool->p_memory_begin) + (pool_size * p_pool->block_size);
+
+        // Check if the element belongs to this pool.
+        if ((p_block < p_pool->p_memory_begin) || (p_block >= p_memory_end))
+        {
+            NRF_LOG_INST_ERROR(p_pool->p_log,
+                              "Attempted to free element (0x%08X) that does not belong to the pool.",
+                              p_element);
+            APP_ERROR_CHECK_BOOL(false);
+        }
+
+        // Check if the pointer is valid.
+        if ((((size_t)(p_block) - (size_t)(p_pool->p_memory_begin)) % p_pool->block_size) != 0)
+        {
+            NRF_LOG_INST_ERROR(p_pool->p_log,
+                               "Attempted to free corrupted element address (0x%08X).", p_element);
+            APP_ERROR_CHECK_BOOL(false);
+        }
+    }
+#else
+    void * p_block = p_element;
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+
+    CRITICAL_REGION_ENTER();
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    // These checks have to be done in critical region as they use p_pool->p_stack_pointer.
+    if (NRF_BALLOC_DEBUG_BASIC_CHECKS_GET(p_pool->debug_flags))
+    {
+        // Check for allocated/free ballance.
+        if (p_pool->p_cb->p_stack_pointer >= p_pool->p_stack_limit)
+        {
+            NRF_LOG_INST_ERROR(p_pool->p_log,
+                               "Attempted to free an element (0x%08X) while the pool is full.",
+                               p_element);
+            APP_ERROR_CHECK_BOOL(false);
+        }
+    }
+
+    if (NRF_BALLOC_DEBUG_DOUBLE_FREE_CHECK_GET(p_pool->debug_flags))
+    {
+        // Check for double free.
+        for (uint8_t * p_idx = p_pool->p_stack_base; p_idx < p_pool->p_cb->p_stack_pointer; p_idx++)
+        {
+            if (nrf_balloc_idx2block(p_pool, *p_idx) == p_block)
+            {
+                NRF_LOG_INST_ERROR(p_pool->p_log, "Attempted to double-free an element (0x%08X).",
+                                   p_element);
+                APP_ERROR_CHECK_BOOL(false);
+            }
+        }
+    }
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+
+    // Free the element.
+    *(p_pool->p_cb->p_stack_pointer)++ = nrf_balloc_block2idx(p_pool, p_block);
+
+    CRITICAL_REGION_EXIT();
+}
+
+#endif // NRF_MODULE_ENABLED(NRF_BALLOC)

components/libraries/balloc/nrf_balloc.h

@@ -0,0 +1,351 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+/**
+  * @defgroup nrf_balloc Block memory allocator
+  * @{
+  * @ingroup app_common
+  * @brief This module handles block memory allocator features.
+  */
+
+
+#ifndef NRF_BALLOC_H__
+#define NRF_BALLOC_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "sdk_errors.h"
+#include "sdk_config.h"
+#include "app_util_platform.h"
+#include "app_util.h"
+#include "nrf_log_instance.h"
+#include "nrf_section.h"
+
+/** @brief Name of the module used for logger messaging.
+ */
+#define NRF_BALLOC_LOG_NAME balloc
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED || NRF_BALLOC_CLI_CMDS
+#define NRF_BALLOC_HAS_NAME 1
+#else
+#define NRF_BALLOC_HAS_NAME 0
+#endif
+
+/**@defgroup NRF_BALLOC_DEBUG Macros for preparing debug flags for block allocator module.
+ * @{ */
+#define NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_SET(words)        (((words) & 0xFF) << 0)
+#define NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_GET(flags)        (((flags) >> 0) & 0xFF)
+#define NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_SET(words)        (((words) & 0xFF) << 8)
+#define NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_GET(flags)        (((flags) >> 8) & 0xFF)
+
+#define NRF_BALLOC_DEBUG_BASIC_CHECKS_SET(enable)           (!!(enable) << 16)
+#define NRF_BALLOC_DEBUG_BASIC_CHECKS_GET(flags)            (flags & (1 << 16))
+#define NRF_BALLOC_DEBUG_DOUBLE_FREE_CHECK_SET(enable)      (!!(enable) << 17)
+#define NRF_BALLOC_DEBUG_DOUBLE_FREE_CHECK_GET(flags)       (flags & (1 << 17))
+#define NRF_BALLOC_DEBUG_DATA_TRASHING_CHECK_SET(enable)    (!!(enable) << 18)
+#define NRF_BALLOC_DEBUG_DATA_TRASHING_CHECK_GET(flags)     (flags & (1 << 18))
+/**@} */
+
+/**@brief Default debug flags for @ref nrf_balloc. This is used by the @ref NRF_BALLOC_DEF macro.
+ *        Flags can be changed in @ref sdk_config.
+ */
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    #define NRF_BALLOC_DEFAULT_DEBUG_FLAGS                                                      \
+    (                                                                                           \
+        NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_SET(NRF_BALLOC_CONFIG_HEAD_GUARD_WORDS)           |   \
+        NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_SET(NRF_BALLOC_CONFIG_TAIL_GUARD_WORDS)           |   \
+        NRF_BALLOC_DEBUG_BASIC_CHECKS_SET(NRF_BALLOC_CONFIG_BASIC_CHECKS_ENABLED)           |   \
+        NRF_BALLOC_DEBUG_DOUBLE_FREE_CHECK_SET(NRF_BALLOC_CONFIG_DOUBLE_FREE_CHECK_ENABLED) |   \
+        NRF_BALLOC_DEBUG_DATA_TRASHING_CHECK_SET(NRF_BALLOC_CONFIG_DATA_TRASHING_CHECK_ENABLED) \
+    )
+#else
+    #define NRF_BALLOC_DEFAULT_DEBUG_FLAGS   0
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+
+/**@brief Block memory allocator control block.*/
+typedef struct
+{
+    uint8_t * p_stack_pointer;          //!< Current allocation stack pointer.
+    uint8_t   max_utilization;          //!< Maximum utilization of the memory pool.
+} nrf_balloc_cb_t;
+
+/**@brief Block memory allocator pool instance. The pool is made of elements of the same size. */
+typedef struct
+{
+    nrf_balloc_cb_t * p_cb;             //!< Pointer to the instance control block.
+    uint8_t         * p_stack_base;     //!< Base of the allocation stack.
+                                        /**<
+                                         * Stack is used to store handlers to not allocated elements.
+                                         */
+    uint8_t         * p_stack_limit;    //!< Maximum possible value of the allocation stack pointer.
+    void            * p_memory_begin;   //!< Pointer to the start of the memory pool.
+                                        /**<
+                                         * Memory is used as a heap for blocks.
+                                         */
+    NRF_LOG_INSTANCE_PTR_DECLARE(p_log) //!< Pointer to instance of the logger object (Conditionally compiled).
+#if NRF_BALLOC_HAS_NAME
+    const char      * p_name;           //!< Pointer to string with pool name.
+#endif
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    uint32_t          debug_flags;      //!< Debugging settings.
+                                        /**<
+                                         * Debug flag should be created by @ref NRF_BALLOC_DEBUG.
+                                         */
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    uint16_t          block_size;       //!< Size of the allocated block (including debug overhead).
+                                        /**<
+                                         * Single block contains user element with header and tail
+                                         * words.
+                                         */
+} nrf_balloc_t;
+
+/**@brief Get total memory consumed by single block (element size with overhead caused by debug
+ *        flags).
+ *
+ * @param[in]   _element_size    Size of an element.
+ * @param[in]   _debug_flags     Debug flags.
+ */
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+    #define NRF_BALLOC_BLOCK_SIZE(_element_size, _debug_flags)                      \
+    (                                                                               \
+       (sizeof(uint32_t) * NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_GET(_debug_flags)) +   \
+       ALIGN_NUM(sizeof(uint32_t), (_element_size)) +                               \
+       (sizeof(uint32_t) * NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_GET(_debug_flags))     \
+    )
+#else
+    #define NRF_BALLOC_BLOCK_SIZE(_element_size, _debug_flags)  \
+                ALIGN_NUM(sizeof(uint32_t), (_element_size))
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+
+
+/**@brief Get element size ( excluding debugging overhead is present)
+ *        flags).
+ *
+ * @param[in]   _p_balloc   Pointer to balloc instance.
+ */
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+#define NRF_BALLOC_ELEMENT_SIZE(_p_balloc) \
+           (ALIGN_NUM(sizeof(uint32_t), (_p_balloc)->block_size) -                                 \
+           ((sizeof(uint32_t) * NRF_BALLOC_DEBUG_HEAD_GUARD_WORDS_GET((_p_balloc)->debug_flags)) + \
+           (sizeof(uint32_t) * NRF_BALLOC_DEBUG_TAIL_GUARD_WORDS_GET((_p_balloc)->debug_flags))))
+#else
+#define NRF_BALLOC_ELEMENT_SIZE(_p_balloc) \
+           (_p_balloc)->block_size
+#endif // NRF_BALLOC_CONFIG_DEBUG_ENABLED
+
+#if NRF_BALLOC_CONFIG_DEBUG_ENABLED
+#define __NRF_BALLOC_ASSIGN_DEBUG_FLAGS(_debug_flags)   .debug_flags = (_debug_flags),
+#else
+#define __NRF_BALLOC_ASSIGN_DEBUG_FLAGS(_debug_flags)
+#endif
+
+#if NRF_BALLOC_HAS_NAME
+#define __NRF_BALLOC_ASSIGN_POOL_NAME(_name)            .p_name = STRINGIFY(_name),
+#else
+#define __NRF_BALLOC_ASSIGN_POOL_NAME(_name)
+#endif
+
+
+/**@brief Create a block allocator instance with custom debug flags.
+ *
+ * @note  This macro reserves memory for the given block allocator instance.
+ *
+ * @param[in]   _name           Name of the allocator.
+ * @param[in]   _element_size   Size of one element.
+ * @param[in]   _pool_size      Size of the pool.
+ * @param[in]   _debug_flags    Debug flags (@ref NRF_BALLOC_DEBUG).
+ */
+#define NRF_BALLOC_DBG_DEF(_name, _element_size, _pool_size, _debug_flags)                      \
+    STATIC_ASSERT((_pool_size) <= UINT8_MAX);                                                   \
+    static uint8_t              CONCAT_2(_name, _nrf_balloc_pool_stack)[(_pool_size)];          \
+    static uint32_t             CONCAT_2(_name,_nrf_balloc_pool_mem)                            \
+        [NRF_BALLOC_BLOCK_SIZE(_element_size, _debug_flags) * (_pool_size) / sizeof(uint32_t)]; \
+    static nrf_balloc_cb_t      CONCAT_2(_name,_nrf_balloc_cb);                                 \
+    NRF_LOG_INSTANCE_REGISTER(NRF_BALLOC_LOG_NAME, _name,                                       \
+                              NRF_BALLOC_CONFIG_INFO_COLOR,                                     \
+                              NRF_BALLOC_CONFIG_DEBUG_COLOR,                                    \
+                              NRF_BALLOC_CONFIG_INITIAL_LOG_LEVEL,                              \
+                              NRF_BALLOC_CONFIG_LOG_ENABLED ?                                   \
+                                      NRF_BALLOC_CONFIG_LOG_LEVEL : NRF_LOG_SEVERITY_NONE);     \
+    NRF_SECTION_ITEM_REGISTER(nrf_balloc, const nrf_balloc_t  _name) =                          \
+        {                                                                                       \
+            .p_cb           = &CONCAT_2(_name,_nrf_balloc_cb),                                  \
+            .p_stack_base   = CONCAT_2(_name,_nrf_balloc_pool_stack),                           \
+            .p_stack_limit  = CONCAT_2(_name,_nrf_balloc_pool_stack) + (_pool_size),            \
+            .p_memory_begin = CONCAT_2(_name,_nrf_balloc_pool_mem),                             \
+            .block_size     = NRF_BALLOC_BLOCK_SIZE(_element_size, _debug_flags),               \
+                                                                                                \
+            NRF_LOG_INSTANCE_PTR_INIT(p_log, NRF_BALLOC_LOG_NAME, _name)                        \
+            __NRF_BALLOC_ASSIGN_POOL_NAME(_name)                                                \
+            __NRF_BALLOC_ASSIGN_DEBUG_FLAGS(_debug_flags)                                       \
+        }
+
+/**@brief Create a block allocator instance.
+ *
+ * @note  This macro reserves memory for the given block allocator instance.
+ *
+ * @param[in]   _name           Name of the allocator.
+ * @param[in]   _element_size   Size of one element.
+ * @param[in]   _pool_size      Size of the pool.
+ */
+#define NRF_BALLOC_DEF(_name, _element_size, _pool_size)                                           \
+            NRF_BALLOC_DBG_DEF(_name, _element_size, _pool_size, NRF_BALLOC_DEFAULT_DEBUG_FLAGS)
+
+/**@brief Create a block allocator interface.
+ *
+ * @param[in]   _type    Type which is allocated.
+ * @param[in]   _name    Name of the allocator.
+ */
+#define NRF_BALLOC_INTERFACE_DEC(_type, _name)    \
+    _type * CONCAT_2(_name,_alloc)(void);                  \
+    void    CONCAT_2(_name,_free)(_type * p_element)
+
+/**@brief Define a custom block allocator interface.
+ *
+ * @param[in]   _attr    Function attribute that will be added to allocator function definition.
+ * @param[in]   _type    Type which is allocated.
+ * @param[in]   _name    Name of the allocator.
+ * @param[in]   _p_pool  Pool from which data will be allocated.
+ */
+#define NRF_BALLOC_INTERFACE_CUSTOM_DEF(_attr, _type, _name, _p_pool)           \
+    _attr _type * CONCAT_2(_name,_alloc)(void)                                  \
+    {                                                                           \
+        GCC_PRAGMA("GCC diagnostic push")                                       \
+        GCC_PRAGMA("GCC diagnostic ignored \"-Waddress\"")                      \
+        ASSERT((_p_pool) != NULL);                                              \
+        ASSERT((_p_pool)->block_size >=                                         \
+               NRF_BALLOC_BLOCK_SIZE(sizeof(_type), (_p_pool)->debug_flags));   \
+        GCC_PRAGMA("GCC diagnostic pop")                                        \
+        return (_type *)(nrf_balloc_alloc(_p_pool));                            \
+    }                                                                           \
+                                                                                \
+    _attr void CONCAT_2(_name,_free)(_type * p_element)                         \
+    {                                                                           \
+        GCC_PRAGMA("GCC diagnostic push")                                       \
+        GCC_PRAGMA("GCC diagnostic ignored \"-Waddress\"")                      \
+        ASSERT((_p_pool) != NULL);                                              \
+        ASSERT((_p_pool)->block_size >=                                         \
+               NRF_BALLOC_BLOCK_SIZE(sizeof(_type), (_p_pool)->debug_flags));   \
+        GCC_PRAGMA("GCC diagnostic pop")                                        \
+        nrf_balloc_free((_p_pool), p_element);                                  \
+    }
+
+/**@brief Define block allocator interface.
+ *
+ * @param[in]   _type    Type which is allocated.
+ * @param[in]   _name    Name of the allocator.
+ * @param[in]   _p_pool  Pool from which data will be allocated.
+ */
+#define NRF_BALLOC_INTERFACE_DEF(_type, _name, _p_pool)        \
+        NRF_BALLOC_INTERFACE_CUSTOM_DEF(/* empty */, _type, _name, _p_pool)
+
+/**@brief Define a local block allocator interface.
+ *
+ * @param[in]   _type    Type which is allocated.
+ * @param[in]   _name    Name of the allocator.
+ * @param[in]   _p_pool  Pool from which data will be allocated.
+ */
+#define NRF_BALLOC_INTERFACE_LOCAL_DEF(_type, _name, _p_pool)  \
+        NRF_BALLOC_INTERFACE_CUSTOM_DEF(static, _type, _name, _p_pool)
+
+/**@brief Function for initializing a block memory allocator pool.
+ *
+ * @param[out]  p_pool          Pointer to the pool that is to be initialized.
+ *
+ * @return  NRF_SUCCESS on success, otherwise error code.
+ */
+ret_code_t nrf_balloc_init(nrf_balloc_t const * p_pool);
+
+/**@brief Function for allocating an element from the pool.
+ *
+ * @note    This module guarantees that the returned memory is aligned to 4.
+ *
+ * @param[in]   p_pool  Pointer to the memory pool from which the element will be allocated.
+ *
+ * @return      Allocated element or NULL if the specified pool is empty.
+ */
+void * nrf_balloc_alloc(nrf_balloc_t const * p_pool);
+
+/**@brief Function for freeing an element back to the pool.
+ *
+ * @param[in]   p_pool      Pointer to the memory pool.
+ * @param[in]   p_element   Element to be freed.
+ */
+void nrf_balloc_free(nrf_balloc_t const * p_pool, void * p_element);
+
+/**@brief Function for getting maximum memory pool utilization.
+ *
+ * @param[in]   p_pool Pointer to the memory pool instance.
+ *
+ * @return Maximum number of elements allocated from the pool.
+ */
+__STATIC_INLINE uint8_t nrf_balloc_max_utilization_get(nrf_balloc_t const * p_pool);
+
+#ifndef SUPPRESS_INLINE_IMPLEMENTATION
+__STATIC_INLINE uint8_t nrf_balloc_max_utilization_get(nrf_balloc_t const * p_pool)
+{
+    ASSERT(p_pool != NULL);
+    return p_pool->p_cb->max_utilization;
+}
+#endif //SUPPRESS_INLINE_IMPLEMENTATION
+
+/**@brief Function for getting current memory pool utilization.
+ *
+ * @param[in]   p_pool Pointer to the memory pool instance.
+ *
+ * @return Maximum number of elements allocated from the pool.
+ */
+__STATIC_INLINE uint8_t nrf_balloc_utilization_get(nrf_balloc_t const * p_pool);
+
+#ifndef SUPPRESS_INLINE_IMPLEMENTATION
+__STATIC_INLINE uint8_t nrf_balloc_utilization_get(nrf_balloc_t const * p_pool)
+{
+    ASSERT(p_pool != NULL);
+    return (p_pool->p_stack_limit - p_pool->p_cb->p_stack_pointer);
+}
+#endif //SUPPRESS_INLINE_IMPLEMENTATION
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_BALLOC_H__
+/** @} */

components/libraries/bootloader/ant_dfu/nrf_dfu_ant.c

@@ -0,0 +1,772 @@
+/**
+ * This software is subject to the ANT+ Shared Source License
+ * www.thisisant.com/swlicenses
+ * Copyright (c) Garmin Canada Inc. 2018
+ * 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 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 Garmin nor the names of its
+ *       contributors may be used to endorse or promote products
+ *       derived from this software without specific prior
+ *       written permission.
+ *
+ * The following actions are prohibited:
+ *
+ *    1) Redistribution of source code containing the ANT+ Network
+ *       Key. The ANT+ Network Key is available to ANT+ Adopters.
+ *       Please refer to http://thisisant.com to become an ANT+
+ *       Adopter and access the key. 
+ *
+ *    2) Reverse engineering, decompilation, and/or disassembly of
+ *       software provided in binary form under this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE HEREBY
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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; DAMAGE TO ANY DEVICE, 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. SOME STATES DO NOT ALLOW 
+ * THE EXCLUSION OF INCIDENTAL OR CONSEQUENTIAL DAMAGES, SO THE
+ * ABOVE LIMITATIONS MAY NOT APPLY TO YOU.
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "sdk_common.h"
+
+#include "ant_channel_config.h"
+#include "ant_interface.h"
+#include "ant_parameters.h"
+#include "nrf_assert.h"
+#include "nrf_balloc.h"
+#include "nrf_bootloader_info.h"
+#include "nrf_dfu_handling_error.h"
+#include "nrf_dfu_req_handler.h"
+#include "nrf_dfu_transport.h"
+#include "nrf_dfu_mbr.h"
+#include "nrf_sdh.h"
+#include "nrf_sdh_ant.h"
+#include "nrf_soc.h"
+
+#define NRF_LOG_MODULE_NAME nrf_dfu_ant
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+/**@file
+ *
+ * @defgroup nrf_dfu_ant ANT transport for reference DFU.
+ * @ingroup  nrf_dfu
+ * @brief    Device Firmware Update (DFU) transport layer using ANT.
+ *
+ * Transport documentation:
+ *
+ * The ANT transport uses all of the same opcodes and payload formats as the
+ * UART serial transport. The only differences are the packet header format and
+ * some extra details to deal with retransmissions.
+ *
+ * The device receiving the update is the ANT master. The format of the
+ * broadcast buffer is as follows;
+ *      Byte 0: Current Slave -> Master sequence number.
+ *      Byte 1: Current Master -> Slave sequence number.
+ *      Bytes 2-7: Reserved, set to 0.
+ *
+ * The sequence numbers are used to detect retransmissions, any messages sent
+ * with a sequence number equivalent to the current sequence will be ignored.
+ *
+ * When the slave first connects to the master it should inspect the broadcast
+ * data in order to synchronize its sequence counters.
+ *
+ * All commands/responses are padded out to the nearest 8-byte boundary after
+ * framing, and then sent using either a burst or acknowledged data depending on
+ * length (ack data is used for 8-byte messages). The message transmission is
+ * retried until an EVENT_TRANSFER_TX_COMPLETE event is received.
+ * All messages are framed using the following format:
+ *      Bytes 0-1: Message length before padding, little endian, includes header.
+ *      Byte 2: Sequence number. Increment for every new message.
+ *      Byte 3: Op code. Always 0x60 for responses.
+ *      Bytes 4-N: Command/Response payload. This follows the same format as the
+ *                 UART serial transport, without any SLIP encoding.
+ *
+ * As a final note, the MTU for this protocol is the maximum size of a burst
+ * that can be received.
+ */
+
+/** Packet header is always 2 byte length + seq num + op code */
+#define PKT_HEADER_SIZE 4
+
+/** Maximum size of the payload in a write command. */
+#define MAX_WRITE_PAYLOAD (NRF_DFU_ANT_MTU - PKT_HEADER_SIZE)
+
+/** Bursts are always a multiple of the standard data size. */
+STATIC_ASSERT_MSG(
+    ALIGN_NUM(ANT_STANDARD_DATA_PAYLOAD_SIZE, NRF_DFU_ANT_MTU) == NRF_DFU_ANT_MTU,
+    "ANT MTU must be a multiple of " STRINGIFY(ANT_STANDARD_DATA_PAYLOAD_SIZE));
+
+/** Number of buffers to reserve space for with balloc. */
+#if (NRF_DFU_ANT_BUFFERS_OVERRIDE)
+#define NUM_BUFFERS NRF_DFU_ANT_BUFFERS
+#else
+#define NUM_BUFFERS CEIL_DIV(CODE_PAGE_SIZE, MAX_WRITE_PAYLOAD)
+#endif
+
+static uint32_t ant_dfu_init(nrf_dfu_observer_t observer);
+static uint32_t ant_dfu_close(nrf_dfu_transport_t const * p_exception);
+
+static ant_channel_config_t m_channel_config = {
+    .channel_number = 0,
+    .channel_type = CHANNEL_TYPE_MASTER,
+    .rf_freq = NRF_DFU_ANT_RF_FREQ,
+    .transmission_type = 1, // Non-shared, no global pages.
+    .device_type = NRF_DFU_ANT_DEV_TYPE,
+    .channel_period = NRF_DFU_ANT_CHANNEL_PERIOD,
+};
+
+static nrf_dfu_observer_t m_observer;
+/** Has transport been initialized by DFU core */
+static bool m_initialized = false;
+/** Has the channel started broadcasting */
+static bool m_started = false;
+/** Has some data been received on the transport. */
+static bool m_active = false;
+
+/** State tracking for rx transfers. */
+static struct
+{
+    /** Buffer for holding the command. */
+    uint8_t * buff;
+    /** Amount of data written */
+    size_t offset;
+    /** Sequence of last processed command. */
+    uint8_t seq;
+} m_rx;
+
+/** State tracking for tx transfers. */
+static struct
+{
+    /** Raw data to send. */
+    uint8_t resp[ALIGN_NUM(ANT_STANDARD_DATA_PAYLOAD_SIZE,
+        PKT_HEADER_SIZE + sizeof(nrf_dfu_response_t))];
+    /** Length of data to send. 0 Indicates no response queued. */
+    size_t len;
+    /** Sequence number of last queued response. */
+    uint8_t seq;
+    /**
+     * Used as burst flag for softdevice, allows to busy loop until all data is
+     * accepted by softdevice.
+     */
+    volatile bool buffering;
+    /**
+     * Indicate that a new response was generated before the last one was
+     * confirmed.
+     */
+    bool response_overwritten;
+    /** Data buffer used for broadcast messages. */
+    uint8_t bcast_data[ANT_STANDARD_DATA_PAYLOAD_SIZE];
+} m_tx;
+
+/** State tracking for progress notifications. */
+static struct
+{
+    /** Requested PRN */
+    uint16_t limit;
+    /** How many more write commands until a CRC should be sent back. */
+    uint16_t remaining;
+} m_pkt_notify;
+
+DFU_TRANSPORT_REGISTER(nrf_dfu_transport_t const ant_dfu_transport) = {
+    .init_func = ant_dfu_init,
+    .close_func = ant_dfu_close,
+};
+
+NRF_BALLOC_DEF(m_buffer_pool, NRF_DFU_ANT_MTU, NUM_BUFFERS);
+
+static void release_rx_buff(void)
+{
+    if (m_rx.buff != NULL)
+    {
+        nrf_balloc_free(&m_buffer_pool, m_rx.buff);
+        m_rx.buff = NULL;
+    }
+}
+
+static void transmit_response(void)
+{
+    uint32_t err_code = NRF_SUCCESS;
+
+    size_t full_len = ALIGN_NUM(ANT_STANDARD_DATA_PAYLOAD_SIZE, m_tx.len);
+
+    // Pad out with 0's.
+    memset(&m_tx.resp[m_tx.len], 0, full_len - m_tx.len);
+
+    if (full_len > ANT_STANDARD_DATA_PAYLOAD_SIZE)
+    {
+        err_code = sd_ant_burst_handler_request(
+            m_channel_config.channel_number,
+            full_len, m_tx.resp,
+            BURST_SEGMENT_START | BURST_SEGMENT_END);
+    } else
+    {
+        err_code = sd_ant_acknowledge_message_tx(
+            m_channel_config.channel_number,
+            full_len, m_tx.resp);
+    }
+
+    // Wait for buffer to be consumed.
+    // TODO: wait flag management needs to be improved if this will coexist with
+    // other channels.
+    while (err_code == NRF_SUCCESS && m_tx.buffering)
+    {
+        err_code = sd_app_evt_wait();
+    }
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Sending response failed with error %d", err_code);
+    }
+}
+
+static void update_bcast_data(void)
+{
+    memset(m_tx.bcast_data, 0, sizeof(m_tx.bcast_data));
+    m_tx.bcast_data[0] = m_rx.seq;
+    m_tx.bcast_data[1] = m_tx.seq;
+
+    if (NRF_SUCCESS != sd_ant_broadcast_message_tx(
+        m_channel_config.channel_number,
+        sizeof(m_tx.bcast_data), m_tx.bcast_data))
+    {
+        NRF_LOG_WARNING("Unable to update broadcast data.");
+    }
+}
+
+static void handle_write_complete(void * p_buf)
+{
+    nrf_balloc_free(&m_buffer_pool, p_buf);
+}
+
+static void prepare_response(nrf_dfu_response_t * p_res)
+{
+    if (m_tx.len)
+    {
+        NRF_LOG_WARNING("Overwriting previous response.");
+        m_tx.response_overwritten = true;
+    }
+
+    // reserve first 2 bytes for length.
+    m_tx.len = 2;
+
+    m_tx.resp[m_tx.len++] = ++(m_tx.seq);
+    m_tx.resp[m_tx.len++] = NRF_DFU_OP_RESPONSE;
+    m_tx.resp[m_tx.len++] = p_res->request;
+    m_tx.resp[m_tx.len++] = p_res->result;
+
+    if (p_res->result == NRF_DFU_RES_CODE_SUCCESS)
+    {
+        switch(p_res->request)
+        {
+            case NRF_DFU_OP_PROTOCOL_VERSION:
+            {
+                m_tx.resp[m_tx.len++] = p_res->protocol.version;
+            } break;
+
+            case NRF_DFU_OP_CRC_GET:
+            {
+                m_tx.len += uint32_encode(
+                    p_res->crc.offset, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->crc.crc, &m_tx.resp[m_tx.len]);
+            } break;
+
+            case NRF_DFU_OP_OBJECT_SELECT:
+            {
+                m_tx.len += uint32_encode(
+                    p_res->select.max_size, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->select.offset, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->select.crc, &m_tx.resp[m_tx.len]);
+            } break;
+
+            case NRF_DFU_OP_MTU_GET:
+            {
+                m_tx.len += uint16_encode(
+                    p_res->mtu.size, &m_tx.resp[m_tx.len]);
+            } break;
+
+            case NRF_DFU_OP_PING:
+            {
+                m_tx.resp[m_tx.len++] = p_res->ping.id;
+            } break;
+
+            case NRF_DFU_OP_HARDWARE_VERSION:
+            {
+                m_tx.len += uint32_encode(
+                    p_res->hardware.part, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->hardware.variant, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->hardware.memory.rom_size, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->hardware.memory.rom_page_size, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->hardware.memory.ram_size, &m_tx.resp[m_tx.len]);
+            } break;
+
+            case NRF_DFU_OP_FIRMWARE_VERSION:
+            {
+                m_tx.resp[m_tx.len++] = p_res->firmware.type;
+                m_tx.len += uint32_encode(
+                    p_res->firmware.version, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->firmware.addr, &m_tx.resp[m_tx.len]);
+                m_tx.len += uint32_encode(
+                    p_res->firmware.len, &m_tx.resp[m_tx.len]);
+            } break;
+
+            default:
+                break;
+        }
+    }
+    else if (p_res->result == NRF_DFU_RES_CODE_EXT_ERROR)
+    {
+        m_tx.resp[m_tx.len++] = ext_error_get();
+        UNUSED_RETURN_VALUE(ext_error_set(NRF_DFU_EXT_ERROR_NO_ERROR));
+    }
+
+    // Finally fill in the length.
+    UNUSED_RETURN_VALUE(uint16_encode(m_tx.len, m_tx.resp));
+
+    // Safety check buffer overflow.
+    ASSERT(m_tx.len <= sizeof(m_tx.resp));
+
+    if (!m_tx.response_overwritten)
+    {
+        // Can send out the response immediately if there wasn't a previous one
+        // queued.
+        transmit_response();
+    }
+}
+
+static void handle_response(nrf_dfu_response_t * p_res, void * p_context)
+{
+    UNUSED_PARAMETER(p_context);
+
+    if (p_res->result != NRF_DFU_RES_CODE_SUCCESS)
+    {
+        NRF_LOG_WARNING("Operation %d had result %d",
+            p_res->request, p_res->result);
+    }
+
+    if (p_res->request == NRF_DFU_OP_OBJECT_WRITE)
+    {
+        if (m_pkt_notify.limit == 0 ||
+            --m_pkt_notify.remaining != 0)
+        {
+            // No packet notification needed, filter out response.
+            return;
+        }
+
+        // Packet Notification time, send a CRC response.
+        m_pkt_notify.remaining = m_pkt_notify.limit;
+        p_res->request = NRF_DFU_OP_CRC_GET;
+        uint32_t offset = p_res->write.offset;
+        uint32_t crc = p_res->write.crc;
+        p_res->crc.offset = offset;
+        p_res->crc.crc = crc;
+    }
+
+    prepare_response(p_res);
+}
+
+static uint32_t handle_request(void)
+{
+    uint16_t len = uint16_decode(m_rx.buff);
+    uint16_t offset = sizeof(uint16_t);
+
+    if (len < PKT_HEADER_SIZE || len > m_rx.offset)
+    {
+        NRF_LOG_WARNING("Ignoring command with invalid length.");
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    uint8_t seq = m_rx.buff[offset++];
+    if (!m_active)
+    {
+        m_active = true;
+        // Close all other transports.
+        UNUSED_RETURN_VALUE(nrf_dfu_transports_close(&ant_dfu_transport));
+    }
+    else if (seq == m_rx.seq)
+    {
+        NRF_LOG_DEBUG("Ignoring repeated command");
+        return NRF_SUCCESS;
+    }
+
+    m_rx.seq = seq;
+
+    nrf_dfu_request_t request = {
+        .request = (nrf_dfu_op_t)m_rx.buff[offset++],
+        .callback.response = handle_response,
+    };
+
+    switch(request.request)
+    {
+        case NRF_DFU_OP_OBJECT_CREATE:
+        {
+            request.create.object_type = m_rx.buff[offset++];
+            request.create.object_size = uint32_decode(&m_rx.buff[offset]);
+            offset += sizeof(uint32_t);
+        } break;
+
+        case NRF_DFU_OP_RECEIPT_NOTIF_SET:
+        {
+            request.prn.target = uint16_decode(&m_rx.buff[offset]);
+            offset += sizeof(uint16_t);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_SELECT:
+        {
+            request.select.object_type = m_rx.buff[offset++];
+        } break;
+
+        case NRF_DFU_OP_OBJECT_WRITE:
+        {
+            request.write.p_data = &m_rx.buff[offset];
+            request.write.len = len - offset;
+            offset = len;
+        } break;
+
+        case NRF_DFU_OP_PING:
+        {
+            request.ping.id = m_rx.buff[offset++];
+        } break;
+
+        case NRF_DFU_OP_FIRMWARE_VERSION:
+        {
+            request.firmware.image_number = m_rx.buff[offset++];
+        } break;
+
+        case NRF_DFU_OP_MTU_GET:
+        {
+            NRF_LOG_DEBUG("ANT DFU: Responding to MTU request with %d",
+                NRF_DFU_ANT_MTU);
+            request.mtu.size = NRF_DFU_ANT_MTU;
+        } break;
+
+        default:
+            // Do nothing.
+            break;
+    }
+
+    if (offset > len)
+    {
+        NRF_LOG_WARNING("Ignoring command with invalid length");
+        return NRF_ERROR_DATA_SIZE;
+    }
+
+    // Some processing that is only safe to do if accepting the command.
+
+    switch (request.request)
+    {
+        case NRF_DFU_OP_RECEIPT_NOTIF_SET:
+        {
+            m_pkt_notify.limit = request.prn.target;
+            m_pkt_notify.remaining = m_pkt_notify.limit;
+        } break;
+
+        case NRF_DFU_OP_OBJECT_CREATE:
+        case NRF_DFU_OP_OBJECT_SELECT:
+        {
+            m_pkt_notify.remaining = m_pkt_notify.limit;
+        } break;
+
+        case NRF_DFU_OP_OBJECT_WRITE:
+        {
+            // Ownership of buffer is transferred to the write command.
+            request.callback.write = handle_write_complete;
+            m_rx.buff = NULL;
+        } break;
+
+        default:
+            break;
+    }
+
+    return nrf_dfu_req_handler_on_req(&request);
+}
+
+static void handle_tx_transfer_complete(bool success)
+{
+    if (m_tx.response_overwritten)
+    {
+        // By treating the result as a failure the retransmission will send out
+        // the new response.
+        success = false;
+        m_tx.response_overwritten = false;
+    }
+
+    if (success)
+    {
+        m_tx.len = 0;
+        update_bcast_data();
+    }
+    else
+    {
+        transmit_response();
+    }
+}
+
+static void handle_rx_transfer_start()
+{
+    if (m_rx.buff == NULL)
+    {
+        m_rx.buff = nrf_balloc_alloc(&m_buffer_pool);
+        if (m_rx.buff != NULL)
+        {
+            NRF_LOG_INFO("Allocated buffer %x", m_rx.buff);
+        }
+        else
+        {
+            NRF_LOG_ERROR("Unable to allocate buffer for incoming packet.");
+            return;
+        }
+    }
+
+    NRF_LOG_DEBUG("Resetting rx pointer.");
+    m_rx.offset = 0;
+}
+
+static void handle_rx_transfer_complete(bool success)
+{
+    if (success)
+    {
+        uint32_t err_code = handle_request();
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_ERROR("Error %d handling request.", err_code);
+        }
+    }
+
+    release_rx_buff();
+}
+
+static void handle_rx_transfer_data(uint8_t * data, size_t len)
+{
+    if (m_rx.buff == NULL)
+    {
+        NRF_LOG_DEBUG("Ignoring transfer data.");
+        return;
+    }
+
+    if (m_rx.offset + len > NRF_DFU_ANT_MTU)
+    {
+        NRF_LOG_ERROR("Received packet overflows MTU.");
+        handle_rx_transfer_complete(false);
+        return;
+    }
+
+    memcpy(&m_rx.buff[m_rx.offset], data, len);
+    m_rx.offset += len;
+}
+
+static void handle_data_mesg(ANT_MESSAGE * p_msg)
+{
+    bool is_first = false;
+    bool is_last = false;
+    uint8_t len = ANT_STANDARD_DATA_PAYLOAD_SIZE;
+
+    switch(p_msg->ANT_MESSAGE_ucMesgID)
+    {
+        case MESG_BROADCAST_DATA_ID:
+        {
+            // Broadcast data is ignored.
+            len = 0;
+        } break;
+
+        case MESG_ACKNOWLEDGED_DATA_ID:
+        {
+            is_first = true;
+            is_last = true;
+        } break;
+
+        case MESG_ADV_BURST_DATA_ID:
+        {
+            len = p_msg->ANT_MESSAGE_ucSize - MESG_CHANNEL_NUM_SIZE;
+        } // FALL-THROUGH : both burst types act the same other than len.
+        case MESG_BURST_DATA_ID:
+        {
+            uint8_t seq = p_msg->ANT_MESSAGE_ucChannel & SEQUENCE_NUMBER_MASK;
+            is_first = seq == SEQUENCE_FIRST_MESSAGE;
+            is_last = !!(seq & SEQUENCE_LAST_MESSAGE);
+        } break;
+    }
+
+    if (len != 0)
+    {
+        if (is_first)
+        {
+            handle_rx_transfer_start();
+        }
+
+        handle_rx_transfer_data(p_msg->ANT_MESSAGE_aucPayload, len);
+
+        if (is_last)
+        {
+            handle_rx_transfer_complete(true);
+        }
+    }
+}
+
+static void ant_dfu_evt_handler(ant_evt_t * p_ant_evt, void * p_context)
+{
+    // Ignore messages meant for other channels.
+    if (p_ant_evt->channel != m_channel_config.channel_number)
+    {
+        return;
+    }
+
+    switch(p_ant_evt->event)
+    {
+        case EVENT_TX:
+        {
+            if (!m_started)
+            {
+                m_started = true;
+                m_observer(NRF_DFU_EVT_TRANSPORT_ACTIVATED);
+            }
+        } break;
+
+        case EVENT_TRANSFER_TX_COMPLETED:
+            handle_tx_transfer_complete(true);
+            break;
+        case EVENT_TRANSFER_TX_FAILED:
+            handle_tx_transfer_complete(false);
+            break;
+
+        case EVENT_RX:
+            handle_data_mesg(&p_ant_evt->message);
+            break;
+
+        case EVENT_TRANSFER_RX_FAILED:
+            handle_rx_transfer_complete(false);
+            break;
+    }
+}
+
+static uint32_t ant_dfu_init(nrf_dfu_observer_t observer)
+{
+    uint32_t err_code = NRF_SUCCESS;
+    if (m_initialized)
+    {
+        return err_code;
+    }
+
+    NRF_SDH_ANT_OBSERVER(ant_dfu_observer, NRF_DFU_ANT_EVT_HANDLER_PRIO,
+        ant_dfu_evt_handler, NULL);
+
+    m_observer = observer;
+    m_tx.seq = m_rx.seq = 0;
+    m_active = false;
+    m_started = false;
+
+    NRF_LOG_DEBUG("Initializing ANT DFU transport");
+
+    err_code = nrf_balloc_init(&m_buffer_pool);
+    VERIFY_SUCCESS(err_code);
+
+    err_code = nrf_dfu_mbr_init_sd();
+    VERIFY_SUCCESS(err_code);
+
+    NRF_LOG_DEBUG("Setting up vector table: 0x%08x", BOOTLOADER_START_ADDR);
+    err_code = sd_softdevice_vector_table_base_set(BOOTLOADER_START_ADDR);
+    VERIFY_SUCCESS(err_code);
+
+    NRF_LOG_DEBUG("Enabling softdevice");
+    err_code = nrf_sdh_enable_request();
+    VERIFY_SUCCESS(err_code);
+
+    err_code = nrf_sdh_ant_enable();
+    VERIFY_SUCCESS(err_code);
+
+    static uint8_t adv_burst_conf[] = {
+        ADV_BURST_MODE_ENABLE,
+        ADV_BURST_MODES_SIZE_24_BYTES,
+        0, // No required modes.
+        0, 0, // Reserved
+        ADV_BURST_MODES_FREQ_HOP, // Optional Modes
+        0, 0, // Reserved
+        // No optional configs.
+    };
+    err_code = sd_ant_adv_burst_config_set(adv_burst_conf, sizeof(adv_burst_conf));
+    VERIFY_SUCCESS(err_code);
+
+    m_channel_config.device_number = NRF_FICR->DEVICEID[0];
+    m_channel_config.transmission_type |= (NRF_FICR->DEVICEID[1] & 0xF) << 4;
+
+    err_code = ant_channel_init(&m_channel_config);
+    VERIFY_SUCCESS(err_code);
+
+    update_bcast_data();
+
+    err_code = sd_ant_channel_open(m_channel_config.channel_number);
+    VERIFY_SUCCESS(err_code);
+
+    NRF_LOG_DEBUG("ANT transport intialized");
+
+    m_initialized = true;
+    return err_code;
+}
+
+static uint32_t ant_dfu_close(nrf_dfu_transport_t const * p_exception)
+{
+    uint32_t err_code = NRF_SUCCESS;
+
+    if (p_exception != &ant_dfu_transport && m_initialized)
+    {
+        NRF_LOG_DEBUG("Shutting down ANT DFU transport");
+
+        m_initialized = false;
+
+        err_code = sd_ant_channel_close(m_channel_config.channel_number);
+        VERIFY_SUCCESS(err_code);
+
+        uint8_t status;
+        do
+        {
+            // The initial wait is safe because the close command above would
+            // have generated at least 1 app event.
+            err_code = sd_app_evt_wait();
+            VERIFY_SUCCESS(err_code);
+            err_code = sd_ant_channel_status_get(
+                m_channel_config.channel_number, &status);
+            VERIFY_SUCCESS(err_code);
+        } while ((status & STATUS_CHANNEL_STATE_MASK) != STATUS_ASSIGNED_CHANNEL);
+
+        err_code = nrf_sdh_disable_request();
+        VERIFY_SUCCESS(err_code);
+
+        NRF_LOG_DEBUG("ANT transport disabled.");
+    }
+
+    return err_code;
+}

components/libraries/bootloader/ble_dfu/nrf_dfu_ble.h

@@ -0,0 +1,121 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_dfu_ble DFU BLE Service
+ * @{
+ * @ingroup  nrf_dfu
+ * @brief    Device Firmware Update (DFU) transport layer for <em>Bluetooth</em> low energy.
+ *
+ * @details  The Device Firmware Update (DFU) Service is a GATT-based service that can be used for
+ *           performing firmware updates over BLE. Note that this implementation uses
+ *           vendor-specific UUIDs for the service and characteristics, and is intended to demonstrate
+ *           firmware updates over BLE. See @ref lib_dfu_transport_ble "DFU Transport: BLE" for more information on the service and the profile.
+ */
+
+#ifndef NRF_DFU_BLE_H__
+#define NRF_DFU_BLE_H__
+
+#include <stdint.h>
+#include "ble_gatts.h"
+#include "ble.h"
+#include "nrf_dfu_transport.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// This is a 16-bit UUID.
+#define BLE_DFU_SERVICE_UUID                 0xFE59                       //!< UUID of the DFU Service.
+
+// These UUIDs are used with the Nordic base address to create a 128-bit UUID (0x8EC9XXXXF3154F609FB8838830DAEA50).
+#define BLE_DFU_CTRL_PT_UUID                 0x0001                       //!< UUID of the DFU Control Point.
+#define BLE_DFU_PKT_CHAR_UUID                0x0002                       //!< UUID of the DFU Packet Characteristic.
+
+
+/**@brief   DFU Service.
+ *
+ * @details This structure contains status information related to the service.
+ */
+typedef struct
+{
+    uint16_t                     service_handle;                        /**< Handle of the DFU Service (as provided by the SoftDevice). */
+    uint8_t                      uuid_type;                             /**< UUID type assigned to the DFU Service by the SoftDevice. */
+    ble_gatts_char_handles_t     dfu_pkt_handles;                       /**< Handles related to the DFU Packet Characteristic. */
+    ble_gatts_char_handles_t     dfu_ctrl_pt_handles;                   /**< Handles related to the DFU Control Point Characteristic. */
+} ble_dfu_t;
+
+
+/**@brief Function for initializing the BLE transport.
+ *
+ * @param[in] observer  Callback function for receiving notifications from the BLE transport.
+ *
+ * @retval NRF_SUCCESS  If successful.
+ * @return Error code from sub-call on error.
+ */
+uint32_t ble_dfu_transport_init(nrf_dfu_observer_t observer);
+
+/**@brief Function for closing the BLE transport.
+ *
+ * This function disconnects and disables the SoftDevice.
+ *
+ * @param[in] p_exception  Optional exception. If the exception refers to this transport,
+ *                         this function will do nothing. Can be NULL to signify no exception.
+ *
+ * @retval NRF_SUCCESS  If successful.
+ * @return Error code from sub-call on error.
+ */
+uint32_t ble_dfu_transport_close(nrf_dfu_transport_t const * p_exception);
+
+/**@brief Function for disconnecting from the BLE peer and starting advertising.
+ *
+ * @retval NRF_SUCCESS  If successful.
+ * @return Error code from sub-call on error.
+ */
+uint32_t ble_dfu_transport_disconnect(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_DFU_BLE_H__
+
+/** @} */

components/libraries/bootloader/ble_dfu/nrf_dfu_ble_svci_bond_sharing.h

@@ -0,0 +1,95 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_dfu_svci_bond_sharing Supervisor call interface for bond sharing
+ * @{
+ * @ingroup  nrf_dfu
+ * @brief The Supervisor call interface is a thread-safe method to call into the current application or into an external application using a Supervisor instruction.
+ *
+ */
+
+
+#ifndef NRF_DFU_BLE_SVCI_BOND_SHARING_H__
+#define NRF_DFU_BLE_SVCI_BOND_SHARING_H__
+
+#include <stdbool.h>
+#include "nrf_svci.h"
+#include "nrf_svci_async_function.h"
+#include "sdk_config.h"
+#include "nrf_dfu_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define NRF_DFU_SVCI_SET_PEER_DATA        2
+#define NRF_DFU_SVCI_SET_ADV_NAME         3
+
+#if defined(NRF_DFU_TRANSPORT_BLE) && NRF_DFU_TRANSPORT_BLE
+
+/**@brief Sets up the async SVCI interface for exchanging peer data like bonding and the system attribute table.
+ *
+ * @details The peer data will be stored in flash by the bootloader. This requires memory management and
+ *          handling forwarding of system events and state from the main application to the bootloader.
+ *
+ * @note    This is only available in the buttonless DFU that supports bond sharing.
+ */
+NRF_SVCI_ASYNC_FUNC_DECLARE(NRF_DFU_SVCI_SET_PEER_DATA, nrf_dfu_set_peer_data, nrf_dfu_peer_data_t, nrf_dfu_peer_data_state_t);
+
+/**@brief Sets up the async SVCI interface for exchanging advertisement name to use when entering DFU mode.
+ *
+ * @details The advertisement name will be stored in flash by the bootloader. This requires memory management
+ *          and handling forwarding of system events and state from the main application to the bootloader.
+ *
+ * @note    This is only available in the buttonless DFU that does not support bond sharing.
+ */
+NRF_SVCI_ASYNC_FUNC_DECLARE(NRF_DFU_SVCI_SET_ADV_NAME, nrf_dfu_set_adv_name, nrf_dfu_adv_name_t, nrf_dfu_set_adv_name_state_t);
+
+#endif // NRF_DFU_TRANSPORT_BLE
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_DFU_BLE_SVCI_BOND_SHARING_H__
+
+/** @} */

components/libraries/bootloader/dfu/dfu-cc.options

@@ -0,0 +1,5 @@
+dfu.Hash.hash                       max_size:32
+dfu.SignedCommand.signature         max_size:64
+dfu.InitCommand.sd_req              max_count:16
+dfu.InitCommand.boot_validation     max_count:3
+dfu.BootValidation.bytes            max_size:64

components/libraries/bootloader/dfu/dfu-cc.pb.c

@@ -0,0 +1,123 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+/* Automatically generated nanopb constant definitions */
+/* Generated by nanopb-0.3.6-dev at Tue Sep 11 14:37:18 2018. */
+
+#include "dfu-cc.pb.h"
+
+/* @@protoc_insertion_point(includes) */
+#if PB_PROTO_HEADER_VERSION != 30
+#error Regenerate this file with the current version of nanopb generator.
+#endif
+
+const bool dfu_init_command_is_debug_default = false;
+
+
+const pb_field_t dfu_hash_fields[3] = {
+    PB_FIELD(  1, UENUM   , REQUIRED, STATIC  , FIRST, dfu_hash_t, hash_type, hash_type, 0),
+    PB_FIELD(  2, BYTES   , REQUIRED, STATIC  , OTHER, dfu_hash_t, hash, hash_type, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t dfu_boot_validation_fields[3] = {
+    PB_FIELD(  1, UENUM   , REQUIRED, STATIC  , FIRST, dfu_boot_validation_t, type, type, 0),
+    PB_FIELD(  2, BYTES   , REQUIRED, STATIC  , OTHER, dfu_boot_validation_t, bytes, type, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t dfu_init_command_fields[11] = {
+    PB_FIELD(  1, UINT32  , OPTIONAL, STATIC  , FIRST, dfu_init_command_t, fw_version, fw_version, 0),
+    PB_FIELD(  2, UINT32  , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, hw_version, fw_version, 0),
+    PB_FIELD(  3, UINT32  , REPEATED, STATIC  , OTHER, dfu_init_command_t, sd_req, hw_version, 0),
+    PB_FIELD(  4, UENUM   , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, type, sd_req, 0),
+    PB_FIELD(  5, UINT32  , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, sd_size, type, 0),
+    PB_FIELD(  6, UINT32  , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, bl_size, sd_size, 0),
+    PB_FIELD(  7, UINT32  , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, app_size, bl_size, 0),
+    PB_FIELD(  8, MESSAGE , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, hash, app_size, &dfu_hash_fields),
+    PB_FIELD(  9, BOOL    , OPTIONAL, STATIC  , OTHER, dfu_init_command_t, is_debug, hash, &dfu_init_command_is_debug_default),
+    PB_FIELD( 10, MESSAGE , REPEATED, STATIC  , OTHER, dfu_init_command_t, boot_validation, is_debug, &dfu_boot_validation_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t dfu_command_fields[3] = {
+    PB_FIELD(  1, UENUM   , OPTIONAL, STATIC  , FIRST, dfu_command_t, op_code, op_code, 0),
+    PB_FIELD(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, dfu_command_t, init, op_code, &dfu_init_command_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t dfu_signed_command_fields[4] = {
+    PB_FIELD(  1, MESSAGE , REQUIRED, STATIC  , FIRST, dfu_signed_command_t, command, command, &dfu_command_fields),
+    PB_FIELD(  2, UENUM   , REQUIRED, STATIC  , OTHER, dfu_signed_command_t, signature_type, command, 0),
+    PB_FIELD(  3, BYTES   , REQUIRED, STATIC  , OTHER, dfu_signed_command_t, signature, signature_type, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t dfu_packet_fields[3] = {
+    PB_FIELD(  1, MESSAGE , OPTIONAL, STATIC  , FIRST, dfu_packet_t, command, command, &dfu_command_fields),
+    PB_FIELD(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, dfu_packet_t, signed_command, command, &dfu_signed_command_fields),
+    PB_LAST_FIELD
+};
+
+
+/* Check that field information fits in pb_field_t */
+#if !defined(PB_FIELD_32BIT)
+/* If you get an error here, it means that you need to define PB_FIELD_32BIT
+ * compile-time option. You can do that in pb.h or on compiler command line.
+ *
+ * The reason you need to do this is that some of your messages contain tag
+ * numbers or field sizes that are larger than what can fit in 8 or 16 bit
+ * field descriptors.
+ */
+PB_STATIC_ASSERT((pb_membersize(dfu_init_command_t, hash) < 65536 && pb_membersize(dfu_init_command_t, boot_validation[0]) < 65536 && pb_membersize(dfu_command_t, init) < 65536 && pb_membersize(dfu_signed_command_t, command) < 65536 && pb_membersize(dfu_packet_t, command) < 65536 && pb_membersize(dfu_packet_t, signed_command) < 65536), YOU_MUST_DEFINE_PB_FIELD_32BIT_FOR_MESSAGES_dfu_hash_dfu_boot_validation_dfu_init_command_dfu_command_dfu_signed_command_dfu_packet)
+#endif
+
+#if !defined(PB_FIELD_16BIT) && !defined(PB_FIELD_32BIT)
+/* If you get an error here, it means that you need to define PB_FIELD_16BIT
+ * compile-time option. You can do that in pb.h or on compiler command line.
+ *
+ * The reason you need to do this is that some of your messages contain tag
+ * numbers or field sizes that are larger than what can fit in the default
+ * 8 bit descriptors.
+ */
+PB_STATIC_ASSERT((pb_membersize(dfu_init_command_t, hash) < 256 && pb_membersize(dfu_init_command_t, boot_validation[0]) < 256 && pb_membersize(dfu_command_t, init) < 256 && pb_membersize(dfu_signed_command_t, command) < 256 && pb_membersize(dfu_packet_t, command) < 256 && pb_membersize(dfu_packet_t, signed_command) < 256), YOU_MUST_DEFINE_PB_FIELD_16BIT_FOR_MESSAGES_dfu_hash_dfu_boot_validation_dfu_init_command_dfu_command_dfu_signed_command_dfu_packet)
+#endif
+
+
+/* @@protoc_insertion_point(eof) */

components/libraries/bootloader/dfu/dfu-cc.pb.h

@@ -0,0 +1,241 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+/* Automatically generated nanopb header */
+/* Generated by nanopb-0.3.6-dev at Tue Sep 11 14:37:18 2018. */
+
+#ifndef PB_DFU_CC_PB_H_INCLUDED
+#define PB_DFU_CC_PB_H_INCLUDED
+#include <pb.h>
+
+/* @@protoc_insertion_point(includes) */
+#if PB_PROTO_HEADER_VERSION != 30
+#error Regenerate this file with the current version of nanopb generator.
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Enum definitions */
+typedef enum
+{
+    DFU_FW_TYPE_APPLICATION = 0,
+    DFU_FW_TYPE_SOFTDEVICE = 1,
+    DFU_FW_TYPE_BOOTLOADER = 2,
+    DFU_FW_TYPE_SOFTDEVICE_BOOTLOADER = 3,
+    DFU_FW_TYPE_EXTERNAL_APPLICATION = 4
+} dfu_fw_type_t;
+#define DFU_FW_TYPE_MIN DFU_FW_TYPE_APPLICATION
+#define DFU_FW_TYPE_MAX DFU_FW_TYPE_EXTERNAL_APPLICATION
+#define DFU_FW_TYPE_ARRAYSIZE ((dfu_fw_type_t)(DFU_FW_TYPE_EXTERNAL_APPLICATION+1))
+
+typedef enum
+{
+    DFU_HASH_TYPE_NO_HASH = 0,
+    DFU_HASH_TYPE_CRC = 1,
+    DFU_HASH_TYPE_SHA128 = 2,
+    DFU_HASH_TYPE_SHA256 = 3,
+    DFU_HASH_TYPE_SHA512 = 4
+} dfu_hash_type_t;
+#define DFU_HASH_TYPE_MIN DFU_HASH_TYPE_NO_HASH
+#define DFU_HASH_TYPE_MAX DFU_HASH_TYPE_SHA512
+#define DFU_HASH_TYPE_ARRAYSIZE ((dfu_hash_type_t)(DFU_HASH_TYPE_SHA512+1))
+
+typedef enum
+{
+    DFU_OP_CODE_INIT = 1
+} dfu_op_code_t;
+#define DFU_OP_CODE_MIN DFU_OP_CODE_INIT
+#define DFU_OP_CODE_MAX DFU_OP_CODE_INIT
+#define DFU_OP_CODE_ARRAYSIZE ((dfu_op_code_t)(DFU_OP_CODE_INIT+1))
+
+typedef enum
+{
+    DFU_VALIDATION_TYPE_NO_VALIDATION = 0,
+    DFU_VALIDATION_TYPE_VALIDATE_GENERATED_CRC = 1,
+    DFU_VALIDATION_TYPE_VALIDATE_SHA256 = 2,
+    DFU_VALIDATION_TYPE_VALIDATE_ECDSA_P256_SHA256 = 3
+} dfu_validation_type_t;
+#define DFU_VALIDATION_TYPE_MIN DFU_VALIDATION_TYPE_NO_VALIDATION
+#define DFU_VALIDATION_TYPE_MAX DFU_VALIDATION_TYPE_VALIDATE_ECDSA_P256_SHA256
+#define DFU_VALIDATION_TYPE_ARRAYSIZE ((dfu_validation_type_t)(DFU_VALIDATION_TYPE_VALIDATE_ECDSA_P256_SHA256+1))
+
+typedef enum
+{
+    DFU_SIGNATURE_TYPE_ECDSA_P256_SHA256 = 0,
+    DFU_SIGNATURE_TYPE_ED25519 = 1
+} dfu_signature_type_t;
+#define DFU_SIGNATURE_TYPE_MIN DFU_SIGNATURE_TYPE_ECDSA_P256_SHA256
+#define DFU_SIGNATURE_TYPE_MAX DFU_SIGNATURE_TYPE_ED25519
+#define DFU_SIGNATURE_TYPE_ARRAYSIZE ((dfu_signature_type_t)(DFU_SIGNATURE_TYPE_ED25519+1))
+
+/* Struct definitions */
+typedef PB_BYTES_ARRAY_T(64) dfu_boot_validation_bytes_t;
+typedef struct {
+    dfu_validation_type_t type;
+    dfu_boot_validation_bytes_t bytes;
+/* @@protoc_insertion_point(struct:dfu_boot_validation_t) */
+} dfu_boot_validation_t;
+
+typedef PB_BYTES_ARRAY_T(32) dfu_hash_hash_t;
+typedef struct {
+    dfu_hash_type_t hash_type;
+    dfu_hash_hash_t hash;
+/* @@protoc_insertion_point(struct:dfu_hash_t) */
+} dfu_hash_t;
+
+typedef struct {
+    bool has_fw_version;
+    uint32_t fw_version;
+    bool has_hw_version;
+    uint32_t hw_version;
+    pb_size_t sd_req_count;
+    uint32_t sd_req[16];
+    bool has_type;
+    dfu_fw_type_t type;
+    bool has_sd_size;
+    uint32_t sd_size;
+    bool has_bl_size;
+    uint32_t bl_size;
+    bool has_app_size;
+    uint32_t app_size;
+    bool has_hash;
+    dfu_hash_t hash;
+    bool has_is_debug;
+    bool is_debug;
+    pb_size_t boot_validation_count;
+    dfu_boot_validation_t boot_validation[3];
+/* @@protoc_insertion_point(struct:dfu_init_command_t) */
+} dfu_init_command_t;
+
+typedef struct {
+    bool has_op_code;
+    dfu_op_code_t op_code;
+    bool has_init;
+    dfu_init_command_t init;
+/* @@protoc_insertion_point(struct:dfu_command_t) */
+} dfu_command_t;
+
+typedef PB_BYTES_ARRAY_T(64) dfu_signed_command_signature_t;
+typedef struct {
+    dfu_command_t command;
+    dfu_signature_type_t signature_type;
+    dfu_signed_command_signature_t signature;
+/* @@protoc_insertion_point(struct:dfu_signed_command_t) */
+} dfu_signed_command_t;
+
+typedef struct {
+    bool has_command;
+    dfu_command_t command;
+    bool has_signed_command;
+    dfu_signed_command_t signed_command;
+/* @@protoc_insertion_point(struct:dfu_packet_t) */
+} dfu_packet_t;
+
+/* Default values for struct fields */
+extern const bool dfu_init_command_is_debug_default;
+
+/* Initializer values for message structs */
+#define DFU_HASH_INIT_DEFAULT                    {(dfu_hash_type_t)0, {0, {0}}}
+#define DFU_BOOT_VALIDATION_INIT_DEFAULT         {(dfu_validation_type_t)0, {0, {0}}}
+#define DFU_INIT_COMMAND_INIT_DEFAULT            {false, 0, false, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, false, (dfu_fw_type_t)0, false, 0, false, 0, false, 0, false, DFU_HASH_INIT_DEFAULT, false, false, 0, {DFU_BOOT_VALIDATION_INIT_DEFAULT, DFU_BOOT_VALIDATION_INIT_DEFAULT, DFU_BOOT_VALIDATION_INIT_DEFAULT}}
+#define DFU_COMMAND_INIT_DEFAULT                 {false, (dfu_op_code_t)0, false, DFU_INIT_COMMAND_INIT_DEFAULT}
+#define DFU_SIGNED_COMMAND_INIT_DEFAULT          {DFU_COMMAND_INIT_DEFAULT, (dfu_signature_type_t)0, {0, {0}}}
+#define DFU_PACKET_INIT_DEFAULT                  {false, DFU_COMMAND_INIT_DEFAULT, false, DFU_SIGNED_COMMAND_INIT_DEFAULT}
+#define DFU_HASH_INIT_ZERO                       {(dfu_hash_type_t)0, {0, {0}}}
+#define DFU_BOOT_VALIDATION_INIT_ZERO            {(dfu_validation_type_t)0, {0, {0}}}
+#define DFU_INIT_COMMAND_INIT_ZERO               {false, 0, false, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, false, (dfu_fw_type_t)0, false, 0, false, 0, false, 0, false, DFU_HASH_INIT_ZERO, false, 0, 0, {DFU_BOOT_VALIDATION_INIT_ZERO, DFU_BOOT_VALIDATION_INIT_ZERO, DFU_BOOT_VALIDATION_INIT_ZERO}}
+#define DFU_COMMAND_INIT_ZERO                    {false, (dfu_op_code_t)0, false, DFU_INIT_COMMAND_INIT_ZERO}
+#define DFU_SIGNED_COMMAND_INIT_ZERO             {DFU_COMMAND_INIT_ZERO, (dfu_signature_type_t)0, {0, {0}}}
+#define DFU_PACKET_INIT_ZERO                     {false, DFU_COMMAND_INIT_ZERO, false, DFU_SIGNED_COMMAND_INIT_ZERO}
+
+/* Field tags (for use in manual encoding/decoding) */
+#define DFU_BOOT_VALIDATION_TYPE_TAG             1
+#define DFU_BOOT_VALIDATION_BYTES_TAG            2
+#define DFU_HASH_HASH_TYPE_TAG                   1
+#define DFU_HASH_HASH_TAG                        2
+#define DFU_INIT_COMMAND_FW_VERSION_TAG          1
+#define DFU_INIT_COMMAND_HW_VERSION_TAG          2
+#define DFU_INIT_COMMAND_SD_REQ_TAG              3
+#define DFU_INIT_COMMAND_TYPE_TAG                4
+#define DFU_INIT_COMMAND_SD_SIZE_TAG             5
+#define DFU_INIT_COMMAND_BL_SIZE_TAG             6
+#define DFU_INIT_COMMAND_APP_SIZE_TAG            7
+#define DFU_INIT_COMMAND_HASH_TAG                8
+#define DFU_INIT_COMMAND_IS_DEBUG_TAG            9
+#define DFU_INIT_COMMAND_BOOT_VALIDATION_TAG     10
+#define DFU_COMMAND_OP_CODE_TAG                  1
+#define DFU_COMMAND_INIT_TAG                     2
+#define DFU_SIGNED_COMMAND_COMMAND_TAG           1
+#define DFU_SIGNED_COMMAND_SIGNATURE_TYPE_TAG    2
+#define DFU_SIGNED_COMMAND_SIGNATURE_TAG         3
+#define DFU_PACKET_COMMAND_TAG                   1
+#define DFU_PACKET_SIGNED_COMMAND_TAG            2
+
+/* Struct field encoding specification for nanopb */
+extern const pb_field_t dfu_hash_fields[3];
+extern const pb_field_t dfu_boot_validation_fields[3];
+extern const pb_field_t dfu_init_command_fields[11];
+extern const pb_field_t dfu_command_fields[3];
+extern const pb_field_t dfu_signed_command_fields[4];
+extern const pb_field_t dfu_packet_fields[3];
+
+/* Maximum encoded size of messages (where known) */
+#define DFU_HASH_SIZE                            36
+#define DFU_BOOT_VALIDATION_SIZE                 68
+#define DFU_INIT_COMMAND_SIZE                    378
+#define DFU_COMMAND_SIZE                         383
+#define DFU_SIGNED_COMMAND_SIZE                  454
+#define DFU_PACKET_SIZE                          843
+
+/* Message IDs (where set with "msgid" option) */
+#ifdef PB_MSGID
+
+#define DFU_CC_MESSAGES \
+
+
+#endif
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+/* @@protoc_insertion_point(eof) */
+
+#endif

components/libraries/bootloader/dfu/dfu-cc.proto

@@ -0,0 +1,82 @@
+package dfu;
+
+// Version 0.1
+
+enum FwType {
+    APPLICATION             = 0;
+    SOFTDEVICE              = 1;
+    BOOTLOADER              = 2;
+    SOFTDEVICE_BOOTLOADER   = 3;
+    EXTERNAL_APPLICATION    = 4;
+}
+
+enum HashType {
+    NO_HASH = 0;
+    CRC     = 1;
+    SHA128  = 2;
+    SHA256  = 3;
+    SHA512  = 4;
+}
+
+enum OpCode {
+    INIT = 1;
+}
+
+enum ValidationType {
+    NO_VALIDATION                   = 0;
+    VALIDATE_GENERATED_CRC          = 1;
+    VALIDATE_SHA256                 = 2;
+    VALIDATE_ECDSA_P256_SHA256      = 3;
+}
+
+message Hash {
+    required HashType   hash_type   = 1;
+    required bytes      hash        = 2;
+}
+
+message BootValidation {
+
+    required ValidationType type    = 1;
+    required bytes          bytes   = 2;
+}
+
+// Commands data
+message InitCommand {
+    optional uint32             fw_version      = 1;
+    optional uint32             hw_version      = 2;
+    repeated uint32             sd_req          = 3 [packed = true];
+    optional FwType             type            = 4;
+
+    optional uint32             sd_size         = 5;
+    optional uint32             bl_size         = 6;
+    optional uint32             app_size        = 7;
+
+    optional Hash               hash            = 8;
+
+    optional bool               is_debug        = 9 [default = false];
+    repeated BootValidation     boot_validation = 10;
+}
+
+// Command type
+message Command {
+    optional OpCode         op_code = 1;
+    optional InitCommand    init    = 2;
+}
+
+// Signed command types
+enum SignatureType {
+    ECDSA_P256_SHA256   = 0;
+    ED25519             = 1;
+}
+
+message SignedCommand {
+    required Command        command         = 1;
+    required SignatureType  signature_type  = 2;
+    required bytes          signature       = 3;
+}
+
+// Parent packet type
+message Packet {
+    optional Command        command         = 1;
+    optional SignedCommand  signed_command  = 2;
+}

components/libraries/bootloader/dfu/nrf_dfu.c

@@ -0,0 +1,98 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#include "nrf_dfu.h"
+
+#include "nrf_dfu_utils.h"
+#include "nrf_dfu_transport.h"
+#include "nrf_dfu_req_handler.h"
+#include "nrf_log.h"
+
+static nrf_dfu_observer_t m_user_observer;                          //<! Observer callback set by the user.
+
+
+
+/**
+ * @brief This function calls the user's observer (@ref m_observer) after it is done handling the event.
+ */
+static void dfu_observer(nrf_dfu_evt_type_t event)
+{
+    switch (event)
+    {
+        case NRF_DFU_EVT_DFU_COMPLETED:
+        case NRF_DFU_EVT_DFU_ABORTED:
+#ifndef NRF_DFU_NO_TRANSPORT
+            UNUSED_RETURN_VALUE(nrf_dfu_transports_close(NULL));
+#endif
+            break;
+        default:
+            break;
+    }
+
+    /* Call user's observer if present. */
+    if (m_user_observer)
+    {
+        m_user_observer(event);
+    }
+}
+
+
+
+uint32_t nrf_dfu_init(nrf_dfu_observer_t observer)
+{
+    uint32_t ret_val;
+
+    m_user_observer = observer;
+
+    NRF_LOG_INFO("Entering DFU mode.");
+
+    dfu_observer(NRF_DFU_EVT_DFU_INITIALIZED);
+
+    // Initializing transports
+    ret_val = nrf_dfu_transports_init(dfu_observer);
+    if (ret_val != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Could not initalize DFU transport: 0x%08x", ret_val);
+        return ret_val;
+    }
+
+    ret_val = nrf_dfu_req_handler_init(dfu_observer);
+
+    return ret_val;
+}

components/libraries/bootloader/dfu/nrf_dfu.h

@@ -0,0 +1,85 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_dfu DFU modules
+ * @{
+ * @ingroup  nrf_bootloader
+ * @brief Modules providing Device Firmware Update (DFU) functionality.
+ *
+ * The DFU module, in combination with the @ref nrf_bootloader module,
+ * can be used to implement a bootloader that supports Device Firmware Updates.
+ */
+
+#ifndef NRF_DFU_H__
+#define NRF_DFU_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "nrf_dfu_types.h"
+#include "nrf_dfu_req_handler.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define NRF_DFU_SCHED_EVENT_DATA_SIZE (sizeof(nrf_dfu_request_t))
+
+
+/** @brief Function for initializing a DFU operation.
+ *
+ * This function initializes a DFU operation and any transports that are registered
+ * in the system.
+ *
+ * @param[in] observer  Callback function for receiving DFU notifications.
+ *
+ * @retval  NRF_SUCCESS     If the DFU operation was successfully initialized.
+ */
+uint32_t nrf_dfu_init(nrf_dfu_observer_t observer);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_DFU_H__
+
+/** @} */

components/libraries/bootloader/dfu/nrf_dfu_flash.c

@@ -0,0 +1,167 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+
+#include "nrf_dfu_flash.h"
+#include "nrf_dfu_types.h"
+
+#include "nrf_fstorage.h"
+#include "nrf_fstorage_sd.h"
+#include "nrf_fstorage_nvmc.h"
+
+
+#define NRF_LOG_MODULE_NAME nrf_dfu_flash
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+
+void dfu_fstorage_evt_handler(nrf_fstorage_evt_t * p_evt);
+
+
+NRF_FSTORAGE_DEF(nrf_fstorage_t m_fs) =
+{
+    .evt_handler = dfu_fstorage_evt_handler,
+    .start_addr  = MBR_SIZE,
+    .end_addr    = BOOTLOADER_SETTINGS_ADDRESS + BOOTLOADER_SETTINGS_PAGE_SIZE
+};
+
+static uint32_t m_flash_operations_pending;
+
+void dfu_fstorage_evt_handler(nrf_fstorage_evt_t * p_evt)
+{
+    if (NRF_LOG_ENABLED && (m_flash_operations_pending > 0))
+    {
+        m_flash_operations_pending--;
+    }
+
+    if (p_evt->result == NRF_SUCCESS)
+    {
+        NRF_LOG_DEBUG("Flash %s success: addr=%p, pending %d",
+                      (p_evt->id == NRF_FSTORAGE_EVT_WRITE_RESULT) ? "write" : "erase",
+                      p_evt->addr, m_flash_operations_pending);
+    }
+    else
+    {
+        NRF_LOG_DEBUG("Flash %s failed (0x%x): addr=%p, len=0x%x bytes, pending %d",
+                      (p_evt->id == NRF_FSTORAGE_EVT_WRITE_RESULT) ? "write" : "erase",
+                      p_evt->result, p_evt->addr, p_evt->len, m_flash_operations_pending);
+    }
+
+    if (p_evt->p_param)
+    {
+        //lint -save -e611 (Suspicious cast)
+        ((nrf_dfu_flash_callback_t)(p_evt->p_param))((void*)p_evt->p_src);
+        //lint -restore
+    }
+}
+
+
+ret_code_t nrf_dfu_flash_init(bool sd_irq_initialized)
+{
+    nrf_fstorage_api_t * p_api_impl;
+
+    /* Setup the desired API implementation. */
+#if defined(BLE_STACK_SUPPORT_REQD) || defined(ANT_STACK_SUPPORT_REQD)
+    if (sd_irq_initialized)
+    {
+        NRF_LOG_DEBUG("Initializing nrf_fstorage_sd backend.");
+        p_api_impl = &nrf_fstorage_sd;
+    }
+    else
+#endif
+    {
+        NRF_LOG_DEBUG("Initializing nrf_fstorage_nvmc backend.");
+        p_api_impl = &nrf_fstorage_nvmc;
+    }
+
+    return nrf_fstorage_init(&m_fs, p_api_impl, NULL);
+}
+
+
+ret_code_t nrf_dfu_flash_store(uint32_t                   dest,
+                               void               const * p_src,
+                               uint32_t                   len,
+                               nrf_dfu_flash_callback_t   callback)
+{
+    ret_code_t rc;
+
+    NRF_LOG_DEBUG("nrf_fstorage_write(addr=%p, src=%p, len=%d bytes), queue usage: %d",
+                  dest, p_src, len, m_flash_operations_pending);
+
+    //lint -save -e611 (Suspicious cast)
+    rc = nrf_fstorage_write(&m_fs, dest, p_src, len, (void *)callback);
+    //lint -restore
+
+    if ((NRF_LOG_ENABLED) && (rc == NRF_SUCCESS))
+    {
+        m_flash_operations_pending++;
+    }
+    else
+    {
+        NRF_LOG_WARNING("nrf_fstorage_write() failed with error 0x%x.", rc);
+    }
+
+    return rc;
+}
+
+
+ret_code_t nrf_dfu_flash_erase(uint32_t                 page_addr,
+                               uint32_t                 num_pages,
+                               nrf_dfu_flash_callback_t callback)
+{
+    ret_code_t rc;
+
+    NRF_LOG_DEBUG("nrf_fstorage_erase(addr=0x%p, len=%d pages), queue usage: %d",
+                  page_addr, num_pages, m_flash_operations_pending);
+
+    //lint -save -e611 (Suspicious cast)
+    rc = nrf_fstorage_erase(&m_fs, page_addr, num_pages, (void *)callback);
+    //lint -restore
+
+    if ((NRF_LOG_ENABLED) && (rc == NRF_SUCCESS))
+    {
+        m_flash_operations_pending++;
+    }
+    else
+    {
+        NRF_LOG_WARNING("nrf_fstorage_erase() failed with error 0x%x.", rc);
+    }
+
+    return rc;
+}

components/libraries/bootloader/dfu/nrf_dfu_flash.h

@@ -0,0 +1,132 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 sdk_nrf_dfu_flash Flash operations
+ * @{
+ * @ingroup  nrf_dfu
+ */
+
+#ifndef NRF_DFU_FLASH_H__
+#define NRF_DFU_FLASH_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "sdk_errors.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**@brief   nrf_fstorage event handler function for DFU fstorage operations.
+ *
+ * This function will be called after a flash operation has completed.
+ */
+typedef void (*nrf_dfu_flash_callback_t)(void * p_buf);
+
+
+/**@brief Function for initializing the flash module.
+ *
+ * Depending on whether or not the SoftDevice is present and its IRQ have been initialized,
+ * this function initializes the correct @ref nrf_fstorage backend.
+ *
+ * @param[in]   sd_irq_initialized  Whether or not the SoftDevice IRQ have been initialized.
+ *
+ * @retval NRF_SUCCESS  If the operation was successful.
+  */
+ret_code_t nrf_dfu_flash_init(bool sd_irq_initialized);
+
+
+/**@brief Function for storing data to flash.
+ *
+ * This functions is asynchronous when the SoftDevice is enabled and synchronous when
+ * the SoftDevice is not present or disabled. In both cases, if a callback function is provided,
+ * it will be called when the operation has completed.
+ *
+ * @note The content of @p p_src should be kept in memory until the operation has completed.
+ *
+ * @param[in]  dest      The address where the data should be stored.
+ * @param[in]  p_src     Pointer to the address where the data should be copied from.
+ *                       This address can be in flash or RAM.
+ * @param[in]  len       The number of bytes to be copied from @p p_src to @p dest.
+ * @param[in]  callback  Callback function.
+ *
+ * @retval  NRF_SUCCESS                 If the operation was successful.
+ * @retval  NRF_ERROR_INVALID_STATE     If nrf_dfu_flash is not initialized.
+ * @retval  NRF_ERROR_INVALID_ADDR      If @p p_src or @p dest is not word-aligned.
+ * @retval  NRF_ERROR_INVALID_LENGTH    If @p len is zero.
+ * @retval  NRF_ERROR_NULL              If @p p_src is NULL.
+ * @retval  NRF_ERROR_NO_MEM            If nrf_fstorage is out of memory.
+ */
+ret_code_t nrf_dfu_flash_store(uint32_t                     dest,
+                               void                 const * p_src,
+                               uint32_t                     len,
+                               nrf_dfu_flash_callback_t     callback);
+
+
+/**@brief Function for erasing data from flash.
+ *
+ * This functions is asynchronous when the SoftDevice is enabled and synchronous when
+ * the SoftDevice is not present or disabled. In both cases, if a callback function is provided,
+ * it will be called when the operation has completed.
+ *
+ * @param[in]  page_addr    The address of the first flash page to be deleted.
+ * @param[in]  num_pages    The number of flash pages to be deleted.
+ * @param[in]  callback     Callback function.
+ *
+ * @retval  NRF_SUCCESS                 If the operation was successful.
+ * @retval  NRF_ERROR_INVALID_STATE     If nrf_dfu_flash is not initialized.
+ * @retval  NRF_ERROR_INVALID_ADDR      If @p page_addr is not aligned to a page boundary or the
+ *                                      operation would go beyond the flash memory boundaries.
+ * @retval  NRF_ERROR_INVALID_LENGTH    If @p num_pages is zero.
+ * @retval  NRF_ERROR_NULL              If @p page_addr is NULL.
+ * @retval  NRF_ERROR_NO_MEM            If the queue of nrf_fstorage is full.
+ */
+ret_code_t nrf_dfu_flash_erase(uint32_t page_addr, uint32_t num_pages, nrf_dfu_flash_callback_t callback);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // NRF_DFU_FLASH_H__
+/** @} */

components/libraries/bootloader/dfu/nrf_dfu_handling_error.c

@@ -0,0 +1,61 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+
+#include "nrf_dfu_handling_error.h"
+
+#include "nrf_log.h"
+#include "nrf_dfu_req_handler.h"
+
+static nrf_dfu_ext_error_code_t m_last_error = NRF_DFU_EXT_ERROR_NO_ERROR;
+
+nrf_dfu_result_t ext_error_set(nrf_dfu_ext_error_code_t error_code)
+{
+    m_last_error = error_code;
+
+    return NRF_DFU_RES_CODE_EXT_ERROR;
+}
+
+nrf_dfu_ext_error_code_t ext_error_get()
+{
+    nrf_dfu_ext_error_code_t last_error = m_last_error;
+    m_last_error = NRF_DFU_EXT_ERROR_NO_ERROR;
+
+    return last_error;
+}

components/libraries/bootloader/dfu/nrf_dfu_handling_error.h

@@ -0,0 +1,125 @@
+/**
+ * Copyright (c) 2017 - 2021, 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 nrf_dfu_rescodes DFU result codes
+ * @{
+ * @ingroup  sdk_nrf_dfu_transport
+ * @brief When the DFU controller sends requests to the DFU bootloader on
+ * the DFU target, the DFU bootloader answers with any of these result codes.
+ */
+
+
+#ifndef DFU_HANDLING_ERROR_H__
+#define DFU_HANDLING_ERROR_H__
+
+#include "nrf_dfu_types.h"
+#include "nrf_dfu_req_handler.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+/**@brief DFU request extended result codes.
+ *
+ * @details When an event returns @ref NRF_DFU_RES_CODE_EXT_ERROR, it also stores an extended error code.
+ *          The transport layer can then send the extended error code together with the error code to give
+ *          the controller additional information about the cause of the error.
+ */
+typedef enum
+{
+    NRF_DFU_EXT_ERROR_NO_ERROR                  = 0x00, /**< No extended error code has been set. This error indicates an implementation problem. */
+    NRF_DFU_EXT_ERROR_INVALID_ERROR_CODE        = 0x01, /**< Invalid error code. This error code should never be used outside of development. */
+    NRF_DFU_EXT_ERROR_WRONG_COMMAND_FORMAT      = 0x02, /**< The format of the command was incorrect. This error code is not used in the
+                                                             current implementation, because @ref NRF_DFU_RES_CODE_OP_CODE_NOT_SUPPORTED
+                                                             and @ref NRF_DFU_RES_CODE_INVALID_PARAMETER cover all
+                                                             possible format errors. */
+    NRF_DFU_EXT_ERROR_UNKNOWN_COMMAND           = 0x03, /**< The command was successfully parsed, but it is not supported or unknown. */
+    NRF_DFU_EXT_ERROR_INIT_COMMAND_INVALID      = 0x04, /**< The init command is invalid. The init packet either has
+                                                             an invalid update type or it is missing required fields for the update type
+                                                             (for example, the init packet for a SoftDevice update is missing the SoftDevice size field). */
+    NRF_DFU_EXT_ERROR_FW_VERSION_FAILURE        = 0x05, /**< The firmware version is too low. For an application or SoftDevice, the version must be greater than
+                                                             or equal to the current version. For a bootloader, it must be greater than the current version.
+                                                             to the current version. This requirement prevents downgrade attacks.*/
+    NRF_DFU_EXT_ERROR_HW_VERSION_FAILURE        = 0x06, /**< The hardware version of the device does not match the required
+                                                             hardware version for the update. */
+    NRF_DFU_EXT_ERROR_SD_VERSION_FAILURE        = 0x07, /**< The array of supported SoftDevices for the update does not contain
+                                                             the FWID of the current SoftDevice or the first FWID is '0' on a
+                                                             bootloader which requires the SoftDevice to be present. */
+    NRF_DFU_EXT_ERROR_SIGNATURE_MISSING         = 0x08, /**< The init packet does not contain a signature. This error code is not used in the
+                                                             current implementation, because init packets without a signature
+                                                             are regarded as invalid. */
+    NRF_DFU_EXT_ERROR_WRONG_HASH_TYPE           = 0x09, /**< The hash type that is specified by the init packet is not supported by the DFU bootloader. */
+    NRF_DFU_EXT_ERROR_HASH_FAILED               = 0x0A, /**< The hash of the firmware image cannot be calculated. */
+    NRF_DFU_EXT_ERROR_WRONG_SIGNATURE_TYPE      = 0x0B, /**< The type of the signature is unknown or not supported by the DFU bootloader. */
+    NRF_DFU_EXT_ERROR_VERIFICATION_FAILED       = 0x0C, /**< The hash of the received firmware image does not match the hash in the init packet. */
+    NRF_DFU_EXT_ERROR_INSUFFICIENT_SPACE        = 0x0D, /**< The available space on the device is insufficient to hold the firmware. */
+} nrf_dfu_ext_error_code_t;
+
+
+/**@brief Function for setting an extended error code that can be retrieved later.
+ *
+ * @details When an extended error occurs in the DFU process, this function can be used to store the error.
+ *
+ * @param error_code The error code to store.
+ *
+ * @retval NRF_DFU_RES_CODE_EXT_ERROR
+ */
+nrf_dfu_result_t ext_error_set(nrf_dfu_ext_error_code_t error_code);
+
+/**@brief Function for getting the most recent extended error code.
+ *
+ * @details This function is used by the transport layer to fetch the most recent extended error code.
+ *
+ * @return The most recent error code. If the function is called again before a new error occurs, @ref NRF_DFU_EXT_ERROR_NO_ERROR is returned.
+ */
+nrf_dfu_ext_error_code_t ext_error_get( void );
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // DFU_HANDLING_ERROR_H__
+
+/** @} */

components/libraries/bootloader/dfu/nrf_dfu_mbr.c

@@ -0,0 +1,105 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#include "nrf_dfu_mbr.h"
+#include "nrf_mbr.h"
+#include "nrf_dfu_types.h"
+#include "nrf_log.h"
+#include "nrf_bootloader_info.h"
+
+#define MBR_IRQ_FORWARD_ADDRESS_ADDRESS (0x20000000) //!< The address of the variable that decides where the MBR forwards interrupts
+
+uint32_t nrf_dfu_mbr_copy_bl(uint32_t * p_src, uint32_t len)
+{
+    uint32_t ret_val;
+    uint32_t const len_words = len / sizeof(uint32_t);
+
+    sd_mbr_command_t command =
+    {
+        .command = SD_MBR_COMMAND_COPY_BL,
+        .params.copy_bl.bl_src = p_src,
+        .params.copy_bl.bl_len = len_words
+    };
+
+    ret_val = sd_mbr_command(&command);
+
+    return ret_val;
+}
+
+
+uint32_t nrf_dfu_mbr_init_sd(void)
+{
+    uint32_t ret_val;
+
+    sd_mbr_command_t command =
+    {
+        .command = SD_MBR_COMMAND_INIT_SD
+    };
+
+    ret_val = sd_mbr_command(&command);
+
+    return ret_val;
+}
+
+
+uint32_t nrf_dfu_mbr_irq_forward_address_set(void)
+{
+    uint32_t ret_val = NRF_ERROR_INVALID_PARAM;
+    uint32_t address = MBR_SIZE;
+
+#if !defined(BLE_STACK_SUPPORT_REQD) && !defined(ANT_STACK_SUPPORT_REQD)
+    sd_mbr_command_t command =
+    {
+        .command = SD_MBR_COMMAND_IRQ_FORWARD_ADDRESS_SET,
+        .params.irq_forward_address_set.address = address,
+    };
+
+    ret_val = sd_mbr_command(&command);
+#endif
+
+    if (ret_val == NRF_ERROR_INVALID_PARAM)
+    {
+        // Manually set the forward address if this MBR doesn't have the command.
+        *(uint32_t *)(MBR_IRQ_FORWARD_ADDRESS_ADDRESS) = address;
+
+        ret_val = NRF_SUCCESS;
+    }
+
+    return ret_val;
+}

components/libraries/bootloader/dfu/nrf_dfu_mbr.h

@@ -0,0 +1,90 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 sdk_nrf_dfu_mbr MBR functions
+ * @{
+ * @ingroup  nrf_dfu
+ */
+
+#ifndef NRF_DFU_MBR_H__
+#define NRF_DFU_MBR_H__
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Function for copying the bootloader using an MBR command.
+ *
+ * @param[in] p_src         Source address of the bootloader data to copy.
+ * @param[in] len           Length of the data to copy in bytes.
+ *
+ * @return  This function will return only if the command request could not be run.
+ *          See @ref sd_mbr_command_copy_bl_t for possible return values.
+ */
+uint32_t nrf_dfu_mbr_copy_bl(uint32_t * p_src, uint32_t len);
+
+
+/** @brief Function for initializing the SoftDevice using an MBR command.
+ *
+ * @retval  NRF_SUCCESS     If the SoftDevice was initialized successfully.
+ *                          Any other return value indicates that the SoftDevice
+ *                          could not be initialized.
+ */
+uint32_t nrf_dfu_mbr_init_sd(void);
+
+
+/** @brief Function for setting the address of the IRQ table to the app's using an MBR command.
+ *
+ * @retval  NRF_SUCCESS  If the address of the new irq table was set. Any other
+ *                       return value indicates that the address could not be set.
+ */
+uint32_t nrf_dfu_mbr_irq_forward_address_set(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_DFU_MBR_H__
+
+/** @} */

components/libraries/bootloader/dfu/nrf_dfu_req_handler.c

@@ -0,0 +1,865 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+#include <stdint.h>
+#include <stdbool.h>
+#include "sdk_config.h"
+#include "nrf_dfu.h"
+#include "nrf_dfu_types.h"
+#include "nrf_dfu_req_handler.h"
+#include "nrf_dfu_handling_error.h"
+#include "nrf_dfu_settings.h"
+#include "nrf_dfu_utils.h"
+#include "nrf_dfu_flash.h"
+#include "nrf_fstorage.h"
+#include "nrf_bootloader_info.h"
+#include "app_util.h"
+#include "pb.h"
+#include "pb_common.h"
+#include "pb_decode.h"
+#include "dfu-cc.pb.h"
+#include "crc32.h"
+#include "app_scheduler.h"
+#include "sdk_macros.h"
+#include "nrf_crypto.h"
+#include "nrf_assert.h"
+#include "nrf_dfu_validation.h"
+
+#define NRF_LOG_MODULE_NAME nrf_dfu_req_handler
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+#define NRF_DFU_PROTOCOL_VERSION    (0x01)
+
+#ifndef NRF_DFU_PROTOCOL_REDUCED
+#define NRF_DFU_PROTOCOL_REDUCED 0
+#endif
+
+STATIC_ASSERT(DFU_SIGNED_COMMAND_SIZE <= INIT_COMMAND_MAX_SIZE);
+
+static uint32_t m_firmware_start_addr;          /**< Start address of the current firmware image. */
+static uint32_t m_firmware_size_req;            /**< The size of the entire firmware image. Defined by the init command. */
+
+static nrf_dfu_observer_t m_observer;
+
+
+static void on_dfu_complete(nrf_fstorage_evt_t * p_evt)
+{
+    UNUSED_PARAMETER(p_evt);
+
+    NRF_LOG_DEBUG("All flash operations have completed. DFU completed.");
+
+    m_observer(NRF_DFU_EVT_DFU_COMPLETED);
+}
+
+
+static nrf_dfu_result_t ext_err_code_handle(nrf_dfu_result_t ret_val)
+{
+    if (ret_val < NRF_DFU_RES_CODE_EXT_ERROR)
+    {
+        return ret_val;
+    }
+    else
+    {
+        nrf_dfu_ext_error_code_t ext_err =
+                (nrf_dfu_ext_error_code_t)((uint8_t)ret_val - (uint8_t)NRF_DFU_RES_CODE_EXT_ERROR);
+        return ext_error_set(ext_err);
+    }
+}
+
+
+#if !NRF_DFU_PROTOCOL_REDUCED
+static void on_protocol_version_request(nrf_dfu_request_t const * p_req, nrf_dfu_response_t * p_res)
+{
+    UNUSED_PARAMETER(p_req);
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_PROTOCOL_VERSION");
+
+    if (NRF_DFU_PROTOCOL_VERSION_MSG)
+    {
+        p_res->protocol.version = NRF_DFU_PROTOCOL_VERSION;
+    }
+    else
+    {
+        NRF_LOG_DEBUG("NRF_DFU_OP_PROTOCOL_VERSION disabled.");
+        p_res->result = NRF_DFU_RES_CODE_OP_CODE_NOT_SUPPORTED;
+    }
+}
+
+
+static void on_hw_version_request(nrf_dfu_request_t const * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_HARDWARE_VERSION");
+
+    p_res->hardware.part    = NRF_FICR->INFO.PART;
+    p_res->hardware.variant = NRF_FICR->INFO.VARIANT;
+
+    /* FICR values are in Kilobytes, we report them in bytes. */
+    p_res->hardware.memory.ram_size      = NRF_FICR->INFO.RAM   * 1024;
+    p_res->hardware.memory.rom_size      = NRF_FICR->INFO.FLASH * 1024;
+    p_res->hardware.memory.rom_page_size = NRF_FICR->CODEPAGESIZE;
+}
+
+
+static void on_fw_version_request(nrf_dfu_request_t const * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_FIRMWARE_VERSION");
+    NRF_LOG_DEBUG("Firmware image requested: %d", p_req->firmware.image_number);
+
+    if (NRF_DFU_PROTOCOL_FW_VERSION_MSG)
+    {
+        uint8_t fw_count = 1;
+
+        if (SD_PRESENT)
+        {
+            fw_count++;
+        }
+
+        if (s_dfu_settings.bank_0.bank_code == NRF_DFU_BANK_VALID_APP)
+        {
+            fw_count++;
+        }
+
+        p_res->result = NRF_DFU_RES_CODE_SUCCESS;
+
+        if (p_req->firmware.image_number == 0)
+        {
+            /* Bootloader is always present and it is always image zero. */
+            p_res->firmware.type    = NRF_DFU_FIRMWARE_TYPE_BOOTLOADER;
+            p_res->firmware.version = s_dfu_settings.bootloader_version;
+            p_res->firmware.addr    = BOOTLOADER_START_ADDR;
+            p_res->firmware.len     = BOOTLOADER_SIZE;
+        }
+        else if ((p_req->firmware.image_number == 1) && SD_PRESENT)
+        {
+            /* If a SoftDevice is present, it will be firmware image one. */
+            p_res->firmware.type    = NRF_DFU_FIRMWARE_TYPE_SOFTDEVICE;
+            p_res->firmware.version = SD_VERSION_GET(MBR_SIZE);
+            p_res->firmware.addr    = MBR_SIZE;
+            p_res->firmware.len     = SD_SIZE_GET(MBR_SIZE);
+        }
+        else if ((p_req->firmware.image_number < fw_count))
+        {
+            /* Either there is no SoftDevice and the firmware image requested is one,
+             * or there is a SoftDevice and the firmware image requested is two.
+             */
+            p_res->firmware.type    = NRF_DFU_FIRMWARE_TYPE_APPLICATION;
+            p_res->firmware.version = s_dfu_settings.app_version;
+            p_res->firmware.addr    = nrf_dfu_app_start_address();
+            p_res->firmware.len     = s_dfu_settings.bank_0.image_size;
+        }
+        else
+        {
+            NRF_LOG_DEBUG("No such firmware image");
+            p_res->firmware.type    = NRF_DFU_FIRMWARE_TYPE_UNKNOWN;
+            p_res->firmware.version = 0x00;
+            p_res->firmware.addr    = 0x00;
+            p_res->firmware.len     = 0x00;
+        }
+    }
+    else
+    {
+        NRF_LOG_DEBUG("NRF_DFU_OP_FIRMWARE_VERSION disabled.");
+        p_res->result        = NRF_DFU_RES_CODE_OP_CODE_NOT_SUPPORTED;
+        p_res->firmware.type = NRF_DFU_FIRMWARE_TYPE_UNKNOWN;
+    }
+}
+
+
+static void on_ping_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_PING");
+    p_res->ping.id = p_req->ping.id;
+}
+
+
+static void on_mtu_get_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_MTU_GET");
+    p_res->mtu.size = p_req->mtu.size;
+}
+#endif // !NRF_DFU_PROTOCOL_REDUCED
+
+
+static void on_prn_set_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    UNUSED_PARAMETER(p_req);
+    UNUSED_PARAMETER(p_res);
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_RECEIPT_NOTIF_SET");
+}
+
+
+static void on_abort_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    UNUSED_PARAMETER(p_req);
+    UNUSED_PARAMETER(p_res);
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_ABORT");
+
+    m_observer(NRF_DFU_EVT_DFU_ABORTED);
+}
+
+
+/* Set offset and CRC fields in the response for a 'command' message. */
+static void cmd_response_offset_and_crc_set(nrf_dfu_response_t * const p_res)
+{
+    ASSERT(p_res);
+
+    /* Copy the CRC and offset of the init packet. */
+    p_res->crc.offset = s_dfu_settings.progress.command_offset;
+    p_res->crc.crc    = s_dfu_settings.progress.command_crc;
+}
+
+
+static void on_cmd_obj_select_request(nrf_dfu_request_t const * p_req, nrf_dfu_response_t * p_res)
+{
+    UNUSED_PARAMETER(p_req);
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_SELECT (command)");
+
+    p_res->select.max_size = INIT_COMMAND_MAX_SIZE;
+    cmd_response_offset_and_crc_set(p_res);
+}
+
+
+static void on_cmd_obj_create_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    ASSERT(p_req);
+    ASSERT(p_res);
+
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_CREATE (command)");
+
+    m_observer(NRF_DFU_EVT_DFU_STARTED);
+
+    nrf_dfu_result_t ret_val = nrf_dfu_validation_init_cmd_create(p_req->create.object_size);
+    p_res->result = ext_err_code_handle(ret_val);
+}
+
+
+static void on_cmd_obj_write_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    ASSERT(p_req);
+    ASSERT(p_req->write.p_data);
+    ASSERT(p_req->write.len);
+    ASSERT(p_res);
+
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_WRITE (command)");
+
+    nrf_dfu_result_t ret_val;
+
+    ret_val = nrf_dfu_validation_init_cmd_append(p_req->write.p_data, p_req->write.len);
+    p_res->result = ext_err_code_handle(ret_val);
+
+    /* Update response. This is only used when the PRN is triggered and the 'write' message
+     * is answered with a CRC message and these field are copied into the response. */
+    cmd_response_offset_and_crc_set(p_res);
+
+    /* If a callback to free the request payload buffer was provided, invoke it now. */
+    if (p_req->callback.write)
+    {
+        p_req->callback.write((void*)p_req->write.p_data);
+    }
+}
+
+
+static void on_cmd_obj_execute_request(nrf_dfu_request_t const * p_req, nrf_dfu_response_t * p_res)
+{
+    ASSERT(p_req);
+    ASSERT(p_res);
+
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_EXECUTE (command)");
+
+    nrf_dfu_result_t ret_val;
+    ret_val = nrf_dfu_validation_init_cmd_execute(&m_firmware_start_addr, &m_firmware_size_req);
+    p_res->result = ext_err_code_handle(ret_val);
+
+    if (p_res->result == NRF_DFU_RES_CODE_SUCCESS)
+    {
+        if (nrf_dfu_settings_write_and_backup(NULL) == NRF_SUCCESS)
+        {
+            /* Setting DFU to initialized */
+            NRF_LOG_DEBUG("Writing valid init command to flash.");
+        }
+        else
+        {
+            p_res->result = NRF_DFU_RES_CODE_OPERATION_FAILED;
+        }
+    }
+}
+
+
+static void on_cmd_obj_crc_request(nrf_dfu_request_t const * p_req, nrf_dfu_response_t * p_res)
+{
+    UNUSED_PARAMETER(p_req);
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_CRC_GET (command)");
+
+    cmd_response_offset_and_crc_set(p_res);
+}
+
+
+/** @brief Function handling command requests from the transport layer.
+ *
+ * @param   p_req[in]       Pointer to the structure holding the DFU request.
+ * @param   p_res[out]      Pointer to the structure holding the DFU response.
+ *
+ * @retval NRF_SUCCESS      If the command request was executed successfully.
+ *                          Any other error code indicates that the data request
+ *                          could not be handled.
+ */
+static void nrf_dfu_command_req(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    ASSERT(p_req);
+    ASSERT(p_res);
+
+    switch (p_req->request)
+    {
+        case NRF_DFU_OP_OBJECT_CREATE:
+        {
+            on_cmd_obj_create_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_CRC_GET:
+        {
+            on_cmd_obj_crc_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_WRITE:
+        {
+            on_cmd_obj_write_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_EXECUTE:
+        {
+            on_cmd_obj_execute_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_SELECT:
+        {
+            on_cmd_obj_select_request(p_req, p_res);
+        } break;
+
+        default:
+        {
+            ASSERT(false);
+        } break;
+    }
+}
+
+
+static void on_data_obj_select_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_SELECT (data)");
+
+    p_res->select.crc    = s_dfu_settings.progress.firmware_image_crc;
+    p_res->select.offset = s_dfu_settings.progress.firmware_image_offset;
+
+    p_res->select.max_size = DATA_OBJECT_MAX_SIZE;
+
+    NRF_LOG_DEBUG("crc = 0x%x, offset = 0x%x, max_size = 0x%x",
+                  p_res->select.crc,
+                  p_res->select.offset,
+                  p_res->select.max_size);
+}
+
+
+static void on_data_obj_create_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_CREATE (data)");
+
+    if (!nrf_dfu_validation_init_cmd_present())
+    {
+        /* Can't accept data because DFU isn't initialized by init command. */
+        NRF_LOG_ERROR("Cannot create data object without valid init command");
+        p_res->result = NRF_DFU_RES_CODE_OPERATION_NOT_PERMITTED;
+        return;
+    }
+
+    if (p_req->create.object_size == 0)
+    {
+        NRF_LOG_ERROR("Object size cannot be 0.")
+        p_res->result = NRF_DFU_RES_CODE_INVALID_PARAMETER;
+        return;
+    }
+
+    if (  ((p_req->create.object_size & (CODE_PAGE_SIZE - 1)) != 0)
+        && (s_dfu_settings.progress.firmware_image_offset_last + p_req->create.object_size != m_firmware_size_req))
+    {
+        NRF_LOG_ERROR("Object size must be page aligned");
+        p_res->result = NRF_DFU_RES_CODE_INVALID_PARAMETER;
+        return;
+    }
+
+    if (p_req->create.object_size > DATA_OBJECT_MAX_SIZE)
+    {
+        /* It is impossible to handle the command because the size is too large */
+        NRF_LOG_ERROR("Invalid size for object (too large)");
+        p_res->result = NRF_DFU_RES_CODE_INSUFFICIENT_RESOURCES;
+        return;
+    }
+
+    if ((s_dfu_settings.progress.firmware_image_offset_last + p_req->create.object_size) >
+        m_firmware_size_req)
+    {
+        NRF_LOG_ERROR("Creating the object with size 0x%08x would overflow firmware size. "
+                      "Offset is 0x%08x and firmware size is 0x%08x.",
+                      p_req->create.object_size,
+                      s_dfu_settings.progress.firmware_image_offset_last,
+                      m_firmware_size_req);
+
+        p_res->result = NRF_DFU_RES_CODE_OPERATION_NOT_PERMITTED;
+        return;
+    }
+
+    s_dfu_settings.progress.data_object_size      = p_req->create.object_size;
+    s_dfu_settings.progress.firmware_image_crc    = s_dfu_settings.progress.firmware_image_crc_last;
+    s_dfu_settings.progress.firmware_image_offset = s_dfu_settings.progress.firmware_image_offset_last;
+    s_dfu_settings.write_offset                   = s_dfu_settings.progress.firmware_image_offset_last;
+
+    /* Erase the page we're at. */
+    if (nrf_dfu_flash_erase((m_firmware_start_addr + s_dfu_settings.progress.firmware_image_offset),
+                            CEIL_DIV(p_req->create.object_size, CODE_PAGE_SIZE), NULL) != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Erase operation failed");
+        p_res->result = NRF_DFU_RES_CODE_INVALID_OBJECT;
+        return;
+    }
+
+    NRF_LOG_DEBUG("Creating object with size: %d. Offset: 0x%08x, CRC: 0x%08x",
+                 s_dfu_settings.progress.data_object_size,
+                 s_dfu_settings.progress.firmware_image_offset,
+                 s_dfu_settings.progress.firmware_image_crc);
+}
+
+
+static void on_data_obj_write_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_WRITE (data)");
+
+    if (!nrf_dfu_validation_init_cmd_present())
+    {
+        /* Can't accept data because DFU isn't initialized by init command. */
+        p_res->result = NRF_DFU_RES_CODE_OPERATION_NOT_PERMITTED;
+        return;
+    }
+
+    uint32_t const data_object_offset = s_dfu_settings.progress.firmware_image_offset -
+                                        s_dfu_settings.progress.firmware_image_offset_last;
+
+    if ((p_req->write.len + data_object_offset) > s_dfu_settings.progress.data_object_size)
+    {
+        /* Can't accept data because too much data has been received. */
+        NRF_LOG_ERROR("Write request too long");
+        p_res->result = NRF_DFU_RES_CODE_INVALID_PARAMETER;
+        return;
+    }
+
+    uint32_t const write_addr = m_firmware_start_addr + s_dfu_settings.write_offset;
+    /* CRC must be calculated before handing off the data to fstorage because the data is
+     * freed on write completion.
+     */
+    uint32_t const next_crc =
+        crc32_compute(p_req->write.p_data, p_req->write.len, &s_dfu_settings.progress.firmware_image_crc);
+
+    ASSERT(p_req->callback.write);
+
+    ret_code_t ret =
+        nrf_dfu_flash_store(write_addr, p_req->write.p_data, p_req->write.len, p_req->callback.write);
+
+    if (ret != NRF_SUCCESS)
+    {
+        /* When nrf_dfu_flash_store() fails because there is no space in the queue,
+         * stop processing the request so that the peer can detect a CRC error
+         * and retransmit this object. Remember to manually free the buffer !
+         */
+        p_req->callback.write((void*)p_req->write.p_data);
+        return;
+    }
+
+    /* Update the CRC of the firmware image. */
+    s_dfu_settings.write_offset                   += p_req->write.len;
+    s_dfu_settings.progress.firmware_image_offset += p_req->write.len;
+    s_dfu_settings.progress.firmware_image_crc     = next_crc;
+
+    /* This is only used when the PRN is triggered and the 'write' message
+     * is answered with a CRC message and these field are copied into the response.
+     */
+    p_res->write.crc    = s_dfu_settings.progress.firmware_image_crc;
+    p_res->write.offset = s_dfu_settings.progress.firmware_image_offset;
+}
+
+
+static void on_data_obj_crc_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_CRC_GET (data)");
+    NRF_LOG_DEBUG("Offset:%d, CRC:0x%08x",
+                 s_dfu_settings.progress.firmware_image_offset,
+                 s_dfu_settings.progress.firmware_image_crc);
+
+    p_res->crc.crc    = s_dfu_settings.progress.firmware_image_crc;
+    p_res->crc.offset = s_dfu_settings.progress.firmware_image_offset;
+}
+
+
+static void on_data_obj_execute_request_sched(void * p_evt, uint16_t event_length)
+{
+    UNUSED_PARAMETER(event_length);
+
+    ret_code_t          ret;
+    nrf_dfu_request_t * p_req = (nrf_dfu_request_t *)(p_evt);
+
+    /* Wait for all buffers to be written in flash. */
+    if (nrf_fstorage_is_busy(NULL))
+    {
+        ret = app_sched_event_put(p_req, sizeof(nrf_dfu_request_t), on_data_obj_execute_request_sched);
+        if (ret != NRF_SUCCESS)
+        {
+            NRF_LOG_ERROR("Failed to schedule object execute: 0x%x.", ret);
+        }
+        return;
+    }
+
+    nrf_dfu_response_t res =
+    {
+        .request = NRF_DFU_OP_OBJECT_EXECUTE,
+    };
+
+    if (s_dfu_settings.progress.firmware_image_offset == m_firmware_size_req)
+    {
+        NRF_LOG_DEBUG("Whole firmware image received. Postvalidating.");
+
+        #if NRF_DFU_IN_APP
+        res.result = nrf_dfu_validation_post_data_execute(m_firmware_start_addr, m_firmware_size_req);
+        #else
+        res.result = nrf_dfu_validation_activation_prepare(m_firmware_start_addr, m_firmware_size_req);
+        #endif
+
+        res.result = ext_err_code_handle(res.result);
+
+        /* Provide response to transport */
+        p_req->callback.response(&res, p_req->p_context);
+
+        ret = nrf_dfu_settings_write_and_backup((nrf_dfu_flash_callback_t)on_dfu_complete);
+        UNUSED_RETURN_VALUE(ret);
+    }
+    else
+    {
+        res.result = NRF_DFU_RES_CODE_SUCCESS;
+
+        /* Provide response to transport */
+        p_req->callback.response(&res, p_req->p_context);
+
+        if (NRF_DFU_SAVE_PROGRESS_IN_FLASH)
+        {
+            /* Allowing skipping settings backup to save time and flash wear. */
+            ret = nrf_dfu_settings_write_and_backup(NULL);
+            UNUSED_RETURN_VALUE(ret);
+        }
+    }
+
+    NRF_LOG_DEBUG("Request handling complete. Result: 0x%x", res.result);
+}
+
+
+static bool on_data_obj_execute_request(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    NRF_LOG_DEBUG("Handle NRF_DFU_OP_OBJECT_EXECUTE (data)");
+
+    uint32_t const data_object_size = s_dfu_settings.progress.firmware_image_offset -
+                                      s_dfu_settings.progress.firmware_image_offset_last;
+
+    if (s_dfu_settings.progress.data_object_size != data_object_size)
+    {
+        /* The size of the written object was not as expected. */
+        NRF_LOG_ERROR("Invalid data. expected: %d, got: %d",
+                      s_dfu_settings.progress.data_object_size,
+                      data_object_size);
+
+        p_res->result = NRF_DFU_RES_CODE_OPERATION_NOT_PERMITTED;
+        return true;
+    }
+
+    /* Update the offset and crc values for the last object written. */
+    s_dfu_settings.progress.data_object_size           = 0;
+    s_dfu_settings.progress.firmware_image_crc_last    = s_dfu_settings.progress.firmware_image_crc;
+    s_dfu_settings.progress.firmware_image_offset_last = s_dfu_settings.progress.firmware_image_offset;
+
+    on_data_obj_execute_request_sched(p_req, 0);
+
+    m_observer(NRF_DFU_EVT_OBJECT_RECEIVED);
+
+    return false;
+}
+
+
+static bool nrf_dfu_data_req(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    ASSERT(p_req);
+    ASSERT(p_res);
+
+    bool response_ready = true;
+
+    switch (p_req->request)
+    {
+        case NRF_DFU_OP_OBJECT_CREATE:
+        {
+            on_data_obj_create_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_WRITE:
+        {
+            on_data_obj_write_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_CRC_GET:
+        {
+            on_data_obj_crc_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_EXECUTE:
+        {
+            response_ready = on_data_obj_execute_request(p_req, p_res);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_SELECT:
+        {
+            on_data_obj_select_request(p_req, p_res);
+        } break;
+
+        default:
+        {
+            ASSERT(false);
+        } break;
+    }
+
+    return response_ready;
+}
+
+
+/**@brief Function for handling requests to manipulate data or command objects.
+ *
+ * @param[in]  p_req    Request.
+ * @param[out] p_res    Response.
+ *
+ * @return  Whether response is ready to be sent.
+ */
+static bool nrf_dfu_obj_op(nrf_dfu_request_t * p_req, nrf_dfu_response_t * p_res)
+{
+    /* Keep track of the current object type since write and execute requests don't contain it. */
+    static nrf_dfu_obj_type_t current_object = NRF_DFU_OBJ_TYPE_COMMAND;
+
+    if (    (p_req->request == NRF_DFU_OP_OBJECT_SELECT)
+        ||  (p_req->request == NRF_DFU_OP_OBJECT_CREATE))
+    {
+        STATIC_ASSERT(offsetof(nrf_dfu_request_select_t, object_type) ==
+                      offsetof(nrf_dfu_request_create_t, object_type),
+                      "Wrong object_type offset!");
+
+        current_object = (nrf_dfu_obj_type_t)(p_req->select.object_type);
+    }
+
+    bool response_ready = true;
+
+    switch (current_object)
+    {
+        case NRF_DFU_OBJ_TYPE_COMMAND:
+            nrf_dfu_command_req(p_req, p_res);
+            break;
+
+        case NRF_DFU_OBJ_TYPE_DATA:
+            response_ready = nrf_dfu_data_req(p_req, p_res);
+            break;
+
+        default:
+            /* The select request had an invalid object type. */
+            NRF_LOG_ERROR("Invalid object type in request.");
+            current_object = NRF_DFU_OBJ_TYPE_INVALID;
+            p_res->result  = NRF_DFU_RES_CODE_INVALID_OBJECT;
+            break;
+    }
+
+    return response_ready;
+}
+
+
+static void nrf_dfu_req_handler_req_process(nrf_dfu_request_t * p_req)
+{
+    ASSERT(p_req->callback.response);
+
+    bool response_ready = true;
+
+    /* The request handlers assume these values to be set. */
+    nrf_dfu_response_t response =
+    {
+        .request = p_req->request,
+        .result  = NRF_DFU_RES_CODE_SUCCESS,
+    };
+
+
+    switch (p_req->request)
+    {
+#if !NRF_DFU_PROTOCOL_REDUCED
+        case NRF_DFU_OP_PROTOCOL_VERSION:
+        {
+            on_protocol_version_request(p_req, &response);
+        } break;
+
+        case NRF_DFU_OP_HARDWARE_VERSION:
+        {
+            on_hw_version_request(p_req, &response);
+        } break;
+
+        case NRF_DFU_OP_FIRMWARE_VERSION:
+        {
+            on_fw_version_request(p_req, &response);
+        } break;
+
+        case NRF_DFU_OP_PING:
+        {
+            on_ping_request(p_req, &response);
+        } break;
+
+        case NRF_DFU_OP_MTU_GET:
+        {
+            on_mtu_get_request(p_req, &response);
+        } break;
+#endif
+        case NRF_DFU_OP_RECEIPT_NOTIF_SET:
+        {
+            on_prn_set_request(p_req, &response);
+        } break;
+
+        case NRF_DFU_OP_ABORT:
+        {
+            on_abort_request(p_req, &response);
+        } break;
+
+        case NRF_DFU_OP_OBJECT_CREATE:
+            /* Restart the inactivity timer on CREATE messages. */
+            /* Fallthrough. */
+        case NRF_DFU_OP_OBJECT_SELECT:
+        case NRF_DFU_OP_OBJECT_WRITE:
+        case NRF_DFU_OP_OBJECT_EXECUTE:
+        case NRF_DFU_OP_CRC_GET:
+        {
+            response_ready = nrf_dfu_obj_op(p_req, &response);
+        } break;
+
+        default:
+            NRF_LOG_INFO("Invalid opcode received: 0x%x.", p_req->request);
+            response.result = NRF_DFU_RES_CODE_OP_CODE_NOT_SUPPORTED;
+            break;
+    }
+
+    if (response_ready)
+    {
+        NRF_LOG_DEBUG("Request handling complete. Result: 0x%x", response.result);
+
+        p_req->callback.response(&response, p_req->p_context);
+
+        if (response.result != NRF_DFU_RES_CODE_SUCCESS)
+        {
+            m_observer(NRF_DFU_EVT_DFU_FAILED);
+        }
+    }
+}
+
+
+static void nrf_dfu_req_handler_req(void * p_evt, uint16_t event_length)
+{
+    nrf_dfu_request_t * p_req = (nrf_dfu_request_t *)(p_evt);
+    nrf_dfu_req_handler_req_process(p_req);
+}
+
+
+ret_code_t nrf_dfu_req_handler_on_req(nrf_dfu_request_t * p_req)
+{
+    ret_code_t ret;
+
+    if (p_req->callback.response == NULL)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+    ret = app_sched_event_put(p_req, sizeof(nrf_dfu_request_t), nrf_dfu_req_handler_req);
+    if (ret != NRF_SUCCESS)
+    {
+        NRF_LOG_WARNING("Scheduler ran out of space!");
+    }
+
+    return ret;
+}
+
+
+ret_code_t nrf_dfu_req_handler_init(nrf_dfu_observer_t observer)
+{
+    ret_code_t       ret_val;
+    nrf_dfu_result_t result;
+
+    if (observer == NULL)
+    {
+        return NRF_ERROR_INVALID_PARAM;
+    }
+
+#if defined(BLE_STACK_SUPPORT_REQD) || defined(ANT_STACK_SUPPORT_REQD)
+    ret_val  = nrf_dfu_flash_init(true);
+#else
+    ret_val = nrf_dfu_flash_init(false);
+#endif
+    if (ret_val != NRF_SUCCESS)
+    {
+        return ret_val;
+    }
+
+    nrf_dfu_validation_init();
+    if (nrf_dfu_validation_init_cmd_present())
+    {
+        /* Execute a previously received init packed. Subsequent executes will have no effect. */
+        result = nrf_dfu_validation_init_cmd_execute(&m_firmware_start_addr, &m_firmware_size_req);
+        if (result != NRF_DFU_RES_CODE_SUCCESS)
+        {
+            /* Init packet in flash is not valid! */
+            return NRF_ERROR_INTERNAL;
+        }
+    }
+
+    m_observer = observer;
+
+    /* Initialize extended error handling with "No error" as the most recent error. */
+    result = ext_error_set(NRF_DFU_EXT_ERROR_NO_ERROR);
+    UNUSED_RETURN_VALUE(result);
+
+    return NRF_SUCCESS;
+}

components/libraries/bootloader/dfu/nrf_dfu_req_handler.h

@@ -0,0 +1,345 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 sdk_nrf_dfu_req_handler Request handling
+ * @{
+ * @ingroup  nrf_dfu
+ */
+
+#ifndef NRF_DFU_REQ_HANDLER_H__
+#define NRF_DFU_REQ_HANDLER_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "app_util_platform.h"
+#include "nrf_dfu_flash.h"
+#include "nrf_dfu_types.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+ANON_UNIONS_ENABLE;
+
+/**
+ * @brief DFU object types.
+ */
+typedef enum
+{
+    NRF_DFU_OBJ_TYPE_INVALID,                   //!< Invalid object type.
+    NRF_DFU_OBJ_TYPE_COMMAND,                   //!< Command object.
+    NRF_DFU_OBJ_TYPE_DATA,                      //!< Data object.
+} nrf_dfu_obj_type_t;
+
+/**
+ * @brief DFU protocol operation.
+ */
+typedef enum
+{
+    NRF_DFU_OP_PROTOCOL_VERSION     = 0x00,     //!< Retrieve protocol version.
+    NRF_DFU_OP_OBJECT_CREATE        = 0x01,     //!< Create selected object.
+    NRF_DFU_OP_RECEIPT_NOTIF_SET    = 0x02,     //!< Set receipt notification.
+    NRF_DFU_OP_CRC_GET              = 0x03,     //!< Request CRC of selected object.
+    NRF_DFU_OP_OBJECT_EXECUTE       = 0x04,     //!< Execute selected object.
+    NRF_DFU_OP_OBJECT_SELECT        = 0x06,     //!< Select object.
+    NRF_DFU_OP_MTU_GET              = 0x07,     //!< Retrieve MTU size.
+    NRF_DFU_OP_OBJECT_WRITE         = 0x08,     //!< Write selected object.
+    NRF_DFU_OP_PING                 = 0x09,     //!< Ping.
+    NRF_DFU_OP_HARDWARE_VERSION     = 0x0A,     //!< Retrieve hardware version.
+    NRF_DFU_OP_FIRMWARE_VERSION     = 0x0B,     //!< Retrieve firmware version.
+    NRF_DFU_OP_ABORT                = 0x0C,     //!< Abort the DFU procedure.
+    NRF_DFU_OP_RESPONSE             = 0x60,     //!< Response.
+    NRF_DFU_OP_INVALID              = 0xFF,
+} nrf_dfu_op_t;
+
+/**
+ * @brief DFU operation result code.
+ */
+typedef enum
+{
+    NRF_DFU_RES_CODE_INVALID                 = 0x00,    //!< Invalid opcode.
+    NRF_DFU_RES_CODE_SUCCESS                 = 0x01,    //!< Operation successful.
+    NRF_DFU_RES_CODE_OP_CODE_NOT_SUPPORTED   = 0x02,    //!< Opcode not supported.
+    NRF_DFU_RES_CODE_INVALID_PARAMETER       = 0x03,    //!< Missing or invalid parameter value.
+    NRF_DFU_RES_CODE_INSUFFICIENT_RESOURCES  = 0x04,    //!< Not enough memory for the data object.
+    NRF_DFU_RES_CODE_INVALID_OBJECT          = 0x05,    //!< Data object does not match the firmware and hardware requirements, the signature is wrong, or parsing the command failed.
+    NRF_DFU_RES_CODE_UNSUPPORTED_TYPE        = 0x07,    //!< Not a valid object type for a Create request.
+    NRF_DFU_RES_CODE_OPERATION_NOT_PERMITTED = 0x08,    //!< The state of the DFU process does not allow this operation.
+    NRF_DFU_RES_CODE_OPERATION_FAILED        = 0x0A,    //!< Operation failed.
+    NRF_DFU_RES_CODE_EXT_ERROR               = 0x0B,    //!< Extended error. The next byte of the response contains the error code of the extended error (see @ref nrf_dfu_ext_error_code_t.
+} nrf_dfu_result_t;
+
+typedef enum
+{
+    NRF_DFU_FIRMWARE_TYPE_SOFTDEVICE    = 0x00,
+    NRF_DFU_FIRMWARE_TYPE_APPLICATION   = 0x01,
+    NRF_DFU_FIRMWARE_TYPE_BOOTLOADER    = 0x02,
+    NRF_DFU_FIRMWARE_TYPE_UNKNOWN       = 0xFF,
+} nrf_dfu_firmware_type_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_PROTOCOL_VERSION response details.
+ */
+typedef struct
+{
+    uint8_t version;                    //!< Protocol version.
+} nrf_dfu_response_protocol_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_HARDWARE_VERSION response details.
+ */
+typedef struct
+{
+    uint32_t part;                      //!< Hardware part, from FICR register.
+    uint32_t variant;                   //!< Hardware variant, from FICR register.
+    struct
+    {
+        uint32_t rom_size;              //!< ROM size, in bytes.
+        uint32_t ram_size;              //!< RAM size, in bytes.
+        uint32_t rom_page_size;         //!< ROM flash page size, in bytes.
+    } memory;
+} nrf_dfu_response_hardware_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_FIRMWARE_VERSION response details.
+ */
+typedef struct
+{
+    nrf_dfu_firmware_type_t type;       //!< Firmware type.
+    uint32_t                version;    //!< Firmware version.
+    uint32_t                addr;       //!< Firmware address in flash.
+    uint32_t                len;        //!< Firmware length in bytes.
+} nrf_dfu_response_firmware_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_OBJECT_SELECT response details.
+ */
+typedef struct
+{
+    uint32_t offset;                    //!< Current offset.
+    uint32_t crc;                       //!< Current CRC.
+    uint32_t max_size;                  //!< Maximum size of selected object.
+} nrf_dfu_response_select_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_OBJECT_CREATE response details.
+ */
+typedef struct
+{
+    uint32_t offset;                    //!< Current offset
+    uint32_t crc;                       //!< Current CRC.
+} nrf_dfu_response_create_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_OBJECT_WRITE response details.
+ */
+typedef struct
+{
+    uint32_t offset;                    //!< Used only when packet receipt notification is used.
+    uint32_t crc;                       //!< Used only when packet receipt notification is used.
+} nrf_dfu_response_write_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_CRC_GET response details.
+ */
+typedef struct
+{
+    uint32_t offset;                    //!< Current offset.
+    uint32_t crc;                       //!< Current CRC.
+} nrf_dfu_response_crc_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_PING response details.
+ */
+typedef struct
+{
+    uint8_t id;                         //!< The received ID which is echoed back.
+} nrf_dfu_response_ping_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_MTU_GET response details.
+ */
+typedef struct
+{
+    uint16_t size;                      //!< The MTU size as specified by the local transport.
+} nrf_dfu_response_mtu_t;
+
+/**
+ * @brief DFU response message.
+ */
+typedef struct
+{
+    nrf_dfu_op_t     request;                      //!< Requested operation.
+    nrf_dfu_result_t result;                       //!< Result of the operation.
+    union
+    {
+        nrf_dfu_response_protocol_t protocol;      //!< Protocol version response.
+        nrf_dfu_response_hardware_t hardware;      //!< Hardware version response.
+        nrf_dfu_response_firmware_t firmware;      //!< Firmware version response.
+        nrf_dfu_response_select_t   select;        //!< Select object response..
+        nrf_dfu_response_create_t   create;        //!< Create object response..
+        nrf_dfu_response_write_t    write;         //!< Write object response.
+        nrf_dfu_response_crc_t      crc;           //!< CRC response.
+        nrf_dfu_response_ping_t     ping;          //!< Ping response.
+        nrf_dfu_response_mtu_t      mtu;           //!< MTU response.
+    };
+} nrf_dfu_response_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_FIRMWARE_VERSION request details.
+ */
+typedef struct
+{
+    uint8_t image_number;  //!< Index of the firmware.
+} nrf_dfu_request_firmware_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_OBJECT_SELECT request details.
+ */
+typedef struct
+{
+    uint32_t object_type;  //!< Object type. See @ref nrf_dfu_obj_type_t.
+} nrf_dfu_request_select_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_OBJECT_CREATE request details.
+ */
+typedef struct
+{
+    uint32_t object_type;  //!< Object type. See @ref nrf_dfu_obj_type_t.
+    uint32_t object_size;  //!< Object size in bytes.
+} nrf_dfu_request_create_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_OBJECT_WRITE request details.
+ */
+typedef struct
+{
+    uint8_t  const * p_data; //!< Data.
+    uint16_t         len;    //!< Length of data in @ref nrf_dfu_request_write_t::p_data.
+} nrf_dfu_request_write_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_PING request details.
+ */
+typedef struct
+{
+    uint8_t id;             //!< Ping ID that will be returned in response.
+} nrf_dfu_request_ping_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_MTU_GET request details.
+ */
+typedef struct
+{
+    uint16_t size;          //!< Transport MTU size in bytes.
+} nrf_dfu_request_mtu_t;
+
+/**
+ * @brief @ref NRF_DFU_OP_RECEIPT_NOTIF_SET request details.
+ */
+typedef struct
+{
+    uint32_t target;        //!< Target PRN.
+} nrf_dfu_request_prn_t;
+
+
+typedef void (*nrf_dfu_response_callback_t)(nrf_dfu_response_t * p_res, void * p_context);
+
+/**
+ *@brief DFU request.
+ */
+typedef struct
+{
+    nrf_dfu_op_t   request;     //!< Requested operation.
+    void         * p_context;
+    struct
+    {
+        nrf_dfu_response_callback_t response; //!< Callback to call to send the response.
+        nrf_dfu_flash_callback_t    write;
+    } callback;
+    union
+    {
+        nrf_dfu_request_firmware_t firmware;    //!< Firmware version request.
+        nrf_dfu_request_select_t   select;      //!< Select object request.
+        nrf_dfu_request_create_t   create;      //!< Create object request.
+        nrf_dfu_request_write_t    write;       //!< Write object request.
+        nrf_dfu_request_ping_t     ping;        //!< Ping.
+        nrf_dfu_request_mtu_t      mtu;         //!< MTU size request.
+        nrf_dfu_request_prn_t      prn;         //!< Set receipt notification request.
+    };
+} nrf_dfu_request_t;
+
+
+/**@brief  Function for initializing the request handling module.
+ *
+ * @param observer  Callback function for receiving notifications.
+ *
+ * @retval NRF_SUCCESS             If the operation was successful.
+ * @retval NRF_ERROR_INTERNAL      If the init packet in flash is not valid.
+ * @retval NRF_ERROR_INVALID_PARAM If observer is not provided.
+ */
+ret_code_t nrf_dfu_req_handler_init(nrf_dfu_observer_t observer);
+
+
+/**@brief  Function for scheduling processing of a DFU request.
+ *
+ * Requests are processed asynchronously by the scheduler.
+ *
+ * @param[in] p_req  Request to be handled. The response callback must be non-null.
+ *
+ * @retval NRF_SUCCESS              If the command request was executed successfully.
+ * @retval NRF_ERROR_NO_MEM         If the scheduler ran out of memory.
+ * @retval NRF_ERROR_INVALID_PARAM  If the response callback is NULL.
+ */
+ret_code_t nrf_dfu_req_handler_on_req(nrf_dfu_request_t * p_req);
+
+
+ANON_UNIONS_DISABLE;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_DFU_REQ_HANDLER_H__
+
+/** @} */

components/libraries/bootloader/dfu/nrf_dfu_settings.c

@@ -0,0 +1,433 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+
+#include "nrf_dfu_settings.h"
+#include <stddef.h>
+#include <string.h>
+#include "nrf_dfu_flash.h"
+#include "nrf_soc.h"
+#include "crc32.h"
+#include "nrf_nvmc.h"
+#include "sdk_config.h"
+
+
+#define DFU_SETTINGS_VERSION_OFFSET             (offsetof(nrf_dfu_settings_t, settings_version))                         //<! Offset in the settings struct where the settings version is located.
+#define DFU_SETTINGS_INIT_COMMAND_OFFSET        (offsetof(nrf_dfu_settings_t, init_command))                             //<! Offset in the settings struct where the InitCommand is located.
+#define DFU_SETTINGS_BOOT_VALIDATION_OFFSET     (offsetof(nrf_dfu_settings_t, boot_validation_crc))                      //<! Offset in the settings struct where the boot validation info is located.
+#define DFU_SETTINGS_BOOT_VALIDATION_SIZE       ((3 * sizeof(boot_validation_t)) + 4)
+#define DFU_SETTINGS_BOND_DATA_OFFSET_V1        (offsetof(nrf_dfu_settings_t, init_command) + INIT_COMMAND_MAX_SIZE_v1)  //<! Offset in the settings struct where the bond data was located in settings version 1.
+#define DFU_SETTINGS_ADV_NAME_OFFSET_V1         (offsetof(nrf_dfu_settings_t, init_command) + INIT_COMMAND_MAX_SIZE_v1 + NRF_DFU_PEER_DATA_LEN)  //<! Offset in the settings struct where the bond data was located in settings version 1.
+
+#define NRF_LOG_MODULE_NAME nrf_dfu_settings
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+
+/**@brief   This variable reserves a page in flash for bootloader settings
+ *          to ensure the linker doesn't place any code or variables at this location.
+ */
+#if defined (__CC_ARM )
+
+    uint8_t m_dfu_settings_buffer[BOOTLOADER_SETTINGS_PAGE_SIZE]
+        __attribute__((at(BOOTLOADER_SETTINGS_ADDRESS)))
+        __attribute__((used));
+
+#elif defined ( __GNUC__ ) || defined ( __SES_ARM )
+
+    uint8_t m_dfu_settings_buffer[BOOTLOADER_SETTINGS_PAGE_SIZE]
+        __attribute__((section(".bootloader_settings_page")))
+        __attribute__((used));
+
+#elif defined ( __ICCARM__ )
+
+    __no_init __root uint8_t m_dfu_settings_buffer[BOOTLOADER_SETTINGS_PAGE_SIZE]
+        @ BOOTLOADER_SETTINGS_ADDRESS;
+
+#else
+
+    #error Not a valid compiler/linker for m_dfu_settings placement.
+
+#endif // Compiler specific
+
+#if defined(NRF52_SERIES)
+
+/**@brief   This variable reserves a page in flash for MBR parameters
+ *          to ensure the linker doesn't place any code or variables at this location.
+ */
+#if defined ( __CC_ARM )
+
+    uint8_t m_mbr_params_page[NRF_MBR_PARAMS_PAGE_SIZE]
+        __attribute__((at(NRF_MBR_PARAMS_PAGE_ADDRESS)))
+        __attribute__((used));
+
+#elif defined ( __GNUC__ ) || defined ( __SES_ARM )
+
+    uint8_t m_mbr_params_page[NRF_MBR_PARAMS_PAGE_SIZE]
+        __attribute__((section(".mbr_params_page")))
+        __attribute__((used));
+
+#elif defined ( __ICCARM__ )
+
+    __no_init uint8_t m_mbr_params_page[NRF_MBR_PARAMS_PAGE_SIZE]
+        @ NRF_MBR_PARAMS_PAGE_ADDRESS;
+
+#else
+
+    #error Not a valid compiler/linker for m_mbr_params_page placement.
+
+#endif // Compiler specific
+
+uint8_t * mp_dfu_settings_backup_buffer = &m_mbr_params_page[0];
+
+
+#ifndef NRF_DFU_IN_APP
+#define NRF_DFU_IN_APP 0
+#endif
+
+
+#define UICR_PARAM_PAGE_ADDR 0x10001018
+
+#if !defined(BL_SETTINGS_ACCESS_ONLY) && !NRF_DFU_IN_APP
+/**@brief   This variable has the linker write the MBR parameters page address to the
+ *          UICR register. This value will be written in the HEX file and thus to the
+ *          UICR when the bootloader is flashed into the chip.
+ */
+#if defined ( __CC_ARM )
+
+    uint32_t const m_uicr_mbr_params_page_address
+        __attribute__((at(UICR_PARAM_PAGE_ADDR))) = NRF_MBR_PARAMS_PAGE_ADDRESS;
+
+#elif defined ( __GNUC__ ) || defined ( __SES_ARM )
+
+    uint32_t const m_uicr_mbr_params_page_address
+        __attribute__ ((section(".uicr_mbr_params_page")))
+        __attribute__ ((used)) = NRF_MBR_PARAMS_PAGE_ADDRESS;
+
+#elif defined ( __ICCARM__ )
+
+    __root uint32_t const m_uicr_mbr_params_page_address
+        @ UICR_PARAM_PAGE_ADDR = NRF_MBR_PARAMS_PAGE_ADDRESS;
+
+#else
+
+    #error Not a valid compiler/linker for m_mbr_params_page placement.
+
+#endif // Compiler specific
+#endif // #ifndef BL_SETTINGS_ACCESS_ONLY
+#endif // #if defined( NRF52_SERIES )
+
+nrf_dfu_settings_t s_dfu_settings;
+
+static uint32_t settings_crc_get(nrf_dfu_settings_t const * p_settings)
+{
+    ASSERT(offsetof(nrf_dfu_settings_t, crc) == 0);
+
+    // The crc is calculated from the s_dfu_settings struct, except the crc itself, the init command, bond data, and boot validation.
+    return crc32_compute((uint8_t*)(p_settings) + 4, DFU_SETTINGS_INIT_COMMAND_OFFSET - 4, NULL);
+}
+
+
+static bool crc_ok(nrf_dfu_settings_t const * p_settings)
+{
+    if (p_settings->crc != 0xFFFFFFFF)
+    {
+        // CRC is set. Content must be valid
+        uint32_t crc = settings_crc_get(p_settings);
+        if (crc == p_settings->crc)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+static uint32_t boot_validation_crc(nrf_dfu_settings_t const * p_settings)
+{
+    return crc32_compute((const uint8_t *)&p_settings->boot_validation_softdevice,
+                          DFU_SETTINGS_BOOT_VALIDATION_SIZE - 4,
+                          NULL);
+}
+
+
+static bool boot_validation_crc_ok(nrf_dfu_settings_t const * p_settings)
+{
+    return (boot_validation_crc(p_settings) == p_settings->boot_validation_crc);
+}
+
+
+static bool settings_crc_ok(void)
+{
+    nrf_dfu_settings_t const * p_settings = (nrf_dfu_settings_t const *)m_dfu_settings_buffer;
+    return crc_ok(p_settings);
+}
+
+
+static bool settings_backup_crc_ok(void)
+{
+    nrf_dfu_settings_t const * p_settings = (nrf_dfu_settings_t const *)mp_dfu_settings_backup_buffer;
+    return crc_ok(p_settings) && ((p_settings->settings_version == 1) || boot_validation_crc_ok(p_settings));
+}
+
+#define REGION_COPY_BY_MEMBER(start_member, end_member, p_dst_addr)        \
+    memcpy(p_dst_addr                    + offsetof(nrf_dfu_settings_t, start_member), \
+           mp_dfu_settings_backup_buffer + offsetof(nrf_dfu_settings_t, start_member), \
+           offsetof(nrf_dfu_settings_t, end_member) - offsetof(nrf_dfu_settings_t, start_member))
+
+
+static void settings_forbidden_parts_copy_from_backup(uint8_t * p_dst_addr)
+{
+#if NRF_DFU_IN_APP || NRF_BL_DFU_ALLOW_UPDATE_FROM_APP
+    REGION_COPY_BY_MEMBER(settings_version, bank_current, p_dst_addr);
+    REGION_COPY_BY_MEMBER(bank_0, write_offset, p_dst_addr);
+    REGION_COPY_BY_MEMBER(sd_size, progress, p_dst_addr);
+    REGION_COPY_BY_MEMBER(boot_validation_crc, peer_data, p_dst_addr);
+#else
+    REGION_COPY_BY_MEMBER(settings_version, enter_buttonless_dfu, p_dst_addr);
+    REGION_COPY_BY_MEMBER(init_command, peer_data, p_dst_addr);
+#endif
+}
+
+void nrf_dfu_settings_reinit(void)
+{
+    bool settings_valid        = settings_crc_ok();
+    bool settings_backup_valid = settings_backup_crc_ok();
+
+    if (settings_valid)
+    {
+        NRF_LOG_DEBUG("Using settings page.");
+        memcpy(&s_dfu_settings, m_dfu_settings_buffer, sizeof(nrf_dfu_settings_t));
+        if (settings_backup_valid)
+        {
+            NRF_LOG_DEBUG("Copying forbidden parts from backup page.");
+            settings_forbidden_parts_copy_from_backup((uint8_t *)&s_dfu_settings);
+        }
+    }
+    else if (settings_backup_valid)
+    {
+        NRF_LOG_INFO("Restoring settings from backup since the settings page contents are "
+                     "invalid (CRC error).");
+        memcpy(&s_dfu_settings,
+               mp_dfu_settings_backup_buffer,
+               sizeof(nrf_dfu_settings_t));
+    }
+    else
+    {
+        NRF_LOG_WARNING("Resetting bootloader settings since neither the settings page nor the "
+                        "backup are valid (CRC error).");
+        memset(&s_dfu_settings, 0x00, sizeof(nrf_dfu_settings_t));
+        s_dfu_settings.settings_version = NRF_DFU_SETTINGS_VERSION;
+    }
+
+    if (NRF_DFU_SETTINGS_COMPATIBILITY_MODE && !NRF_DFU_IN_APP && (s_dfu_settings.settings_version == 1))
+    {
+        NRF_LOG_INFO("Old settings page detected. Upgrading info.");
+
+        // Old version. Translate.
+        memcpy(&s_dfu_settings.peer_data, (uint8_t *)&s_dfu_settings + DFU_SETTINGS_BOND_DATA_OFFSET_V1, NRF_DFU_PEER_DATA_LEN);
+        memcpy(&s_dfu_settings.adv_name,  (uint8_t *)&s_dfu_settings + DFU_SETTINGS_ADV_NAME_OFFSET_V1,  NRF_DFU_ADV_NAME_LEN);
+
+        // Initialize with defaults.
+        s_dfu_settings.boot_validation_softdevice.type = NO_VALIDATION;
+        s_dfu_settings.boot_validation_app.type        = VALIDATE_CRC;
+        s_dfu_settings.boot_validation_bootloader.type = NO_VALIDATION;
+        memcpy(s_dfu_settings.boot_validation_app.bytes, &s_dfu_settings.bank_0.image_crc, sizeof(uint32_t));
+
+        s_dfu_settings.settings_version = NRF_DFU_SETTINGS_VERSION;
+    }
+
+    return;
+}
+
+ret_code_t nrf_dfu_settings_init(bool sd_irq_initialized)
+{
+    NRF_LOG_DEBUG("Calling nrf_dfu_settings_init()...");
+
+    ret_code_t err_code = nrf_dfu_flash_init(sd_irq_initialized);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_dfu_flash_init() failed with error: %x", err_code);
+        return NRF_ERROR_INTERNAL;
+    }
+
+    nrf_dfu_settings_reinit();
+
+    err_code = nrf_dfu_settings_write_and_backup(NULL);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("nrf_dfu_settings_write_and_backup() failed with error: %x", err_code);
+        return NRF_ERROR_INTERNAL;
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+static bool settings_forbidden_parts_equal_to_backup(uint8_t * p_compare_addr)
+{
+    nrf_dfu_settings_t temp_settings;
+    memcpy(&temp_settings, p_compare_addr, sizeof(nrf_dfu_settings_t));
+    settings_forbidden_parts_copy_from_backup((uint8_t *)&temp_settings);
+    return memcmp(&temp_settings, p_compare_addr, sizeof(nrf_dfu_settings_t)) == 0;
+}
+
+
+static ret_code_t settings_write(void                   * p_dst,
+                                 void const             * p_src,
+                                 nrf_dfu_flash_callback_t callback,
+                                 nrf_dfu_settings_t     * p_dfu_settings_buffer)
+{
+    ret_code_t err_code;
+
+    if (memcmp(p_dst, p_src, sizeof(nrf_dfu_settings_t)) == 0)
+    {
+        NRF_LOG_DEBUG("Destination settings are identical to source, write not needed. Skipping.");
+        if (callback != NULL)
+        {
+            callback(NULL);
+        }
+        return NRF_SUCCESS;
+    }
+
+    if (NRF_DFU_IN_APP && !settings_forbidden_parts_equal_to_backup((uint8_t *)&s_dfu_settings))
+    {
+        NRF_LOG_WARNING("Settings write aborted since it tries writing to forbidden settings.");
+        return NRF_ERROR_FORBIDDEN;
+    }
+
+    NRF_LOG_DEBUG("Writing settings...");
+    NRF_LOG_DEBUG("Erasing old settings at: 0x%08x", p_dst);
+
+    // Not setting the callback function because ERASE is required before STORE
+    // Only report completion on successful STORE.
+    err_code = nrf_dfu_flash_erase((uint32_t)p_dst, 1, NULL);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Could not erase the settings page!");
+        return NRF_ERROR_INTERNAL;
+    }
+
+    ASSERT(p_dfu_settings_buffer != NULL);
+    memcpy(p_dfu_settings_buffer, p_src, sizeof(nrf_dfu_settings_t));
+
+    err_code = nrf_dfu_flash_store((uint32_t)p_dst,
+                                   p_dfu_settings_buffer,
+                                   sizeof(nrf_dfu_settings_t),
+                                   callback);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Could not write the DFU settings page!");
+        return NRF_ERROR_INTERNAL;
+    }
+
+    return NRF_SUCCESS;
+}
+
+
+ret_code_t nrf_dfu_settings_write(nrf_dfu_flash_callback_t callback)
+{
+    static nrf_dfu_settings_t dfu_settings_buffer;
+    s_dfu_settings.crc = settings_crc_get(&s_dfu_settings);
+    s_dfu_settings.boot_validation_crc = boot_validation_crc(&s_dfu_settings);
+    return settings_write(m_dfu_settings_buffer,
+                          &s_dfu_settings,
+                          callback,
+                          &dfu_settings_buffer);
+}
+
+
+void settings_backup(nrf_dfu_flash_callback_t callback, void * p_src)
+{
+#if NRF_DFU_IN_APP
+    NRF_LOG_INFO("Settings backup not available from app.");
+#else
+    static nrf_dfu_settings_t dfu_settings_buffer;
+    NRF_LOG_INFO("Backing up settings page to address 0x%x.", mp_dfu_settings_backup_buffer);
+    ASSERT(crc_ok(p_src));
+    ret_code_t err_code = settings_write(mp_dfu_settings_backup_buffer,
+                                         p_src,
+                                         callback,
+                                         &dfu_settings_buffer);
+
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Could not perform backup of bootloader settings! Error: 0x%x", err_code);
+    }
+#endif
+}
+
+
+void nrf_dfu_settings_backup(nrf_dfu_flash_callback_t callback)
+{
+    settings_backup(callback, m_dfu_settings_buffer);
+}
+
+
+ret_code_t nrf_dfu_settings_write_and_backup(nrf_dfu_flash_callback_t callback)
+{
+#if NRF_DFU_IN_APP
+    ret_code_t err_code = nrf_dfu_settings_write(callback);
+#else
+    ret_code_t err_code = nrf_dfu_settings_write(NULL);
+    if (err_code == NRF_SUCCESS)
+    {
+        settings_backup(callback, &s_dfu_settings);
+    }
+#endif
+    return err_code;
+}
+
+
+__WEAK ret_code_t nrf_dfu_settings_additional_erase(void)
+{
+    NRF_LOG_WARNING("No additional data erased");
+    return NRF_SUCCESS;
+}
+
+
+void nrf_dfu_settings_progress_reset(void)
+{
+    memset(s_dfu_settings.init_command, 0xFF, INIT_COMMAND_MAX_SIZE); // Remove the last init command
+    memset(&s_dfu_settings.progress, 0, sizeof(dfu_progress_t));
+    s_dfu_settings.write_offset = 0;
+}

components/libraries/bootloader/dfu/nrf_dfu_settings.h

@@ -0,0 +1,212 @@
+/**
+ * Copyright (c) 2016 - 2021, 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 nrf_dfu_settings DFU settings
+ * @{
+ * @ingroup  nrf_dfu
+ */
+
+#ifndef NRF_DFU_SETTINGS_H__
+#define NRF_DFU_SETTINGS_H__
+
+#include <stdint.h>
+#include "nrf_dfu_types.h"
+#include "nrf_dfu_flash.h"
+#include "sdk_config.h"
+#include "sdk_errors.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**@brief   Global settings.
+ *
+ * @note Using this variable is not thread-safe.
+ *
+ */
+extern nrf_dfu_settings_t s_dfu_settings;
+
+
+/**@brief   Function for writing DFU settings to flash.
+ *
+ * @param[in]   callback    Pointer to a function that is called after completing the write operation.
+ *
+ * @retval  NRF_SUCCESS         If the write process was successfully initiated.
+ * @retval  NRF_ERROR_INTERNAL  If a flash error occurred.
+ */
+ret_code_t nrf_dfu_settings_write(nrf_dfu_flash_callback_t callback);
+
+
+/**@brief   Function for backing up the settings.
+ *
+ * This function copies the contents of the settings page (in flash) to a separate page (in flash).
+ * During @ref nrf_dfu_settings_init, the backup is restored if the original is invalid.
+ *
+ * @param[in]   callback    Pointer to a function that is called after completing the write operation.
+ */
+void nrf_dfu_settings_backup(nrf_dfu_flash_callback_t callback);
+
+
+/**@brief   Function for writing DFU settings to flash and to backup.
+ *
+ * This function first calls @ref nrf_dfu_settings_write and then @ref nrf_dfu_settings_backup.
+ *
+ * @param[in]   callback    Pointer to a function that is called after completing the write and backup operation.
+ *
+ * @retval  NRF_SUCCESS         If the write process was successfully initiated.
+ * @retval  NRF_ERROR_INTERNAL  If a flash error occurred during the first write.
+ */
+ret_code_t nrf_dfu_settings_write_and_backup(nrf_dfu_flash_callback_t callback);
+
+
+/**@brief   Function for initializing the DFU settings structure.
+ *
+ * Initializes the RAM structure from the flash contents.
+ * This function is called as part of @ref nrf_dfu_settings_init.
+ *
+ * @retval  NRF_SUCCESS         If the initialization was successful.
+ * @retval  NRF_ERROR_INTERNAL  If a flash error occurred.
+ */
+void nrf_dfu_settings_reinit(void);
+
+
+/**@brief   Function for initializing the DFU settings module.
+ *
+ * @retval  NRF_SUCCESS         If the initialization was successful.
+ * @retval  NRF_ERROR_INTERNAL  If a flash error occurred.
+ */
+ret_code_t nrf_dfu_settings_init(bool sd_irq_initialized);
+
+
+#if defined(NRF_DFU_TRANSPORT_BLE) && NRF_DFU_TRANSPORT_BLE
+/** @brief Function for storing peer data received through an SVCI call in DFU settings.
+ *
+ * @note The content of the type can be verified by a CRC value stored inside the struct
+ *       If the CRC value is 0xFFFFFFFF, it means that no data is set.
+ *
+ * @note The storage operation is an asynchronous progress. Success will be notified
+ *       through system events raised by the SoftDevice.
+ *
+ * @param[in]   p_data  Peer data to be stored in flash.
+ *
+ * @retval  NRF_SUCCESS     Asynchronous operation was successfully started.
+ * @retval  NRF_ERROR_NULL  p_data was NULL.
+ * @retval  Any other error code reported by SoftDevice API calls.
+ */
+ret_code_t nrf_dfu_settings_peer_data_write(nrf_dfu_peer_data_t * p_data);
+
+
+/** @brief Function for copying peer data from DFU settings to RAM.
+ *
+ * @param[in,out] p_data    Structure to copy peer data to.
+ *
+ * @retval  NRF_SUCCESS     Peer data was successfully copied.
+ * @retval  NRF_ERROR_NULL  p_data was NULL.
+ */
+ret_code_t nrf_dfu_settings_peer_data_copy(nrf_dfu_peer_data_t * p_data);
+
+
+/** @brief Function for validating peer data in DFU settings.
+ *
+ * @retval  True if peer data is validated by CRC, false if not.
+ */
+bool nrf_dfu_settings_peer_data_is_valid(void);
+
+
+/** @brief Function for storing an advertisement name received through an SVCI call in DFU settings.
+ *
+ * @note The content of the type is verifyable by a CRC-value stored inside the struct.
+ *
+ * @note The storage operation is an asynchronous progress. Success will be notified
+ *       through system events raised by the SoftDevice.
+ *
+ * @param[in]   p_adv_name  Structure holding information about the new advertisement name.
+ *
+ * @retval  NRF_SUCCESS     Asynchronous operation was successfully started.
+ * @retval  NRF_ERROR_NULL  p_adv_name was NULL.
+ * @retval  Any other error code reported by SoftDevice API calls.
+ */
+ret_code_t nrf_dfu_settings_adv_name_write(nrf_dfu_adv_name_t * p_adv_name);
+
+
+/** @brief Function for copying the advertisement name from DFU settings to RAM.
+ *
+ * @param[in,out]   p_adv_name  Structure to copy the new advertisement name to.
+ *
+ * @retval  NRF_SUCCESS     Advertisement name was successfully copied.
+ * @retval  NRF_ERROR_NULL  p_adv_name was NULL.
+ */
+ret_code_t nrf_dfu_settings_adv_name_copy(nrf_dfu_adv_name_t * p_adv_name);
+
+
+/** @brief Function for validating advertisement data in DFU settings.
+ *
+ * @retval  True if advertisement name is validated by CRC, false if not.
+ */
+bool nrf_dfu_settings_adv_name_is_valid(void);
+
+#endif // NRF_DFU_TRANSPORT_BLE
+
+/** @brief Function for erasing additional data in DFU settings.
+ *
+ * @note    Erasing additional data in DFU settings is only possible
+ *          if nrf_dfu_flash is initialized to not use SoftDevice calls.
+ *
+ * @retval  NRF_SUCCESS     Additional data was successfully erased.
+ * @retval  Any other error code reported by nrf_dfu_flash
+ */
+ret_code_t nrf_dfu_settings_additional_erase(void);
+
+/** @brief Function for resetting both init command and DFU transfer progress inside settings structure.
+ *
+ * @note    This function does not perform flash operation.
+ *          In order to save the reset state, please use @ref nrf_dfu_settings_write function.
+ */
+void nrf_dfu_settings_progress_reset(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NRF_DFU_SETTINGS_H__
+
+/**@} */

components/libraries/bootloader/dfu/nrf_dfu_settings_svci.c

@@ -0,0 +1,185 @@
+/**
+ * Copyright (c) 2017 - 2021, 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.
+ *
+ */
+
+#include <stddef.h>
+#include <string.h>
+#include "app_error.h"
+#include "sdk_macros.h"
+#include "nrf_dfu_settings.h"
+#include "nrf_nvmc.h"
+#include "crc32.h"
+
+#define NRF_LOG_MODULE_NAME nrf_dfu_settings_svci
+#include "nrf_log.h"
+NRF_LOG_MODULE_REGISTER();
+
+#define DFU_SETTINGS_PEER_DATA_OFFSET           offsetof(nrf_dfu_settings_t, peer_data)             //<! Offset in the settings struct where the additional peer data is located.
+#define DFU_SETTINGS_ADV_NAME_OFFSET            offsetof(nrf_dfu_settings_t, adv_name)              //<! Offset in the settings struct where the additional advertisement name is located.
+
+extern nrf_dfu_settings_t s_dfu_settings;
+extern uint8_t m_dfu_settings_buffer[CODE_PAGE_SIZE];
+
+#if defined(NRF_DFU_BLE_REQUIRES_BONDS) && (NRF_DFU_BLE_REQUIRES_BONDS == 1)
+
+ret_code_t nrf_dfu_settings_peer_data_write(nrf_dfu_peer_data_t * p_data)
+{
+    uint32_t        ret_val;
+
+    uint32_t *      p_peer_data_settings =
+        (uint32_t*) &m_dfu_settings_buffer[DFU_SETTINGS_PEER_DATA_OFFSET];
+
+    uint32_t crc = (uint32_t)*p_peer_data_settings;
+
+    VERIFY_PARAM_NOT_NULL(p_data);
+
+    if (crc != 0xFFFFFFFF)
+    {
+        // Already written to, must be cleared out
+        // Reset required.
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    // Calculate the CRC for the structure excluding the CRC value itself.
+    p_data->crc = crc32_compute((uint8_t*)p_data + 4, sizeof(nrf_dfu_peer_data_t) - 4, NULL);
+
+    // Using SoftDevice call since this function cannot use static memory.
+    ret_val = sd_flash_write(p_peer_data_settings,
+                             (uint32_t*)p_data,
+                             sizeof(nrf_dfu_peer_data_t)/4);
+
+    return ret_val;
+}
+
+
+ret_code_t nrf_dfu_settings_peer_data_copy(nrf_dfu_peer_data_t * p_data)
+{
+    VERIFY_PARAM_NOT_NULL(p_data);
+
+    memcpy(p_data, &m_dfu_settings_buffer[DFU_SETTINGS_PEER_DATA_OFFSET], sizeof(nrf_dfu_peer_data_t));
+
+    return NRF_SUCCESS;
+}
+
+
+bool nrf_dfu_settings_peer_data_is_valid(void)
+{
+    nrf_dfu_peer_data_t * p_peer_data =
+        (nrf_dfu_peer_data_t*) &m_dfu_settings_buffer[DFU_SETTINGS_PEER_DATA_OFFSET];
+
+    // Calculate the CRC for the structure excluding the CRC value itself.
+    uint32_t crc = crc32_compute((uint8_t*)p_peer_data + 4, sizeof(nrf_dfu_peer_data_t) - 4, NULL);
+
+    return (p_peer_data->crc == crc);
+}
+
+#else // not NRF_DFU_BLE_REQUIRES_BONDS
+
+ret_code_t nrf_dfu_settings_adv_name_write(nrf_dfu_adv_name_t * p_adv_name)
+{
+    uint32_t        ret_val;
+
+    uint32_t * p_adv_name_settings =
+        (uint32_t*) &m_dfu_settings_buffer[DFU_SETTINGS_ADV_NAME_OFFSET];
+
+    uint32_t crc = (uint32_t)*p_adv_name_settings;
+
+    VERIFY_PARAM_NOT_NULL(p_adv_name);
+
+    if (crc != 0xFFFFFFFF)
+    {
+        // Already written to, must be cleared out.
+        // Reset required
+        return NRF_ERROR_INVALID_STATE;
+    }
+
+    // Calculate the CRC for the structure excluding the CRC value itself.
+    p_adv_name->crc = crc32_compute((uint8_t *)p_adv_name + 4, sizeof(nrf_dfu_adv_name_t) - 4, NULL);
+
+    // Using SoftDevice call since this function cannot use static memory.
+    ret_val = sd_flash_write(p_adv_name_settings,
+                             (uint32_t*) p_adv_name,
+                             sizeof(nrf_dfu_adv_name_t)/4);
+    return ret_val;
+}
+
+
+ret_code_t nrf_dfu_settings_adv_name_copy(nrf_dfu_adv_name_t * p_adv_name)
+{
+    VERIFY_PARAM_NOT_NULL(p_adv_name);
+    memcpy(p_adv_name, &m_dfu_settings_buffer[DFU_SETTINGS_ADV_NAME_OFFSET], sizeof(nrf_dfu_adv_name_t));
+
+    return NRF_SUCCESS;
+}
+
+
+bool nrf_dfu_settings_adv_name_is_valid(void)
+{
+    nrf_dfu_adv_name_t * p_adv_name =
+        (nrf_dfu_adv_name_t*)&m_dfu_settings_buffer[DFU_SETTINGS_ADV_NAME_OFFSET];
+
+    // Calculate the CRC for the structure excluding the CRC value itself.
+    uint32_t crc = crc32_compute((uint8_t*)p_adv_name + 4, sizeof(nrf_dfu_adv_name_t) - 4, NULL);
+
+    return (p_adv_name->crc == crc);
+}
+
+#endif
+
+
+//lint -save -e(14)
+ret_code_t nrf_dfu_settings_additional_erase(void)
+{
+    ret_code_t ret_code = NRF_SUCCESS;
+    
+    // Check CRC for both types.
+    if (   (s_dfu_settings.peer_data.crc != 0xFFFFFFFF)
+        || (s_dfu_settings.adv_name.crc  != 0xFFFFFFFF))
+    {
+        NRF_LOG_DEBUG("Erasing settings page additional data.");
+
+        // Erasing and resetting the settings page without the peer data/adv data
+        nrf_nvmc_page_erase(BOOTLOADER_SETTINGS_ADDRESS);
+        nrf_nvmc_write_words(BOOTLOADER_SETTINGS_ADDRESS, (uint32_t const *)&s_dfu_settings, DFU_SETTINGS_PEER_DATA_OFFSET / 4);
+    }
+
+    return ret_code;
+}
+//lint -restore
+

components/libraries/bootloader/dfu/nrf_dfu_svci.c

@@ -0,0 +1,87 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "nrf_log.h"
+#include "nrf_sdm.h"
+#include "app_util.h"
+
+#define APP_START_ADDR CODE_START
+
+
+uint32_t nrf_dfu_svci_vector_table_set(void)
+{
+    uint32_t err_code;
+    uint32_t bootloader_addr = BOOTLOADER_ADDRESS;
+
+    if (bootloader_addr != 0xFFFFFFFF)
+    {
+        NRF_LOG_INFO("Setting vector table to bootloader: 0x%08x", bootloader_addr);
+        err_code = sd_softdevice_vector_table_base_set(bootloader_addr);
+        if (err_code != NRF_SUCCESS)
+        {
+            NRF_LOG_ERROR("Failed running sd_softdevice_vector_table_base_set");
+            return err_code;
+        }
+
+        return NRF_SUCCESS;
+    }
+
+    NRF_LOG_ERROR("No bootloader was found");
+    return NRF_ERROR_NO_MEM;
+}
+
+
+uint32_t nrf_dfu_svci_vector_table_unset(void)
+{
+    uint32_t err_code;
+
+    NRF_LOG_INFO("Setting vector table to main app: 0x%08x", APP_START_ADDR);
+    err_code = sd_softdevice_vector_table_base_set(APP_START_ADDR);
+    if (err_code != NRF_SUCCESS)
+    {
+        NRF_LOG_ERROR("Failed running sd_softdevice_vector_table_base_set");
+        return err_code;
+    }
+
+    return NRF_SUCCESS;
+}
+

components/libraries/bootloader/dfu/nrf_dfu_svci_handler.c

@@ -0,0 +1,212 @@
+/**
+ * Copyright (c) 2016 - 2021, 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.
+ *
+ */
+
+#include <string.h>
+#include "nrf_svci_async_handler.h"
+#include "app_error.h"
+#include "nrf_nvmc.h"
+#include "nrf_dfu_types.h"
+#include "nrf_dfu_ble_svci_bond_sharing.h"
+#include "nrf_log.h"
+#include "nrf_dfu_settings.h"
+#include "sdk_config.h"
+
+
+#if (NRF_DFU_TRANSPORT_BLE && NRF_DFU_BLE_REQUIRES_BONDS)
+
+
+NRF_SVCI_ASYNC_HANDLER_CREATE(NRF_DFU_SVCI_SET_PEER_DATA,
+    nrf_dfu_set_peer_data, nrf_dfu_peer_data_t, nrf_dfu_peer_data_state_t);
+
+
+static uint32_t nrf_dfu_set_peer_data_handler(nrf_dfu_set_peer_data_svci_async_t * p_async)
+{
+    VERIFY_PARAM_NOT_NULL(p_async);
+
+    p_async->async_func      = nrf_dfu_set_peer_data_on_call;
+    p_async->sys_evt_handler = nrf_dfu_set_peer_data_on_sys_evt;
+    p_async->state           = DFU_PEER_DATA_STATE_INITIALIZED;
+
+    return NRF_SUCCESS;
+}
+
+
+static uint32_t nrf_dfu_set_peer_data_on_call(nrf_dfu_peer_data_t       * p_data,
+                                              nrf_dfu_peer_data_state_t * p_state)
+{
+    uint32_t ret_val = NRF_ERROR_BUSY;
+
+    VERIFY_PARAM_NOT_NULL(p_state);
+
+    switch (*p_state)
+    {
+        case DFU_PEER_DATA_STATE_INVALID:
+            return NRF_ERROR_INVALID_STATE;
+
+        case DFU_PEER_DATA_STATE_INITIALIZED:
+            ret_val = nrf_dfu_settings_peer_data_write(p_data);
+            if (ret_val == NRF_SUCCESS)
+            {
+                *p_state = DFU_PEER_DATA_STATE_WRITE_REQUESTED;
+            }
+            break;
+
+        case DFU_PEER_DATA_STATE_WRITE_REQUESTED:
+            return NRF_ERROR_BUSY;
+
+        case DFU_PEER_DATA_STATE_WRITE_FINISHED:
+            return NRF_ERROR_INVALID_STATE;
+
+        case DFU_PEER_DATA_STATE_WRITE_FAILED:
+            return NRF_ERROR_INVALID_STATE;
+    }
+
+    return ret_val;
+}
+
+
+static uint32_t nrf_dfu_set_peer_data_on_sys_evt(uint32_t sys_event, nrf_dfu_peer_data_state_t * p_state)
+{
+    uint32_t ret_val = NRF_ERROR_INVALID_STATE;
+
+    VERIFY_PARAM_NOT_NULL(p_state);
+
+    if (*p_state == DFU_PEER_DATA_STATE_WRITE_REQUESTED)
+    {
+        switch (sys_event)
+        {
+            case NRF_EVT_FLASH_OPERATION_ERROR:
+                return NRF_ERROR_BUSY;
+
+            case NRF_EVT_FLASH_OPERATION_SUCCESS:
+                ret_val = NRF_SUCCESS;
+                (*p_state) = DFU_PEER_DATA_STATE_WRITE_FINISHED;
+                break;
+
+            default:
+                // Event not intended for us
+                break;
+        }
+    }
+
+    return ret_val;
+}
+
+#elif (NRF_DFU_TRANSPORT_BLE && !NRF_DFU_BLE_REQUIRES_BONDS)
+
+
+NRF_SVCI_ASYNC_HANDLER_CREATE(NRF_DFU_SVCI_SET_ADV_NAME,
+    nrf_dfu_set_adv_name,  nrf_dfu_adv_name_t,  nrf_dfu_set_adv_name_state_t);
+
+
+static uint32_t nrf_dfu_set_adv_name_handler(nrf_dfu_set_adv_name_svci_async_t * p_async)
+{
+    VERIFY_PARAM_NOT_NULL(p_async);
+
+    p_async->async_func      = nrf_dfu_set_adv_name_on_call;
+    p_async->sys_evt_handler = nrf_dfu_set_adv_name_on_sys_evt;
+    p_async->state           = DFU_ADV_NAME_STATE_INITIALIZED;
+
+    return NRF_SUCCESS;
+}
+
+
+static uint32_t nrf_dfu_set_adv_name_on_call(nrf_dfu_adv_name_t           * p_adv_name,
+                                             nrf_dfu_set_adv_name_state_t * p_state)
+{
+    uint32_t ret_val = NRF_ERROR_BUSY;
+
+    VERIFY_PARAM_NOT_NULL(p_state);
+
+    switch (*p_state)
+    {
+        case DFU_ADV_NAME_STATE_INVALID:
+            return NRF_ERROR_INVALID_STATE;
+
+        case DFU_ADV_NAME_STATE_INITIALIZED:
+            ret_val = nrf_dfu_settings_adv_name_write(p_adv_name);
+            if (ret_val == NRF_SUCCESS)
+            {
+                *p_state = DFU_ADV_NAME_STATE_WRITE_REQUESTED;
+            }
+            break;
+
+        case DFU_ADV_NAME_STATE_WRITE_REQUESTED:
+            return NRF_ERROR_BUSY;
+
+        case DFU_ADV_NAME_STATE_WRITE_FINISHED:
+            return NRF_ERROR_INVALID_STATE;
+
+        case DFU_ADV_NAME_STATE_WRITE_FAILED:
+            return NRF_ERROR_INVALID_STATE;
+    }
+
+    return ret_val;
+}
+
+
+static uint32_t nrf_dfu_set_adv_name_on_sys_evt(uint32_t sys_event, nrf_dfu_set_adv_name_state_t * p_state)
+{
+    uint32_t ret_val = NRF_ERROR_INVALID_STATE;
+
+    VERIFY_PARAM_NOT_NULL(p_state);
+
+    if (*p_state == DFU_ADV_NAME_STATE_WRITE_REQUESTED)
+    {
+        switch (sys_event)
+        {
+            case NRF_EVT_FLASH_OPERATION_ERROR:
+                return NRF_ERROR_BUSY;
+
+            case NRF_EVT_FLASH_OPERATION_SUCCESS:
+                ret_val = NRF_SUCCESS;
+                (*p_state) = DFU_ADV_NAME_STATE_WRITE_FINISHED;
+                break;
+
+            default:
+                // Event not intended for us
+                break;
+        }
+    }
+
+    return ret_val;
+}
+
+#endif  // NRF_DFU_TRANSPORT_BLE && !NRF_DFU_BLE_REQUIRES_BONDS